mppi parameters_handler: Improve static/dynamic/not defined logging (#…

…4704) Attempts to change undefined parameters will not be successful and will log an error. Attempts to change static parameters will be ignored, a debug message is logged if a change in parameters is attempted.
ros-navigation · Oct 8, 2024 · b8d38c7 · b8d38c7
1 parent 7cb8ef5
commit b8d38c7
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 23 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
@@ -100,9 +100,11 @@ class ParametersHandler
     * @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 setDynamicParamCallback(
+    T & setting, const std::string & name, ParameterType param_type = ParameterType::Dynamic);
 
   /**
     * @brief Get mutex lock for changing parameters
@@ -208,19 +210,7 @@ void ParametersHandler::getParam(
     node, name, rclcpp::ParameterValue(default_value));
 
   setParam<ParamT>(setting, name, node);
-
-  if (param_type == ParameterType::Dynamic) {
-    if (verbose_) {
-      RCLCPP_DEBUG(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());
-    }
-  }
+  setDynamicParamCallback(setting, name, param_type);
 }
 
 template<typename ParamT, typename SettingT, typename NodeT>
@@ -233,24 +223,32 @@ void ParametersHandler::setParam(
 }
 
 template<typename T>
-void ParametersHandler::setDynamicParamCallback(T & setting, const std::string & name)
+void ParametersHandler::setDynamicParamCallback(
+  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) {
       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) {
+      if (verbose_) {
+        RCLCPP_DEBUG(logger_, "Ignoring change to static parameter: %s",
+          std::to_string(param).c_str());
+      }
+    };
 
-  if (verbose_) {
-    RCLCPP_INFO(logger_, "Dynamic Parameter added %s", name.c_str());
+  if (param_type == ParameterType::Dynamic) {
+    addDynamicParamCallback(name, dynamic_callback);
+  } else {
+    addDynamicParamCallback(name, static_callback);
   }
 }
 

diff --git a/nav2_mppi_controller/src/parameters_handler.cpp b/nav2_mppi_controller/src/parameters_handler.cpp
@@ -54,7 +54,7 @@ ParametersHandler::dynamicParamsCallback(
 {
   rcl_interfaces::msg::SetParametersResult result;
   std::lock_guard<std::mutex> lock(parameters_change_mutex_);
-  bool success;
+  bool success = true;
 
   for (auto & pre_cb : pre_callbacks_) {
     pre_cb();
@@ -68,9 +68,7 @@ ParametersHandler::dynamicParamsCallback(
     {
       callback->second(param);
     } else {
-      if (verbose_) {
-        RCLCPP_WARN(logger_, "Parameter callback func for '%s' not found", param_name.c_str());
-      }
+      RCLCPP_ERROR(logger_, "Parameter callback func for '%s' not found", param_name.c_str());
       success = false;
     }
   }