Use mutex to protect costmap reads. (ros-navigation#3877)

* Use mutex to protect costmap reads. Otherwise costmap can be read during a map update and return 0. * Revert "Use mutex to protect costmap reads." This reverts commit e16a44c. * Lock costmap before running MPPI controller. * Fix typo. * Protect against costmap updates in MPP and RotationShim controllers. --------- Co-authored-by: Leif Terry <[email protected]>
floatic-unicorn · Oct 18, 2023 · a1c9fd5 · a1c9fd5
1 parent 7009ffb
commit a1c9fd5
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 1 deletion.
diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp
@@ -83,9 +83,12 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(
   auto start = std::chrono::system_clock::now();
 #endif
 
-  std::lock_guard<std::mutex> lock(*parameters_handler_->getLock());
+  std::lock_guard<std::mutex> param_lock(*parameters_handler_->getLock());
   nav_msgs::msg::Path transformed_plan = path_handler_.transformPath(robot_pose);
 
+  nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
+  std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> costmap_lock(*(costmap->getMutex()));
+
   geometry_msgs::msg::TwistStamped cmd =
     optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal_checker);
 

diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp
@@ -157,6 +157,9 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
 {
   std::lock_guard<std::mutex> lock_reinit(param_handler_->getMutex());
 
+  nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
+  std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
+
   // Update for the current goal checker's state
   geometry_msgs::msg::Pose pose_tolerance;
   geometry_msgs::msg::Twist vel_tolerance;

diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp
@@ -140,6 +140,9 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands
   nav2_core::GoalChecker * goal_checker)
 {
   if (path_updated_) {
+    nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
+    std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
+
     std::lock_guard<std::mutex> lock_reinit(mutex_);
     try {
       geometry_msgs::msg::Pose sampled_pt_base = transformPoseToBaseFrame(getSampledPathPt());