Protect access to footprint via mutex.

nav2_costmap_2d/include/nav2_costmap_2d/layered_costmap.hpp

@@ -188,21 +188,30 @@ class LayeredCostmap
   void setFootprint(const std::vector<geometry_msgs::msg::Point> & footprint_spec);
 
   /** @brief Returns the latest footprint stored with setFootprint(). */
-  const std::vector<geometry_msgs::msg::Point> & getFootprint() {return footprint_;}
+  const std::vector<geometry_msgs::msg::Point> & getFootprint() {
+      std::lock_guard<std::mutex> lock(footprint_mutex_);
+      return footprint_;
+  }
 
   /** @brief The radius of a circle centered at the origin of the
    * robot which just surrounds all points on the robot's
    * footprint.
    *
    * This is updated by setFootprint(). */
-  double getCircumscribedRadius() {return circumscribed_radius_;}
+  double getCircumscribedRadius() {
+      std::lock_guard<std::mutex> lock(footprint_mutex_);
+      return circumscribed_radius_;
+  }
 
   /** @brief The radius of a circle centered at the origin of the
    * robot which is just within all points and edges of the robot's
    * footprint.
    *
    * This is updated by setFootprint(). */
-  double getInscribedRadius() {return inscribed_radius_;}
+  double getInscribedRadius() {
+      std::lock_guard<std::mutex> lock(footprint_mutex_);
+      return inscribed_radius_;
+  }
 
   /** @brief Checks if the robot is outside the bounds of its costmap in the case
   * of poorly configured setups. */
@@ -230,6 +239,7 @@ class LayeredCostmap
   bool size_locked_;
   double circumscribed_radius_, inscribed_radius_;
   std::vector<geometry_msgs::msg::Point> footprint_;
+  std::mutex footprint_mutex_;
 };
 
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/src/layered_costmap.cpp

@@ -274,9 +274,12 @@ bool LayeredCostmap::isCurrent()
 
 void LayeredCostmap::setFootprint(const std::vector<geometry_msgs::msg::Point> & footprint_spec)
 {
-  footprint_ = footprint_spec;
-  std::tie(inscribed_radius_, circumscribed_radius_) = nav2_costmap_2d::calculateMinAndMaxDistances(
-    footprint_spec);
+  auto min_max = nav2_costmap_2d::calculateMinAndMaxDistances(footprint_spec);
+  {
+    std::lock_guard<std::mutex> lock(footprint_mutex_);
+    footprint_ = footprint_spec;
+    std::tie(inscribed_radius_, circumscribed_radius_) = min_max;
+  }
 
   for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin();
     plugin != plugins_.end();