From 892ead2d0cb9cc596f253d69b9e5b9431dbea47b Mon Sep 17 00:00:00 2001 From: Ernestas Date: Mon, 7 Jun 2021 20:39:43 +0300 Subject: [PATCH 1/3] parallelize costmap bound updates --- nav2_costmap_2d/CMakeLists.txt | 7 +++ nav2_costmap_2d/src/layered_costmap.cpp | 70 ++++++++++++++----------- 2 files changed, 45 insertions(+), 32 deletions(-) diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 6eee80bf820..14bab960da5 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -27,6 +27,13 @@ find_package(angles REQUIRED) remove_definitions(-DDISABLE_LIBUSB-1.0) find_package(Eigen3 REQUIRED) +find_package(OpenMP) +if (OPENMP_FOUND) + set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") +endif() + nav2_package() include_directories( diff --git a/nav2_costmap_2d/src/layered_costmap.cpp b/nav2_costmap_2d/src/layered_costmap.cpp index e31ebe1414c..4ec9de88816 100644 --- a/nav2_costmap_2d/src/layered_costmap.cpp +++ b/nav2_costmap_2d/src/layered_costmap.cpp @@ -145,40 +145,46 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) minx_ = miny_ = std::numeric_limits::max(); maxx_ = maxy_ = std::numeric_limits::lowest(); - for (vector>::iterator plugin = plugins_.begin(); - plugin != plugins_.end(); ++plugin) + #pragma omp parallel reduction(min:minx_) reduction(min: miny_) \ + reduction(max: maxx_) reduction(max: maxy_) num_threads(2) { - double prev_minx = minx_; - double prev_miny = miny_; - double prev_maxx = maxx_; - double prev_maxy = maxy_; - (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_); - if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy) { - RCLCPP_WARN( - rclcpp::get_logger( - "nav2_costmap_2d"), "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but " - "is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s", - prev_minx, prev_miny, prev_maxx, prev_maxy, - minx_, miny_, maxx_, maxy_, - (*plugin)->getName().c_str()); + #pragma omp for schedule(static, 1) + for (vector>::iterator plugin = plugins_.begin(); + plugin != plugins_.end(); ++plugin) + { + double prev_minx = minx_; + double prev_miny = miny_; + double prev_maxx = maxx_; + double prev_maxy = maxy_; + (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_); + if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy) { + RCLCPP_WARN( + rclcpp::get_logger( + "nav2_costmap_2d"), "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but " + "is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s", + prev_minx, prev_miny, prev_maxx, prev_maxy, + minx_, miny_, maxx_, maxy_, + (*plugin)->getName().c_str()); + } } - } - for (vector>::iterator filter = filters_.begin(); - filter != filters_.end(); ++filter) - { - double prev_minx = minx_; - double prev_miny = miny_; - double prev_maxx = maxx_; - double prev_maxy = maxy_; - (*filter)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_); - if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy) { - RCLCPP_WARN( - rclcpp::get_logger( - "nav2_costmap_2d"), "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but " - "is now [tl: (%f, %f), br: (%f, %f)]. The offending filter is %s", - prev_minx, prev_miny, prev_maxx, prev_maxy, - minx_, miny_, maxx_, maxy_, - (*filter)->getName().c_str()); + #pragma omp for nowait schedule(static, 1) + for (vector>::iterator filter = filters_.begin(); + filter != filters_.end(); ++filter) + { + double prev_minx = minx_; + double prev_miny = miny_; + double prev_maxx = maxx_; + double prev_maxy = maxy_; + (*filter)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_); + if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy) { + RCLCPP_WARN( + rclcpp::get_logger( + "nav2_costmap_2d"), "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but " + "is now [tl: (%f, %f), br: (%f, %f)]. The offending filter is %s", + prev_minx, prev_miny, prev_maxx, prev_maxy, + minx_, miny_, maxx_, maxy_, + (*filter)->getName().c_str()); + } } } From c2bf850c8e2e9dd018d88e31549fc47a9e22c1e3 Mon Sep 17 00:00:00 2001 From: Ernestas Date: Tue, 8 Jun 2021 20:42:10 +0300 Subject: [PATCH 2/3] remove whitespaces from cmake --- nav2_costmap_2d/CMakeLists.txt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 14bab960da5..888df7a264b 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -28,10 +28,10 @@ remove_definitions(-DDISABLE_LIBUSB-1.0) find_package(Eigen3 REQUIRED) find_package(OpenMP) -if (OPENMP_FOUND) - set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") - set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") - set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") +if(OPENMP_FOUND) + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") endif() nav2_package() From e7323cb2ffadea47035b67da05bbe0a929e1c5f2 Mon Sep 17 00:00:00 2001 From: Ernestas Date: Fri, 11 Jun 2021 20:54:19 +0300 Subject: [PATCH 3/3] separate parallel block for plugins & threads --- nav2_costmap_2d/src/layered_costmap.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/nav2_costmap_2d/src/layered_costmap.cpp b/nav2_costmap_2d/src/layered_costmap.cpp index 4ec9de88816..bc4550f2332 100644 --- a/nav2_costmap_2d/src/layered_costmap.cpp +++ b/nav2_costmap_2d/src/layered_costmap.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #include "nav2_costmap_2d/footprint.hpp" @@ -145,10 +146,14 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) minx_ = miny_ = std::numeric_limits::max(); maxx_ = maxy_ = std::numeric_limits::lowest(); + int n_cores = omp_get_num_procs(); + int n_plugin_threads = std::max(std::min((int)plugins_.size(), n_cores), 1); + int n_filter_threads = std::max(std::min((int)filters_.size(), n_cores), 1); + #pragma omp parallel reduction(min:minx_) reduction(min: miny_) \ - reduction(max: maxx_) reduction(max: maxy_) num_threads(2) + reduction(max: maxx_) reduction(max: maxy_) num_threads(n_plugin_threads) { - #pragma omp for schedule(static, 1) + #pragma omp for nowait schedule(auto) for (vector>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { @@ -167,7 +172,12 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) (*plugin)->getName().c_str()); } } - #pragma omp for nowait schedule(static, 1) + } + + #pragma omp parallel reduction(min:minx_) reduction(min: miny_) \ + reduction(max: maxx_) reduction(max: maxy_) num_threads(n_filter_threads) + { + #pragma omp for nowait schedule(auto) for (vector>::iterator filter = filters_.begin(); filter != filters_.end(); ++filter) {