Dynamic param patterns (ros-navigation#4971)

* redesign dynamic param patterns

Signed-off-by: Nils-ChristianIseke <nilsmailiseke@gmail.com>

* change cache version

Signed-off-by: Nils-ChristianIseke <nilsmailiseke@gmail.com>

* check that parameter of type double are  >=0.0

Signed-off-by: Nils-ChristianIseke <nilsmailiseke@gmail.com>

---------

Signed-off-by: Nils-ChristianIseke <nilsmailiseke@gmail.com>
Signed-off-by: Sakshay Mahna <sakshum19@gmail.com>

Author: Nils-ChristianIseke

.circleci/config.yml

@@ -33,12 +33,12 @@ _commands:
         - restore_cache:
             name: Restore Cache << parameters.key >>
             keys:
-              - "<< parameters.key >>-v32\
+              - "<< parameters.key >>-v33\
                 -{{ arch }}\
                 -{{ .Branch }}\
                 -{{ .Environment.CIRCLE_PR_NUMBER }}\
                 -{{ checksum  \"<< parameters.workspace >>/lockfile.txt\" }}"
-              - "<< parameters.key >>-v32\
+              - "<< parameters.key >>-v33\
                 -{{ arch }}\
                 -main\
                 -<no value>\
@@ -58,7 +58,7 @@ _commands:
       steps:
         - save_cache:
             name: Save Cache << parameters.key >>
-            key: "<< parameters.key >>-v32\
+            key: "<< parameters.key >>-v33\
               -{{ arch }}\
               -{{ .Branch }}\
               -{{ .Environment.CIRCLE_PR_NUMBER }}\

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp

@@ -93,12 +93,14 @@ class ParameterHandler
    * @brief Callback executed when a parameter change is detected
    * @param event ParameterEvent message
    */
+  void
+  updateParametersCallback(std::vector<rclcpp::Parameter> parameters);
   rcl_interfaces::msg::SetParametersResult
-  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
-
+  validateParameterUpdatesCallback(std::vector<rclcpp::Parameter> parameters);
   // Dynamic parameters handler
   std::mutex mutex_;
-  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
+  rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr post_set_params_handler_;
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_params_handler_;
   Parameters params_;
   std::string plugin_name_;
   rclcpp::Logger logger_ {rclcpp::get_logger("RegulatedPurePursuitController")};

nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp

@@ -189,40 +189,75 @@ ParameterHandler::ParameterHandler(
     params_.use_cost_regulated_linear_velocity_scaling = false;
   }
 
-  dyn_params_handler_ = node->add_on_set_parameters_callback(
+  post_set_params_handler_ = node->add_post_set_parameters_callback(
     std::bind(
-      &ParameterHandler::dynamicParametersCallback,
+      &ParameterHandler::updateParametersCallback,
+      this, std::placeholders::_1));
+  on_set_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(
+      &ParameterHandler::validateParameterUpdatesCallback,
       this, std::placeholders::_1));
 }
 
 ParameterHandler::~ParameterHandler()
 {
   auto node = node_.lock();
-  if (dyn_params_handler_ && node) {
-    node->remove_on_set_parameters_callback(dyn_params_handler_.get());
+  if (post_set_params_handler_ && node) {
+    node->remove_post_set_parameters_callback(post_set_params_handler_.get());
+  }
+  post_set_params_handler_.reset();
+  if (on_set_params_handler_ && node) {
+    node->remove_on_set_parameters_callback(on_set_params_handler_.get());
   }
-  dyn_params_handler_.reset();
+  on_set_params_handler_.reset();
 }
-
-rcl_interfaces::msg::SetParametersResult
-ParameterHandler::dynamicParametersCallback(
+rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpdatesCallback(
   std::vector<rclcpp::Parameter> parameters)
 {
   rcl_interfaces::msg::SetParametersResult result;
-  std::lock_guard<std::mutex> lock_reinit(mutex_);
-
+  result.successful = true;
   for (auto parameter : parameters) {
     const auto & type = parameter.get_type();
     const auto & name = parameter.get_name();
 
     if (type == ParameterType::PARAMETER_DOUBLE) {
-      if (name == plugin_name_ + ".inflation_cost_scaling_factor") {
-        if (parameter.as_double() <= 0.0) {
+      if (name == plugin_name_ + ".inflation_cost_scaling_factor" && parameter.as_double() <= 0.0) {
+        RCLCPP_WARN(
+        logger_, "The value inflation_cost_scaling_factor is incorrectly set, "
+        "it should be >0. Ignoring parameter update.");
+        result.successful = false;
+      } else if (parameter.as_double() < 0.0) {
+        RCLCPP_WARN(
+        logger_, "The value of parameter '%s' is incorrectly set to %f, "
+        "it should be >=0. Ignoring parameter update.",
+        name.c_str(), parameter.as_double());
+        result.successful = false;
+      }
+    } else if (type == ParameterType::PARAMETER_BOOL) {
+      if (name == plugin_name_ + ".allow_reversing") {
+        if (params_.use_rotate_to_heading && parameter.as_bool()) {
           RCLCPP_WARN(
-            logger_, "The value inflation_cost_scaling_factor is incorrectly set, "
-            "it should be >0. Ignoring parameter update.");
-          continue;
+            logger_, "Both use_rotate_to_heading and allow_reversing "
+            "parameter cannot be set to true. Rejecting parameter update.");
+          result.successful = false;
         }
+      }
+    }
+  }
+  return result;
+}
+void
+ParameterHandler::updateParametersCallback(
+  std::vector<rclcpp::Parameter> parameters)
+{
+  std::lock_guard<std::mutex> lock_reinit(mutex_);
+
+  for (const auto & parameter : parameters) {
+    const auto & type = parameter.get_type();
+    const auto & name = parameter.get_name();
+
+    if (type == ParameterType::PARAMETER_DOUBLE) {
+      if (name == plugin_name_ + ".inflation_cost_scaling_factor") {
         params_.inflation_cost_scaling_factor = parameter.as_double();
       } else if (name == plugin_name_ + ".desired_linear_vel") {
         params_.desired_linear_vel = parameter.as_double();
@@ -257,6 +292,10 @@ ParameterHandler::dynamicParametersCallback(
         params_.cancel_deceleration = parameter.as_double();
       } else if (name == plugin_name_ + ".rotate_to_heading_min_angle") {
         params_.rotate_to_heading_min_angle = parameter.as_double();
+      } else if (name == plugin_name_ + ".transform_tolerance") {
+        params_.transform_tolerance = parameter.as_double();
+      } else if (name == plugin_name_ + ".max_robot_pose_search_dist") {
+        params_.max_robot_pose_search_dist = parameter.as_double();
       }
     } else if (type == ParameterType::PARAMETER_BOOL) {
       if (name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") {
@@ -274,19 +313,12 @@ ParameterHandler::dynamicParametersCallback(
       } else if (name == plugin_name_ + ".use_cancel_deceleration") {
         params_.use_cancel_deceleration = parameter.as_bool();
       } else if (name == plugin_name_ + ".allow_reversing") {
-        if (params_.use_rotate_to_heading && parameter.as_bool()) {
-          RCLCPP_WARN(
-            logger_, "Both use_rotate_to_heading and allow_reversing "
-            "parameter cannot be set to true. Rejecting parameter update.");
-          continue;
-        }
         params_.allow_reversing = parameter.as_bool();
+      } else if (name == plugin_name_ + ".interpolate_curvature_after_goal") {
+        params_.interpolate_curvature_after_goal = parameter.as_bool();
       }
     }
   }
-
-  result.successful = true;
-  return result;
 }
 
 }  // namespace nav2_regulated_pure_pursuit_controller