Lock costmap in getCostsCallback (#5075)

* Add IsStoppedBTNode

Signed-off-by: Tony Najjar <[email protected]>

* add topic name + reformat

Signed-off-by: Tony Najjar <[email protected]>

* fix comment

Signed-off-by: Tony Najjar <[email protected]>

* fix abs

Signed-off-by: Tony Najjar <[email protected]>

* remove log

Signed-off-by: Tony Najjar <[email protected]>

* add getter functions for raw twist

Signed-off-by: Tony Najjar <[email protected]>

* remove unused code

Signed-off-by: Tony Najjar <[email protected]>

* use odomsmoother

Signed-off-by: Tony Najjar <[email protected]>

* fix formatting

Signed-off-by: Tony Najjar <[email protected]>

* update groot

Signed-off-by: Tony Najjar <[email protected]>

* Add test

Signed-off-by: Tony Najjar <[email protected]>

* reset at success

Signed-off-by: Tony Najjar <[email protected]>

* FIX velocity_threshold_

Signed-off-by: Tony Najjar <[email protected]>

* Fix stopped Node

Signed-off-by: Tony Najjar <[email protected]>

* Add tests  to odometry_utils

Signed-off-by: Tony Najjar <[email protected]>

* fix linting

Signed-off-by: Tony Najjar <[email protected]>

* lock costmap

Signed-off-by: Tony Najjar <[email protected]>

* improvement

Signed-off-by: Tony Najjar <[email protected]>

* remove spacing

Signed-off-by: Tony Najjar <[email protected]>

* remove unlock

Signed-off-by: Tony Najjar <[email protected]>

---------

Signed-off-by: Tony Najjar <[email protected]>

Author: tonynajjar

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -831,6 +831,7 @@ void Costmap2DROS::getCostsCallback(
   unsigned int mx, my;
 
   Costmap2D * costmap = layered_costmap_->getCostmap();
+  std::unique_lock<Costmap2D::mutex_t> lock(*(costmap->getMutex()));
   response->success = true;
   for (const auto & pose : request->poses) {
     geometry_msgs::msg::PoseStamped pose_transformed;