Forse ROS2 parameters to be static to meet RCLCPP API changes (#2239)

* Forse ROS2 parameters to be static to meet RCLCPP API changes

* Fix comments

* Move all common CostmapFilter parameter declarations into try-catch block

Author: AlexeyMerzlyakov

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -49,7 +49,7 @@ BtActionServer<ActionT>::BtActionServer(
   clock_ = node->get_clock();
 
   // Declare this node's parameters
-  node->declare_parameter("default_bt_xml_filename");
+  node->declare_parameter("default_bt_xml_filename", rclcpp::PARAMETER_STRING);
   node->declare_parameter("enable_groot_monitoring", true);
   node->declare_parameter("groot_zmq_publisher_port", 1666);
   node->declare_parameter("groot_zmq_server_port", 1667);

nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp

@@ -128,7 +128,8 @@ class Layer  // TODO(mjeronimo): public nav2_util::LifecycleHelperInterface
     const std::string & param_name,
     const rclcpp::ParameterValue & value);
   void declareParameter(
-    const std::string & param_name);
+    const std::string & param_name,
+    const rclcpp::ParameterType & param_type);
   bool hasParameter(const std::string & param_name);
   std::string getFullName(const std::string & param_name);
 

nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp

@@ -60,22 +60,22 @@ void CostmapFilter::onInitialize()
     throw std::runtime_error{"Failed to lock node"};
   }
 
-  // Declare common for all costmap filters parameters
-  declareParameter("enabled", rclcpp::ParameterValue(true));
-  declareParameter("filter_info_topic");
-  declareParameter("transform_tolerance", rclcpp::ParameterValue(0.1));
-
-  // Get parameters
-  node->get_parameter(name_ + "." + "enabled", enabled_);
   try {
+    // Declare common for all costmap filters parameters
+    declareParameter("enabled", rclcpp::ParameterValue(true));
+    declareParameter("filter_info_topic", rclcpp::PARAMETER_STRING);
+    declareParameter("transform_tolerance", rclcpp::ParameterValue(0.1));
+
+    // Get parameters
+    node->get_parameter(name_ + "." + "enabled", enabled_);
     filter_info_topic_ = node->get_parameter(name_ + "." + "filter_info_topic").as_string();
-  } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) {
-    RCLCPP_ERROR(logger_, "filter_info_topic parameter is not set");
+    double transform_tolerance;
+    node->get_parameter(name_ + "." + "transform_tolerance", transform_tolerance);
+    transform_tolerance_ = tf2::durationFromSec(transform_tolerance);
+  } catch (const std::exception & ex) {
+    RCLCPP_ERROR(logger_, "Parameter problem: %s", ex.what());
     throw ex;
   }
-  double transform_tolerance;
-  node->get_parameter(name_ + "." + "transform_tolerance", transform_tolerance);
-  transform_tolerance_ = tf2::durationFromSec(transform_tolerance);
 }
 
 void CostmapFilter::activate()

nav2_costmap_2d/src/layer.cpp

@@ -91,15 +91,16 @@ Layer::declareParameter(
 
 void
 Layer::declareParameter(
-  const std::string & param_name)
+  const std::string & param_name,
+  const rclcpp::ParameterType & param_type)
 {
   auto node = node_.lock();
   if (!node) {
     throw std::runtime_error{"Failed to lock node"};
   }
   local_params_.insert(param_name);
   nav2_util::declare_parameter_if_not_declared(
-    node, getFullName(param_name));
+    node, getFullName(param_name), param_type);
 }
 
 bool

nav2_costmap_2d/test/unit/declare_parameter_test.cpp

@@ -36,7 +36,7 @@ class LayerWrapper : public nav2_costmap_2d::Layer
   bool isClearable() {return false;}
 };
 
-TEST(DeclareParameter, useValidParameter2Args)
+TEST(DeclareParameter, useValidParameter)
 {
   LayerWrapper layer;
   nav2_util::LifecycleNode::SharedPtr node =
@@ -55,7 +55,7 @@ TEST(DeclareParameter, useValidParameter2Args)
   }
 }
 
-TEST(DeclareParameter, useValidParameter1Arg)
+TEST(DeclareParameter, useInvalidParameter)
 {
   LayerWrapper layer;
   nav2_util::LifecycleNode::SharedPtr node =
@@ -65,31 +65,10 @@ TEST(DeclareParameter, useValidParameter1Arg)
 
   layer.initialize(&layers, "test_layer", &tf, node, nullptr, nullptr);
 
-  layer.declareParameter("test2");
   try {
-    node->set_parameter(rclcpp::Parameter("test_layer.test2", "test_val2"));
-    std::string val = node->get_parameter("test_layer.test2").as_string();
-    EXPECT_EQ(val, "test_val2");
-  } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) {
-    FAIL() << "test_layer.test2 parameter is not set";
-  }
-}
-
-TEST(DeclareParameter, useInvalidParameter1Arg)
-{
-  LayerWrapper layer;
-  nav2_util::LifecycleNode::SharedPtr node =
-    std::make_shared<nav2_util::LifecycleNode>("test_node");
-  tf2_ros::Buffer tf(node->get_clock());
-  nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
-
-  layer.initialize(&layers, "test_layer", &tf, node, nullptr, nullptr);
-
-  layer.declareParameter("test3");
-  try {
-    std::string val = node->get_parameter("test_layer.test3").as_string();
-    FAIL() << "Incorrectly handling test_layer.test3 parameters which was not set";
-  } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) {
+    layer.declareParameter("test2", rclcpp::PARAMETER_STRING);
+    FAIL() << "Incorrectly handling test_layer.test2 parameter which was not set";
+  } catch (rclcpp::exceptions::NoParameterOverrideProvided & ex) {
     SUCCEED();
   }
 }

nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp

@@ -76,8 +76,12 @@ void DWBLocalPlanner::configure(
   costmap_ros_ = costmap_ros;
   tf_ = tf;
   dwb_plugin_name_ = name;
-  declare_parameter_if_not_declared(node, dwb_plugin_name_ + ".critics");
-  declare_parameter_if_not_declared(node, dwb_plugin_name_ + ".default_critic_namespaces");
+  declare_parameter_if_not_declared(
+    node, dwb_plugin_name_ + ".critics",
+    rclcpp::PARAMETER_STRING_ARRAY);
+  declare_parameter_if_not_declared(
+    node, dwb_plugin_name_ + ".default_critic_namespaces",
+    rclcpp::ParameterValue(std::vector<std::string>()));
   declare_parameter_if_not_declared(
     node, dwb_plugin_name_ + ".prune_plan",
     rclcpp::ParameterValue(true));

nav2_dwb_controller/dwb_plugins/src/limited_accel_generator.cpp

@@ -51,10 +51,20 @@ void LimitedAccelGenerator::initialize(
   plugin_name_ = plugin_name;
   StandardTrajectoryGenerator::initialize(nh, plugin_name_);
 
-  nav2_util::declare_parameter_if_not_declared(nh, plugin_name + ".sim_period");
-
-  if (nh->get_parameter(plugin_name + ".sim_period", acceleration_time_)) {
-  } else {
+  try {
+    nav2_util::declare_parameter_if_not_declared(
+      nh, plugin_name + ".sim_period", rclcpp::PARAMETER_DOUBLE);
+    if (!nh->get_parameter(plugin_name + ".sim_period", acceleration_time_)) {
+      // This actually should never appear, since declare_parameter_if_not_declared()
+      // completed w/o exceptions guarantee that static parameter will be initialized
+      // with some value. However for reliability we should also process the case
+      // when get_parameter() will return a failure for some other reasons.
+      throw std::runtime_error("Failed to get 'sim_period' value");
+    }
+  } catch (std::exception &) {
+    RCLCPP_WARN(
+      rclcpp::get_logger("LimitedAccelGenerator"),
+      "'sim_period' parameter is not set for %s", plugin_name.c_str());
     double controller_frequency = nav_2d_utils::searchAndGetParam(
       nh, "controller_frequency", 20.0);
     if (controller_frequency > 0) {

nav2_lifecycle_manager/src/lifecycle_manager.cpp

@@ -38,7 +38,7 @@ LifecycleManager::LifecycleManager()
 
   // The list of names is parameterized, allowing this module to be used with a different set
   // of nodes
-  declare_parameter("node_names");
+  declare_parameter("node_names", rclcpp::PARAMETER_STRING_ARRAY);
   declare_parameter("autostart", rclcpp::ParameterValue(false));
   declare_parameter("bond_timeout", 4.0);
 

nav2_map_server/src/map_server/map_server.cpp

@@ -68,7 +68,7 @@ MapServer::MapServer()
   RCLCPP_INFO(get_logger(), "Creating");
 
   // Declare the node parameters
-  declare_parameter("yaml_filename");
+  declare_parameter("yaml_filename", rclcpp::PARAMETER_STRING);
   declare_parameter("topic_name", "map");
   declare_parameter("frame_id", "map");
 }

nav2_util/include/nav2_util/node_utils.hpp

@@ -80,11 +80,20 @@ std::string time_to_string(size_t len);
 rclcpp::NodeOptions
 get_node_options_default(bool allow_undeclared = true, bool declare_initial_params = true);
 
+/// Declares static ROS2 parameter and sets it to a given value if it was not already declared
+/* Declares static ROS2 parameter and sets it to a given value
+ * if it was not already declared.
+ *
+ * \param[in] node A node in which given parameter to be declared
+ * \param[in] param_name The name of parameter
+ * \param[in] default_value Parameter value to initialize with
+ * \param[in] parameter_descriptor Parameter descriptor (optional)
+ */
 template<typename NodeT>
 void declare_parameter_if_not_declared(
   NodeT node,
   const std::string & param_name,
-  const rclcpp::ParameterValue & default_value = rclcpp::ParameterValue(),
+  const rclcpp::ParameterValue & default_value,
   const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
   rcl_interfaces::msg::ParameterDescriptor())
 {
@@ -93,15 +102,54 @@ void declare_parameter_if_not_declared(
   }
 }
 
+/// Declares static ROS2 parameter with given type if it was not already declared
+/* Declares static ROS2 parameter with given type if it was not already declared.
+ * NOTE: The parameter should be set via input param-file
+ * or throught a command-line. Otherwise according to the RCLCPP API,
+ * NoParameterOverrideProvided exception will be thrown by declare_parameter().
+ *
+ * \param[in] node A node in which given parameter to be declared
+ * \param[in] param_type The type of parameter
+ * \param[in] default_value Parameter value to initialize with
+ * \param[in] parameter_descriptor Parameter descriptor (optional)
+ */
+template<typename NodeT>
+void declare_parameter_if_not_declared(
+  NodeT node,
+  const std::string & param_name,
+  const rclcpp::ParameterType & param_type,
+  const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
+  rcl_interfaces::msg::ParameterDescriptor())
+{
+  if (!node->has_parameter(param_name)) {
+    node->declare_parameter(param_name, param_type, parameter_descriptor);
+  }
+}
+
+/// Gets the type of plugin for the selected node and its plugin
+/**
+ * Gets the type of plugin for the selected node and its plugin.
+ * Actually seeks for the value of "<plugin_name>.plugin" parameter.
+ *
+ * \param[in] node Selected node
+ * \param[in] plugin_name The name of plugin the type of which is being searched for
+ * \return A string containing the type of plugin (the value of "<plugin_name>.plugin" parameter)
+ */
 template<typename NodeT>
 std::string get_plugin_type_param(
   NodeT node,
   const std::string & plugin_name)
 {
-  declare_parameter_if_not_declared(node, plugin_name + ".plugin");
+  try {
+    declare_parameter_if_not_declared(node, plugin_name + ".plugin", rclcpp::PARAMETER_STRING);
+  } catch (rclcpp::exceptions::NoParameterOverrideProvided & ex) {
+    RCLCPP_FATAL(node->get_logger(), "'plugin' param not defined for %s", plugin_name.c_str());
+    exit(-1);
+  }
   std::string plugin_type;
   if (!node->get_parameter(plugin_name + ".plugin", plugin_type)) {
-    RCLCPP_FATAL(node->get_logger(), "'plugin' param not defined for %s", plugin_name.c_str());
+    RCLCPP_FATAL(
+      node->get_logger(), "Can not get 'plugin' param value for %s", plugin_name.c_str());
     exit(-1);
   }
   return plugin_type;