mppi parameters_handler: Improve verbose handling (#4704)

The "verbose" parameter of the parameters_handler is a special case that needs registration before the dynamic parameter handler callback is registered. In verbose mode make the parameter handler info/warn/debug messages more expressive. Signed-off-by: Mike Wake <[email protected]>
ros-navigation · Oct 7, 2024 · 7e9304b · 7e9304b
1 parent 4e62a89
commit 7e9304b
Show file tree

Hide file tree

Showing 3 changed files with 66 additions and 7 deletions.
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp
@@ -210,7 +210,16 @@ void ParametersHandler::getParam(
   setParam<ParamT>(setting, name, node);
 
   if (param_type == ParameterType::Dynamic) {
+    if (verbose_) {
+      RCLCPP_INFO(node->get_logger(), "setDynamicParamCallback for %s", name.c_str());
+    }
     setDynamicParamCallback(setting, name);
+  } else {
+    if (verbose_) {
+      RCLCPP_DEBUG(
+        node->get_logger(), "ParameterType::Static therefore no setDynamicParamCallback for %s",
+        name.c_str());
+    }
   }
 }
 

diff --git a/nav2_mppi_controller/src/parameters_handler.cpp b/nav2_mppi_controller/src/parameters_handler.cpp
@@ -38,13 +38,15 @@ ParametersHandler::~ParametersHandler()
 void ParametersHandler::start()
 {
   auto node = node_.lock();
+
+  // Register the special case "verbose" parameter before registering dynamicParamsCallback
+  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
@@ -53,6 +55,7 @@ ParametersHandler::dynamicParamsCallback(
 {
   rcl_interfaces::msg::SetParametersResult result;
   std::lock_guard<std::mutex> lock(parameters_change_mutex_);
+  bool success;
 
   for (auto & pre_cb : pre_callbacks_) {
     pre_cb();
@@ -66,15 +69,21 @@ ParametersHandler::dynamicParamsCallback(
     {
       callback->second(param);
     } else {
-      RCLCPP_WARN(logger_, "Parameter %s not found", param_name.c_str());
+      if (verbose_) {
+        // Expected if static parameter, ie one with no registered callback is attempted
+        // to be changed
+        RCLCPP_WARN(logger_, "Parameter callback func for '%s' not found", param_name.c_str());
+      }
+      // success = true; // Decision ??? Still return true to avoid exception ???
+      success = false;
     }
   }
 
   for (auto & post_cb : post_callbacks_) {
     post_cb();
   }
 
-  result.successful = true;
+  result.successful = success;
   return result;
 }
 

diff --git a/nav2_mppi_controller/test/parameter_handler_test.cpp b/nav2_mppi_controller/test/parameter_handler_test.cpp
@@ -168,8 +168,49 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersTest)
     node->get_node_services_interface());
 
   auto results = rec_param->set_parameters_atomically(
-    {rclcpp::Parameter("dynamic_int", 10),
-      rclcpp::Parameter("static_int", 10)});
+  {
+    rclcpp::Parameter("my_node.verbose", true),
+    rclcpp::Parameter("dynamic_int", 10),
+    rclcpp::Parameter("static_int", 10)
+  });
+
+  rclcpp::spin_until_future_complete(
+    node->get_node_base_interface(),
+    results);
+
+  // 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));
+  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());
+
+  auto results = 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(
     node->get_node_base_interface(),