Use mutex to protect costmap reads.

Otherwise costmap can be read during a map update and return 0.
floatic-unicorn · Oct 12, 2023 · e16a44c · e16a44c
1 parent 57dd8d4
commit e16a44c
Showing 1 changed file with 3 additions and 0 deletions.
diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp
@@ -265,16 +265,19 @@ unsigned char * Costmap2D::getCharMap() const
 
 unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const
 {
+  std::unique_lock<mutex_t> lock(*access_);
   return costmap_[getIndex(mx, my)];
 }
 
 unsigned char Costmap2D::getCost(unsigned int undex) const
 {
+  std::unique_lock<mutex_t> lock(*access_);
   return costmap_[undex];
 }
 
 void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost)
 {
+  std::unique_lock<mutex_t> lock(*access_);
   costmap_[getIndex(mx, my)] = cost;
 }