Fixed costmap plugins parameter re-declaration bug (#752)

nav2_bt_navigator/behavior_trees/recovery_test.xml

+<!--
+  This Behavior Tree first computes a path using the global planner (ComputePathToPose).
+  Then, it runs two sub-branches in parallel. The first sub-branch is a FollowPath
+  operation (the local planner). In parallel, there is a rate controlled execution of
+  FollowPath (the global planner). Each time a new path is computed, the path update
+  is sent to the local planner. The right branch, which is the rate controlled
+  ComputePathToPose, always returns RUNNING. Because the Parallel node uses a
+  threshold of 1, whenever the FollowPath returns SUCCESS or FAILURE, the parallel
+  node will return this result.
+
+  The goal (input to the global planner) and the resulting path (output of the global
+  planner and input to the local planner) are passed on the blackboard.
+
+  The rate at which the ComputePathToPose operation is invoked can be controlled with
+  the hz parameter to the RateController node.
+-->
+
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <Sequence name="root">
+      <BackUp/>
+      <Spin/>
+      <clearEntirelyCostmapServiceRequest service_name="/local_costmap/clear_entirely_local_costmap"/>
+      <clearEntirelyCostmapServiceRequest service_name="/global_costmap/clear_entirely_global_costmap"/>
+    </Sequence>
+  </BehaviorTree>
+</root>

nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp

@@ -97,7 +97,11 @@ public:
   }
   virtual void matchSize();
 
-  virtual void reset() {onInitialize();}
+  virtual void reset()
+  {
+    undeclareAllParameters();
+    onInitialize();
+  }
 
   /** @brief  Given a distance, compute a cost.
    * @param  distance The distance from an obstacle in cells

nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp

@@ -39,6 +39,7 @@
 
 #include <string>
 #include <vector>
+#include <unordered_set>
 
 #include "tf2_ros/buffer.h"
 #include "rclcpp/rclcpp.hpp"
@@ -121,6 +122,12 @@ public:
   /** @brief Convenience function for layered_costmap_->getFootprint(). */
   const std::vector<geometry_msgs::msg::Point> & getFootprint() const;
 
+  /** @brief Convenience functions for declaring ROS parameters */
+  void declareParameter(const std::string & param_name, const rclcpp::ParameterValue & value);
+  bool hasParameter(const std::string & param_name);
+  void undeclareAllParameters();
+  std::string getFullName(const std::string & param_name);
+
 protected:
   LayeredCostmap * layered_costmap_;
   std::string name_;
@@ -141,6 +148,9 @@ protected:
   // TODO(bpwilcox): make this managed by this class and/or container class.
   bool enabled_;
 
+  // Names of the parameters declared on the ROS node
+  std::unordered_set<std::string> local_params_;
+
 private:
   std::vector<geometry_msgs::msg::Point> footprint_spec_;
 };

nav2_costmap_2d/plugins/inflation_layer.cpp

@@ -75,10 +75,10 @@ InflationLayer::InflationLayer()
 void
 InflationLayer::onInitialize()
 {
-  node_->declare_parameter(name_ + "." + "enabled", rclcpp::ParameterValue(true));
-  node_->declare_parameter(name_ + "." + "inflation_radius", rclcpp::ParameterValue(0.55));
-  node_->declare_parameter(name_ + "." + "cost_scaling_factor", rclcpp::ParameterValue(10.0));
-  node_->declare_parameter(name_ + "." + "inflate_unknown", rclcpp::ParameterValue(false));
+  declareParameter("enabled", rclcpp::ParameterValue(true));
+  declareParameter("inflation_radius", rclcpp::ParameterValue(0.55));
+  declareParameter("cost_scaling_factor", rclcpp::ParameterValue(10.0));
+  declareParameter("inflate_unknown", rclcpp::ParameterValue(false));
 
   node_->get_parameter(name_ + "." + "enabled", enabled_);
   node_->get_parameter(name_ + "." + "inflation_radius", inflation_radius_);

nav2_costmap_2d/plugins/obstacle_layer.cpp

@@ -63,7 +63,7 @@ namespace nav2_costmap_2d
 
 ObstacleLayer::~ObstacleLayer()
 {
-  for (auto &notifier : observation_notifiers_) {
+  for (auto & notifier : observation_notifiers_) {
     notifier.reset();
   }
 }
@@ -78,7 +78,8 @@ void ObstacleLayer::onInitialize()
 
   // TODO(mjeronimo): these four are candidates for dynamic update
   node_->declare_parameter(name_ + "." + "enabled", rclcpp::ParameterValue(true));
-  node_->declare_parameter(name_ + "." + "footprint_clearing_enabled", rclcpp::ParameterValue(true));
+  node_->declare_parameter(name_ + "." + "footprint_clearing_enabled",
+    rclcpp::ParameterValue(true));
   node_->declare_parameter(name_ + "." + "max_obstacle_height", rclcpp::ParameterValue(2.0));
   node_->declare_parameter(name_ + "." + "combination_method", rclcpp::ParameterValue(1));
 
@@ -116,10 +117,12 @@ void ObstacleLayer::onInitialize()
     bool inf_is_valid, clearing, marking;
 
     node_->declare_parameter(source + "." + "topic", rclcpp::ParameterValue(source));
-    node_->declare_parameter(source + "." + "sensor_frame", rclcpp::ParameterValue(std::string("")));
+    node_->declare_parameter(source + "." + "sensor_frame",
+      rclcpp::ParameterValue(std::string("")));
     node_->declare_parameter(source + "." + "observation_persistence", rclcpp::ParameterValue(0.0));
     node_->declare_parameter(source + "." + "expected_update_rate", rclcpp::ParameterValue(0.0));
-    node_->declare_parameter(source + "." + "data_type", rclcpp::ParameterValue(std::string("LaserScan")));
+    node_->declare_parameter(source + "." + "data_type",
+      rclcpp::ParameterValue(std::string("LaserScan")));
     node_->declare_parameter(source + "." + "min_obstacle_height", rclcpp::ParameterValue(0.0));
     node_->declare_parameter(source + "." + "max_obstacle_height", rclcpp::ParameterValue(0.0));
     node_->declare_parameter(source + "." + "inf_is_valid", rclcpp::ParameterValue(false));
@@ -622,6 +625,7 @@ ObstacleLayer::reset()
   resetMaps();
   current_ = true;
   activate();
+  undeclareAllParameters();
 }
 
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/plugins/static_layer.cpp

@@ -75,10 +75,10 @@ StaticLayer::onInitialize()
   getMap();
 
   if (!first_map_only_) {
-    RCLCPP_INFO(node_->get_logger(), "Subscribing to the map topic");
+    RCLCPP_INFO(node_->get_logger(), "Subscribing to the map topic (%s)", map_topic_.c_str());
     map_sub_ = node_->create_subscription<nav_msgs::msg::OccupancyGrid>(map_topic_,
-      rclcpp::SystemDefaultsQoS(),
-      std::bind(&StaticLayer::incomingMap, this, std::placeholders::_1));
+        rclcpp::SystemDefaultsQoS(),
+        std::bind(&StaticLayer::incomingMap, this, std::placeholders::_1));
   }
 
   if (subscribe_to_updates_) {
@@ -108,6 +108,7 @@ StaticLayer::reset()
   map_sub_.reset();
   map_update_sub_.reset();
 
+  undeclareAllParameters();
   onInitialize();
 }
 
@@ -116,9 +117,9 @@ StaticLayer::getParameters()
 {
   int temp_lethal_threshold = 0;
 
-  node_->declare_parameter(name_ + "." + "enabled", rclcpp::ParameterValue(true));
-  node_->declare_parameter(name_ + "." + "first_map_only", rclcpp::ParameterValue(false));
-  node_->declare_parameter(name_ + "." + "subscribe_to_updates", rclcpp::ParameterValue(false));
+  declareParameter("enabled", rclcpp::ParameterValue(true));
+  declareParameter("first_map_only", rclcpp::ParameterValue(false));
+  declareParameter("subscribe_to_updates", rclcpp::ParameterValue(false));
 
   node_->get_parameter(name_ + "." + "enabled", enabled_);
   node_->get_parameter(name_ + "." + "first_map_only", first_map_only_);

nav2_costmap_2d/plugins/voxel_layer.cpp

@@ -60,7 +60,8 @@ void VoxelLayer::onInitialize()
   ObstacleLayer::onInitialize();
 
   node_->declare_parameter(name_ + "." + "enabled", rclcpp::ParameterValue(true));
-  node_->declare_parameter(name_ + "." + "footprint_clearing_enabled", rclcpp::ParameterValue(true));
+  node_->declare_parameter(name_ + "." + "footprint_clearing_enabled",
+    rclcpp::ParameterValue(true));
   node_->declare_parameter(name_ + "." + "max_obstacle_height", rclcpp::ParameterValue(2.0));
   node_->declare_parameter(name_ + "." + "z_voxels", rclcpp::ParameterValue(10));
   node_->declare_parameter(name_ + "." + "origin_z", rclcpp::ParameterValue(0.0));
@@ -112,6 +113,7 @@ void VoxelLayer::reset()
   resetMaps();
   voxel_grid_.reset();
   activate();
+  undeclareAllParameters();
 }
 
 void VoxelLayer::resetMaps()

nav2_costmap_2d/src/layer.cpp

@@ -66,4 +66,33 @@ Layer::getFootprint() const
   return layered_costmap_->getFootprint();
 }
 
+void
+Layer::declareParameter(
+  const std::string & param_name, const rclcpp::ParameterValue & value)
+{
+  local_params_.insert(param_name);
+  node_->declare_parameter(getFullName(param_name), value);
+}
+
+bool
+Layer::hasParameter(const std::string & param_name)
+{
+  return node_->has_parameter(getFullName(param_name));
+}
+
+void
+Layer::undeclareAllParameters()
+{
+  std::for_each(begin(local_params_), end(local_params_), [this](const std::string & param_name) {
+     node_->undeclare_parameter(getFullName(param_name));
+  });
+  local_params_.clear();
+}
+
+std::string
+Layer::getFullName(const std::string & param_name)
+{
+  return std::string(name_ + "." + param_name);
+}
+
 }  // end namespace nav2_costmap_2d

nav2_costmap_2d/test/integration/costmap_tests_launch.py

@@ -20,6 +20,7 @@ import sys
 from launch import LaunchDescription
 from launch import LaunchService
 from launch.actions import ExecuteProcess
+import launch_ros.actions
 from launch.actions import IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_testing.legacy import LaunchTestService
@@ -29,8 +30,16 @@ def main(argv=sys.argv[1:]):
     launchFile = os.path.join(os.getenv('TEST_LAUNCH_DIR'), 'costmap_map_server.launch.py')
     testExecutable = os.getenv('TEST_EXECUTABLE')
 
+    lifecycle_manager = launch_ros.actions.Node(
+        package='nav2_lifecycle_manager',
+        node_executable='lifecycle_manager',
+        node_name='lifecycle_manager',
+        output='screen',
+        parameters=[{'node_names': ['map_server']}, {'autostart': True}])
+
     ld = LaunchDescription([
         IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])),
+        lifecycle_manager
     ])
 
     test1_action = ExecuteProcess(

nav2_costmap_2d/test/integration/obstacle_tests.cpp

@@ -33,8 +33,9 @@
  */
 
 #include <memory>
-#include <set>
 #include <string>
+#include <algorithm>
+#include <utility>
 
 #include "gtest/gtest.h"
 #include "nav2_costmap_2d/costmap_2d.hpp"
@@ -42,6 +43,14 @@
 #include "nav2_costmap_2d/observation_buffer.hpp"
 #include "nav2_costmap_2d/testing_helper.hpp"
 
+using std::begin;
+using std::end;
+using std::for_each;
+using std::all_of;
+using std::none_of;
+using std::pair;
+using std::string;
+
 class RclCppFixture
 {
 public:
@@ -53,7 +62,7 @@ RclCppFixture g_rclcppfixture;
 class TestLifecycleNode : public nav2_lifecycle::LifecycleNode
 {
 public:
-  explicit TestLifecycleNode(const std::string & name)
+  explicit TestLifecycleNode(const string & name)
   : nav2_lifecycle::LifecycleNode(name)
   {
   }
@@ -95,6 +104,8 @@ public:
   TestNode()
   {
     node_ = std::make_shared<TestLifecycleNode>("obstacle_test_node");
+
+    node_->declare_parameter("map_topic", rclcpp::ParameterValue(string("/map")));
   }
 
   ~TestNode() {}
@@ -129,6 +140,7 @@ protected:
  *   upper left is 0,0, lower right is 9,9
  */
 
+#if (0)
 /**
  * Test for ray tracing free space
  */
@@ -300,3 +312,58 @@ TEST_F(TestNode, testMultipleAdditions) {
 
   ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 20);
 }
+#endif
+/**
+ * Verify correct init/reset cycling of layer
+ */
+TEST_F(TestNode, testRepeatedResets) {
+  tf2_ros::Buffer tf(node_->get_clock());
+  nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
+  addStaticLayer(layers, tf, node_);
+
+  // TODO(orduno) Add obstacle layer
+
+  // Define a node-level parameter
+  pair<string, string> node_dummy = {"node_dummy_param", "node_dummy_val"};
+  node_->declare_parameter(node_dummy.first, rclcpp::ParameterValue(node_dummy.second));
+
+  // Define a layer-level parameter
+  pair<string, string> layer_dummy = {"dummy_param", "dummy_val"};
+
+  // Set parameters
+  auto plugins = layers.getPlugins();
+  for_each(begin(*plugins), end(*plugins), [&layer_dummy](const auto & plugin) {
+      string layer_param = layer_dummy.first + "_" + plugin->getName();
+
+      // Notice we are using Layer::declareParameter
+      plugin->declareParameter(layer_param, rclcpp::ParameterValue(layer_dummy.second));
+    });
+
+  // Check that all parameters have been set
+  // node-level param
+  ASSERT_TRUE(node_->has_parameter(node_dummy.first));
+
+  // layer-level param
+  ASSERT_TRUE(
+    all_of(begin(*plugins), end(*plugins), [&layer_dummy](const auto & plugin) {
+      string layer_param = layer_dummy.first + "_" + plugin->getName();
+      return plugin->hasParameter(layer_param);
+    }));
+
+  // Reset all layers. This will un-declare all params and might re-declare internal ones
+  // Should run without throwing exceptions
+  ASSERT_NO_THROW(
+    for_each(begin(*plugins), end(*plugins), [](const auto & plugin) {
+      plugin->reset();
+    }));
+
+  // Check for node-level param
+  ASSERT_TRUE(node_->has_parameter(node_dummy.first));
+
+  // Layer-level parameters shouldn't be found
+  ASSERT_TRUE(
+    none_of(begin(*plugins), end(*plugins), [&layer_dummy](const auto & plugin) {
+      string layer_param = layer_dummy.first + "_" + plugin->getName();
+      return plugin->hasParameter(layer_param);
+    }));
+}