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

…os-navigation#3897) * 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 <leif@peanutrobotics.com> (cherry picked from commit a1c9fd5) # Conflicts: # nav2_mppi_controller/src/controller.cpp * fix merge conflict --------- Co-authored-by: LeifHookedWireless <leif@hookedwireless.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
EnjoyRobotics · Nov 20, 2023 · 4d85df0 · 4d85df0
1 parent e76dd36
commit 4d85df0
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
@@ -91,9 +91,12 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(
   }
   last_time_called_ = clock_->now();
 
-  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
@@ -282,6 +282,9 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
 {
   std::lock_guard<std::mutex> lock_reinit(mutex_);
 
+  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());