mppi parameters_handler: Improve verbose handling (#4704)

nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp

@@ -36,12 +36,13 @@
 
 /**
  * @class mppi::ParametersHandler
- * @brief Handles getting parameters and dynamic parmaeter changes
+ * @brief Handles getting parameters and dynamic parameter changes
  */
 class ParametersHandler
 {
 public:
-  using get_param_func_t = void (const rclcpp::Parameter & param);
+  using get_param_func_t = void (const rclcpp::Parameter & param,
+    rcl_interfaces::msg::SetParametersResult & result);
   using post_callback_t = void ();
   using pre_callback_t = void ();
 
@@ -100,9 +101,11 @@
     * @brief Set a parameter to a dynamic parameter callback
     * @param setting Parameter
     * @param name Name of parameter
+    * @param param_type Type of parameter (dynamic or static)
     */
   template<typename T>
-  void setDynamicParamCallback(T & setting, const std::string & name);
+  void setParamCallback(
+    T & setting, const std::string & name, ParameterType param_type = ParameterType::Dynamic);
 
   /**
     * @brief Get mutex lock for changing parameters
@@ -114,12 +117,20 @@
   }
 
   /**
-    * @brief Set a parameter to a dynamic parameter callback
+    * @brief register a function to be called when setting a parameter
+    *
+    * The callback funciton is expected to behave as follows.
+    * Successful parameter changes should not interfere with
+    * the result parameter.
+    * Unsuccessful parameter changes should set the result.successful = false
+    * The result.reason should have "\n" appended if not empty before
+    * appending the reason that setting THIS parameter has failed.
+    *
     * @param name Name of parameter
     * @param callback Parameter callback
     */
   template<typename T>
-  void addDynamicParamCallback(const std::string & name, T && callback);
+  void addParamCallback(const std::string & name, T && callback);
 
 protected:
   /**
@@ -160,8 +171,7 @@
 
   bool verbose_{false};
 
-  std::unordered_map<std::string, std::function<get_param_func_t>>
-  get_param_callbacks_;
+  std::unordered_map<std::string, std::function<get_param_func_t>> get_param_callbacks_;
 
   std::vector<std::function<pre_callback_t>> pre_callbacks_;
   std::vector<std::function<post_callback_t>> post_callbacks_;
@@ -179,7 +189,7 @@
 }
 
 template<typename T>
-void ParametersHandler::addDynamicParamCallback(const std::string & name, T && callback)
+void ParametersHandler::addParamCallback(const std::string & name, T && callback)
 {
   get_param_callbacks_[name] = callback;
 }
@@ -208,10 +218,7 @@
     node, name, rclcpp::ParameterValue(default_value));
 
   setParam<ParamT>(setting, name, node);
-
-  if (param_type == ParameterType::Dynamic) {
-    setDynamicParamCallback(setting, name);
-  }
+  setParamCallback(setting, name, param_type);
 }
 
 template<typename ParamT, typename SettingT, typename NodeT>
@@ -224,24 +231,37 @@
 }
 
 template<typename T>
-void ParametersHandler::setDynamicParamCallback(T & setting, const std::string & name)
+void ParametersHandler::setParamCallback(
+  T & setting, const std::string & name, ParameterType param_type)
 {
   if (get_param_callbacks_.find(name) != get_param_callbacks_.end()) {
     return;
   }
 
-  auto callback = [this, &setting, name](const rclcpp::Parameter & param) {
+  auto dynamic_callback =
+    [this, &setting, name](
+    const rclcpp::Parameter & param, rcl_interfaces::msg::SetParametersResult & /*result*/) {
       setting = as<T>(param);
-
       if (verbose_) {
         RCLCPP_INFO(logger_, "Dynamic parameter changed: %s", std::to_string(param).c_str());
       }
     };
 
-  addDynamicParamCallback(name, callback);
+  auto static_callback =
+    [this, &setting, name](
+    const rclcpp::Parameter & param, rcl_interfaces::msg::SetParametersResult & result) {
+      std::string reason = "Rejected change to static parameter: " + std::to_string(param);
+      result.successful = false;
+      if (!result.reason.empty()) {
+        result.reason += "\n";
+      }
+      result.reason += reason;
+    };
 
-  if (verbose_) {
-    RCLCPP_INFO(logger_, "Dynamic Parameter added %s", name.c_str());
+  if (param_type == ParameterType::Dynamic) {
+    addParamCallback(name, dynamic_callback);
+  } else {
+    addParamCallback(name, static_callback);
   }
 }
 

nav2_mppi_controller/src/critics/cost_critic.cpp

@@ -35,10 +35,11 @@ void CostCritic::initialize()
   weight_ /= 254.0f;
 
   // Normalize weight when parameter is changed dynamically as well
-  auto weightDynamicCb = [&](const rclcpp::Parameter & weight) {
+  auto weightDynamicCb = [&](
+    const rclcpp::Parameter & weight, rcl_interfaces::msg::SetParametersResult & /*result*/) {
       weight_ = weight.as_double() / 254.0f;
     };
-  parameters_handler_->addDynamicParamCallback(name_ + ".cost_weight", weightDynamicCb);
+  parameters_handler_->addParamCallback(name_ + ".cost_weight", weightDynamicCb);
 
   collision_checker_.setCostmap(costmap_);
   possible_collision_cost_ = findCircumscribedCost(costmap_ros_);

nav2_mppi_controller/src/parameters_handler.cpp

@@ -38,20 +38,24 @@ ParametersHandler::~ParametersHandler()
 void ParametersHandler::start()
 {
   auto node = node_.lock();
+
+  auto get_param = getParamGetter(node_name_);
+  get_param(verbose_, "verbose", false);
+
   on_set_param_handler_ = node->add_on_set_parameters_callback(
     std::bind(
       &ParametersHandler::dynamicParamsCallback, this,
       std::placeholders::_1));
-
-  auto get_param = getParamGetter(node_name_);
-  get_param(verbose_, "verbose", false);
 }
 
 rcl_interfaces::msg::SetParametersResult
 ParametersHandler::dynamicParamsCallback(
   std::vector<rclcpp::Parameter> parameters)
 {
   rcl_interfaces::msg::SetParametersResult result;
+  result.successful = true;
+  result.reason = "";
+
   std::lock_guard<std::mutex> lock(parameters_change_mutex_);
 
   for (auto & pre_cb : pre_callbacks_) {
@@ -64,17 +68,23 @@ ParametersHandler::dynamicParamsCallback(
     if (auto callback = get_param_callbacks_.find(param_name);
       callback != get_param_callbacks_.end())
     {
-      callback->second(param);
+      callback->second(param, result);
     } else {
-      RCLCPP_WARN(logger_, "Parameter %s not found", param_name.c_str());
+      result.successful = false;
+      if (!result.reason.empty()) {
+        result.reason += "\n";
+      }
+      result.reason += "get_param_callback func for '" + param_name + "' not found.\n";
     }
   }
 
   for (auto & post_cb : post_callbacks_) {
     post_cb();
   }
 
-  result.successful = true;
+  if (!result.successful) {
+    RCLCPP_ERROR(logger_, result.reason.c_str());
+  }
   return result;
 }
 

nav2_mppi_controller/test/parameter_handler_test.cpp

@@ -93,7 +93,8 @@ TEST(ParameterHandlerTest, PrePostDynamicCallbackTest)
       post_triggered = true;
     };
 
-  auto dynamicCb = [&](const rclcpp::Parameter & /*param*/) {
+  auto dynamicCb = [&](const rclcpp::Parameter & /*param*/,
+    rcl_interfaces::msg::SetParametersResult & /*result*/) {
       dynamic_triggered = true;
     };
 
@@ -104,8 +105,8 @@ TEST(ParameterHandlerTest, PrePostDynamicCallbackTest)
   ParametersHandlerWrapper a;
   a.addPreCallback(preCb);
   a.addPostCallback(postCb);
-  a.addDynamicParamCallback("use_sim_time", dynamicCb);
-  a.setDynamicParamCallback(val, "blah_blah");
+  a.addParamCallback("use_sim_time", dynamicCb);
+  a.setParamCallback(val, "blah_blah");
 
   // Dynamic callback should not trigger, wrong parameter, but val should be updated
   a.dynamicParamsCallback(std::vector<rclcpp::Parameter>{random_param});
@@ -146,6 +147,52 @@ TEST(ParameterHandlerTest, GetSystemParamsTest)
 }
 
 TEST(ParameterHandlerTest, DynamicAndStaticParametersTest)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
+
+  node->declare_parameter("dynamic_int", rclcpp::ParameterValue(7));
+  node->declare_parameter("static_int", rclcpp::ParameterValue(7));
+  ParametersHandlerWrapper handler(node);
+  handler.start();
+
+  // Get parameters and check they have initial values
+  auto getParamer = handler.getParamGetter("");
+  int p1 = 0, p2 = 0;
+  getParamer(p1, "dynamic_int", 0, ParameterType::Dynamic);
+  getParamer(p2, "static_int", 0, ParameterType::Static);
+  EXPECT_EQ(p1, 7);
+  EXPECT_EQ(p2, 7);
+
+  // Now change them both via dynamic parameters
+  auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
+    node->get_node_base_interface(), node->get_node_topics_interface(),
+    node->get_node_graph_interface(),
+    node->get_node_services_interface());
+
+  std::shared_future<rcl_interfaces::msg::SetParametersResult> result_future =
+    rec_param->set_parameters_atomically({
+    rclcpp::Parameter("my_node.verbose", true),
+    rclcpp::Parameter("dynamic_int", 10),
+    rclcpp::Parameter("static_int", 10)
+  });
+
+  auto rc = rclcpp::spin_until_future_complete(
+    node->get_node_base_interface(),
+    result_future);
+  ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, rc);
+
+  auto result = result_future.get();
+  EXPECT_EQ(result.successful, false);
+  EXPECT_FALSE(result.reason.empty());
+  EXPECT_EQ(result.reason, std::string("Rejected change to static parameter: ") +
+    "{\"name\": \"static_int\", \"type\": \"integer\", \"value\": \"10\"}");
+
+  // Now, only param1 should change, param 2 should be the same
+  EXPECT_EQ(p1, 10);
+  EXPECT_EQ(p2, 7);
+}
+
+TEST(ParameterHandlerTest, DynamicAndStaticParametersNotVerboseTest)
 {
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
   node->declare_parameter("dynamic_int", rclcpp::ParameterValue(7));
@@ -167,15 +214,85 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersTest)
     node->get_node_graph_interface(),
     node->get_node_services_interface());
 
-  auto results = rec_param->set_parameters_atomically(
-    {rclcpp::Parameter("dynamic_int", 10),
-      rclcpp::Parameter("static_int", 10)});
+  std::shared_future<rcl_interfaces::msg::SetParametersResult> result_future =
+    rec_param->set_parameters_atomically({
+    // Don't set default param rclcpp::Parameter("my_node.verbose", false),
+    rclcpp::Parameter("dynamic_int", 10),
+    rclcpp::Parameter("static_int", 10)
+  });
 
-  rclcpp::spin_until_future_complete(
+  auto rc = rclcpp::spin_until_future_complete(
     node->get_node_base_interface(),
-    results);
+    result_future);
+  ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, rc);
+
+  auto result = result_future.get();
+  EXPECT_EQ(result.successful, false);
+  EXPECT_FALSE(result.reason.empty());
+  EXPECT_EQ(result.reason, std::string("Rejected change to static parameter: ") +
+    "{\"name\": \"static_int\", \"type\": \"integer\", \"value\": \"10\"}");
 
   // Now, only param1 should change, param 2 should be the same
   EXPECT_EQ(p1, 10);
   EXPECT_EQ(p2, 7);
 }
+
+TEST(ParameterHandlerTest, DynamicAndStaticParametersNotDeclaredTest)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
+
+  node->declare_parameter("dynamic_int", rclcpp::ParameterValue(7));
+  node->declare_parameter("static_int", rclcpp::ParameterValue(7));
+  ParametersHandlerWrapper handler(node);
+  handler.start();
+
+  // Set verbose true to get more information about bad parameter usage
+  auto getParamer = handler.getParamGetter("");
+  auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
+    node->get_node_base_interface(), node->get_node_topics_interface(),
+    node->get_node_graph_interface(),
+    node->get_node_services_interface());
+
+  std::shared_future<rcl_interfaces::msg::SetParametersResult>
+  result_future = rec_param->set_parameters_atomically({
+    rclcpp::Parameter("my_node.verbose", true),
+  });
+
+  auto rc = rclcpp::spin_until_future_complete(
+    node->get_node_base_interface(),
+    result_future);
+  ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, rc);
+
+  auto result = result_future.get();
+  EXPECT_EQ(result.successful, true);
+  EXPECT_TRUE(result.reason.empty());
+
+  // Try to access some parameters that have not been declared
+  int p1 = 0, p2 = 0;
+  EXPECT_THROW(getParamer(p1, "not_declared", 8, ParameterType::Dynamic),
+               rclcpp::exceptions::InvalidParameterValueException);
+  EXPECT_THROW(getParamer(p2, "not_declared2", 9, ParameterType::Static),
+               rclcpp::exceptions::InvalidParameterValueException);
+
+  // Try to set some parameters that have not been declared via the service client
+  result_future = rec_param->set_parameters_atomically({
+    rclcpp::Parameter("static_int", 10),
+    rclcpp::Parameter("not_declared", true),
+    rclcpp::Parameter("not_declared2", true),
+  });
+
+  rc = rclcpp::spin_until_future_complete(
+    node->get_node_base_interface(),
+    result_future);
+  ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, rc);
+
+  result = result_future.get();
+  EXPECT_EQ(result.successful, false);
+  EXPECT_FALSE(result.reason.empty());
+  // The ParameterNotDeclaredException handler in rclcpp/parameter_service.cpp
+  // overrides any other reasons and does not provide details to the service client.
+  EXPECT_EQ(result.reason, std::string("One or more parameters were not declared before setting"));
+
+  EXPECT_EQ(p1, 0);
+  EXPECT_EQ(p2, 0);
+}