From a1c9fd5ad29bb00e40ce6e696d899a2bcd50cde5 Mon Sep 17 00:00:00 2001 From: LeifHookedWireless Date: Wed, 18 Oct 2023 16:18:46 -0400 Subject: [PATCH] Use mutex to protect costmap reads. (#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 e16a44c65ee7064e2271118894b92bb6e24ce28d. * Lock costmap before running MPPI controller. * Fix typo. * Protect against costmap updates in MPP and RotationShim controllers. --------- Co-authored-by: Leif Terry --- nav2_mppi_controller/src/controller.cpp | 5 ++++- .../src/regulated_pure_pursuit_controller.cpp | 3 +++ .../src/nav2_rotation_shim_controller.cpp | 3 +++ 3 files changed, 10 insertions(+), 1 deletion(-) diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 5f8dcffd26..6c83131263 100644 --- 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 lock(*parameters_handler_->getLock()); + std::lock_guard 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 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 index c40852c445..0483931671 100644 --- 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 lock_reinit(param_handler_->getMutex()); + nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); + std::unique_lock 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 index 7d32283db7..afd3238145 100644 --- 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 lock(*(costmap->getMutex())); + std::lock_guard lock_reinit(mutex_); try { geometry_msgs::msg::Pose sampled_pt_base = transformPoseToBaseFrame(getSampledPathPt());