Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

parallelize costmap bound updates #2393

Closed
wants to merge 3 commits into from
Closed

parallelize costmap bound updates #2393

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
892ead2
parallelize costmap bound updates
simutisernestas Jun 7, 2021
c2bf850
remove whitespaces from cmake
simutisernestas Jun 8, 2021
e7323cb
separate parallel block for plugins & threads
simutisernestas Jun 11, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 7 additions & 0 deletions nav2_costmap_2d/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down
70 changes: 38 additions & 32 deletions nav2_costmap_2d/src/layered_costmap.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -145,40 +145,46 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
minx_ = miny_ = std::numeric_limits<double>::max();
maxx_ = maxy_ = std::numeric_limits<double>::lowest();

for (vector<std::shared_ptr<Layer>>::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)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

From my understanding, this scheduling makes it so that each thread takes 1 iteration at a time statically. Why specify this and not have it use auto or the other options? Genuinely not sure, this might be the right thing to do but asking

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Similar to the previous point, if you have a small number of iteration and auto sets a schedule for one thread to take all iterations at once this wouldn't make any sense. I'm just cautious about auto because I don't know what the compiler will do here in that case. Therefore one plugin or filter update allocated to one thread seems most appropriate because the number is usually less than 5

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

did you see a demonstrable change for your testing with and without this?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

i'll get back with some quantitative results on this one, haven't tested with auto actually

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

(or even just lacking of schedule specification, which I assume is auto. I think it defaults to the number of cores)

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah, I think we can leave scheduling for OpenMP.

I would go with the third one since the difference is not noticeable and it seems more flexible. I've pushed code with that version. Probably those variables in front of the loop could be made static member variables.

I'll update you on the speed-up in the near future.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I've got back to the speed-up evaluation. Funny enough, without DEBUG log level parallel version is actually underperforming compared to the plain loop... Log level was influencing my measurements previously somehow.

plain average update time: 0.001102279375405933
parallel average update time: 0.0010157955731191883

These are averaged updated from both local and global cost maps. With this config: https://github.com/simutisernestas/navigation2/blob/parallel_config/nav2_bringup/bringup/params/nav2_params.yaml. I wonder if that would be the case with heavier cost map layers or bigger map.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

1.1 ms, why so fast? Oh, is this in the TB3 sandbox world? Yeah, it might be worth trying something larger. This shows an overview of getting nav2 running in the AWS warehouse world https://github.com/ros-planning/navigation2/tree/main/nav2_simple_commander.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Strangely enough, map updates are even faster on AWS warehouse world demo. I've pulled the latest changes from the repo the code must have been upgraded since the last time I've benchmarked this. Both parallel and serial versions do the job in about the same amount of time. With parallel slightly better, but it's insignificant I would say. I'm testing on Intel® Core™ i7-3770 CPU @ 3.40GHz × 8 with the same config as mentioned previously.

serial avg update time: 0.7053497955298016 ms from 2416 samples
parallel avg update time: 0.6474942507522566 ms from 2991 samples

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That doesn't surprise me, since you if each thread is doing "more" work, then the overhead of spinning up a new thread is more worth it. It's about 8% it seems, do you know why its lower than what you were seeing before when you reported a ~30% increase in speed? I'm also a little dubious since the updates are so fast. Your other experiment reported 12-20ms in update times (I assume more data) while this is less than 1ms. 1ms seems way too fast to be what you'd see in a practical deployed application so I wonder if in a more realistic setup where things are taking longer if we do see that benefit again.

for (vector<std::shared_ptr<Layer>>::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<std::shared_ptr<Layer>>::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<std::shared_ptr<Layer>>::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());
}
}
}

Expand Down