Increase scope of costmap mutex in publishCostmap to cover costmap calls (#992)

griswaldbrooks · mikeferguson · commit 954aebade863 · 2020-12-30T10:15:16.000-05:00
diff --git a/costmap_2d/src/costmap_2d_publisher.cpp b/costmap_2d/src/costmap_2d_publisher.cpp
@@ -125,6 +125,7 @@ void Costmap2DPublisher::publishCostmap()
     return;
   }
 
+  boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
   float resolution = costmap_->getResolution();
 
   if (always_send_full_costmap_ || grid_.info.resolution != resolution ||
@@ -138,7 +139,6 @@ void Costmap2DPublisher::publishCostmap()
   }
   else if (x0_ < xn_)
   {
-    boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
     // Publish Just an Update
     map_msgs::OccupancyGridUpdate update;
     update.header.stamp = ros::Time::now();

Original file line number	Diff line number	Diff line change
`@@ -125,6 +125,7 @@ void Costmap2DPublisher::publishCostmap()`
`125`	`125`	`return;`
`126`	`126`	`}`
`127`	`127`
	`128`	`+ boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));`
`128`	`129`	`float resolution = costmap_->getResolution();`
`129`	`130`
`130`	`131`	`if (always_send_full_costmap_ \|\| grid_.info.resolution != resolution \|\|`
`@@ -138,7 +139,6 @@ void Costmap2DPublisher::publishCostmap()`
`138`	`139`	`}`
`139`	`140`	`else if (x0_ < xn_)`
`140`	`141`	`{`
`141`		`- boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));`
`142`	`142`	`// Publish Just an Update`
`143`	`143`	`map_msgs::OccupancyGridUpdate update;`
`144`	`144`	`update.header.stamp = ros::Time::now();`