Skip to content

Commit

Permalink
Costmap lock while copying data in navfn planner (#1913)
Browse files Browse the repository at this point in the history
* 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>
  • Loading branch information
daisukes authored Aug 5, 2020
1 parent 6f4a558 commit cf3a9f8
Show file tree
Hide file tree
Showing 2 changed files with 12 additions and 0 deletions.
8 changes: 8 additions & 0 deletions nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand All @@ -344,6 +347,8 @@ DWBLocalPlanner::computeVelocityCommands(
critic->debrief(cmd_vel.velocity);
}

lock.unlock();

pub_->publishLocalPlan(pose.header, best.traj);
pub_->publishCostGrid(costmap_ros_, critics_);

Expand All @@ -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_);

Expand Down
4 changes: 4 additions & 0 deletions nav2_navfn_planner/src/navfn_planner.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down

0 comments on commit cf3a9f8

Please sign in to comment.