Costmap lock while copying data in navfn planner (#1913)

* acquire the costmap lock while copying data in navfn planner

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

* add costmap lock to dwb local plannger

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp

@@ -325,6 +325,9 @@ DWBLocalPlanner::computeVelocityCommands(
 
   prepareGlobalPlan(pose, transformed_plan, goal_pose);
 
+  nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
+  std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
+
   for (TrajectoryCritic::Ptr critic : critics_) {
     if (critic->prepare(pose.pose, velocity, goal_pose.pose, transformed_plan) == false) {
       RCLCPP_WARN(rclcpp::get_logger("DWBLocalPlanner"), "A scoring function failed to prepare");
@@ -344,6 +347,8 @@ DWBLocalPlanner::computeVelocityCommands(
       critic->debrief(cmd_vel.velocity);
     }
 
+    lock.unlock();
+
     pub_->publishLocalPlan(pose.header, best.traj);
     pub_->publishCostGrid(costmap_ros_, critics_);
 
@@ -355,6 +360,9 @@ DWBLocalPlanner::computeVelocityCommands(
     for (TrajectoryCritic::Ptr critic : critics_) {
       critic->debrief(empty_cmd);
     }
+
+    lock.unlock();
+
     pub_->publishLocalPlan(pose.header, empty_traj);
     pub_->publishCostGrid(costmap_ros_, critics_);
 

nav2_navfn_planner/src/navfn_planner.cpp

@@ -174,13 +174,17 @@ NavfnPlanner::makePlan(
   // clear the starting cell within the costmap because we know it can't be an obstacle
   clearRobotCell(mx, my);
 
+  std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
+
   // make sure to resize the underlying array that Navfn uses
   planner_->setNavArr(
     costmap_->getSizeInCellsX(),
     costmap_->getSizeInCellsY());
 
   planner_->setCostmap(costmap_->getCharMap(), true, allow_unknown_);
 
+  lock.unlock();
+
   int map_start[2];
   map_start[0] = mx;
   map_start[1] = my;