Use mutex to protect costmap reads.

Otherwise costmap can be read during a map update and return 0.

Author: Leif Terry

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;
 }