Thread safety fixes for footprint (#3875)

* Don't assign bad values to radius, even temporarily.

* Thread safety:
 * Keep same smart pointer to object within a scope.

* Revert can_load_messages fixes. those will go in a different PR

* There's no easy way to pass references back in a thread-safe way.
Have calculateMinAndMaxDistances return values instead.

* Revert unnecessary changes "Thread safety:"

This reverts commit 98e2f8d.

* Protect access to footprint via mutex.

* Revert "Protect access to footprint via mutex."

This reverts commit 405cc17.

* Use atomic assignment for footprint.

* Add comment about atomic load/store.

* Initialize footprint. If accessed before assigned was returning a NULL
reference.

---------

Co-authored-by: Leif Terry <leif@peanutrobotics.com>

nav2_costmap_2d/include/nav2_costmap_2d/footprint.hpp

@@ -58,9 +58,8 @@ namespace nav2_costmap_2d
  * @param min_dist Output parameter of the minimum distance
  * @param max_dist Output parameter of the maximum distance
  */
-void calculateMinAndMaxDistances(
-  const std::vector<geometry_msgs::msg::Point> & footprint,
-  double & min_dist, double & max_dist);
+std::pair<double, double> calculateMinAndMaxDistances(
+  const std::vector<geometry_msgs::msg::Point> & footprint);
 
 /**
  * @brief Convert Point32 to Point

nav2_costmap_2d/include/nav2_costmap_2d/layered_costmap.hpp

@@ -188,21 +188,24 @@ 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()
+  {
+    return *std::atomic_load(&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() {return circumscribed_radius_.load();}
 
   /** @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() {return inscribed_radius_.load();}
 
   /** @brief Checks if the robot is outside the bounds of its costmap in the case
   * of poorly configured setups. */
@@ -228,8 +231,8 @@ class LayeredCostmap
 
   bool initialized_;
   bool size_locked_;
-  double circumscribed_radius_, inscribed_radius_;
-  std::vector<geometry_msgs::msg::Point> footprint_;
+  std::atomic<double> circumscribed_radius_, inscribed_radius_;
+  std::shared_ptr<std::vector<geometry_msgs::msg::Point>> footprint_;
 };
 
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/src/footprint.cpp

@@ -40,15 +40,14 @@
 namespace nav2_costmap_2d
 {
 
-void calculateMinAndMaxDistances(
-  const std::vector<geometry_msgs::msg::Point> & footprint,
-  double & min_dist, double & max_dist)
+std::pair<double, double> calculateMinAndMaxDistances(
+  const std::vector<geometry_msgs::msg::Point> & footprint)
 {
-  min_dist = std::numeric_limits<double>::max();
-  max_dist = 0.0;
+  double min_dist = std::numeric_limits<double>::max();
+  double max_dist = 0.0;
 
   if (footprint.size() <= 2) {
-    return;
+    return std::pair<double, double>(min_dist, max_dist);
   }
 
   for (unsigned int i = 0; i < footprint.size() - 1; ++i) {
@@ -68,6 +67,8 @@ void calculateMinAndMaxDistances(
     footprint.front().x, footprint.front().y);
   min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
   max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
+
+  return std::pair<double, double>(min_dist, max_dist);
 }
 
 geometry_msgs::msg::Point32 toPoint32(geometry_msgs::msg::Point pt)

nav2_costmap_2d/src/layered_costmap.cpp

@@ -68,7 +68,8 @@ LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bo
   initialized_(false),
   size_locked_(false),
   circumscribed_radius_(1.0),
-  inscribed_radius_(0.1)
+  inscribed_radius_(0.1),
+  footprint_(std::make_shared<std::vector<geometry_msgs::msg::Point>>())
 {
   if (track_unknown) {
     primary_costmap_.setDefaultValue(255);
@@ -274,10 +275,15 @@ bool LayeredCostmap::isCurrent()
 
 void LayeredCostmap::setFootprint(const std::vector<geometry_msgs::msg::Point> & footprint_spec)
 {
-  footprint_ = footprint_spec;
-  nav2_costmap_2d::calculateMinAndMaxDistances(
-    footprint_spec,
-    inscribed_radius_, circumscribed_radius_);
+  std::pair<double, double> inside_outside = nav2_costmap_2d::calculateMinAndMaxDistances(
+    footprint_spec);
+  // use atomic store here since footprint is used by various planners/controllers
+  // and not otherwise locked
+  std::atomic_store(
+    &footprint_,
+    std::make_shared<std::vector<geometry_msgs::msg::Point>>(footprint_spec));
+  inscribed_radius_.store(std::get<0>(inside_outside));
+  circumscribed_radius_.store(std::get<1>(inside_outside));
 
   for (vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin();
     plugin != plugins_.end();

nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp

@@ -172,7 +172,7 @@ TEST(collision_footprint, not_enough_points)
   double min_dist = 0.0;
   double max_dist = 0.0;
 
-  nav2_costmap_2d::calculateMinAndMaxDistances(footprint, min_dist, max_dist);
+  std::tie(min_dist, max_dist) = nav2_costmap_2d::calculateMinAndMaxDistances(footprint);
   EXPECT_EQ(min_dist, std::numeric_limits<double>::max());
   EXPECT_EQ(max_dist, 0.0f);
 }