adding dynamic params to RPP and cleaningup style in others (ros-navigation#2607)

* adding dynamic params to RPP and cleaningup style in others

* adding tests

* tests working

Author: SteveMacenski

nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp

@@ -76,7 +76,7 @@ class SimpleGoalChecker : public nav2_core::GoalChecker
   // Cached squared xy_goal_tolerance_
   double xy_goal_tolerance_sq_;
   // Dynamic parameters handler
-  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler;
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
   std::string plugin_name_;
 
   /**

nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp

@@ -63,7 +63,7 @@ class SimpleProgressChecker : public nav2_core::ProgressChecker
 
   bool baseline_pose_set_{false};
   // Dynamic parameters handler
-  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler;
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
   std::string plugin_name_;
 
   /**

nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp

@@ -68,7 +68,7 @@ class StoppedGoalChecker : public SimpleGoalChecker
 protected:
   double rot_stopped_velocity_, trans_stopped_velocity_;
   // Dynamic parameters handler
-  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler;
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
   std::string plugin_name_;
 
   /**

nav2_controller/plugins/simple_goal_checker.cpp

@@ -85,7 +85,7 @@ void SimpleGoalChecker::initialize(
   xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
 
   // Add callback for dynamic parameters
-  dyn_params_handler = node->add_on_set_parameters_callback(
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
     std::bind(&SimpleGoalChecker::dynamicParametersCallback, this, _1));
 }
 

nav2_controller/plugins/simple_progress_checker.cpp

@@ -51,7 +51,7 @@ void SimpleProgressChecker::initialize(
   time_allowance_ = rclcpp::Duration::from_seconds(time_allowance_param);
 
   // Add callback for dynamic parameters
-  dyn_params_handler = node->add_on_set_parameters_callback(
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
     std::bind(&SimpleProgressChecker::dynamicParametersCallback, this, _1));
 }
 

nav2_controller/plugins/stopped_goal_checker.cpp

@@ -75,7 +75,7 @@ void StoppedGoalChecker::initialize(
   node->get_parameter(plugin_name + ".trans_stopped_velocity", trans_stopped_velocity_);
 
   // Add callback for dynamic parameters
-  dyn_params_handler = node->add_on_set_parameters_callback(
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
     std::bind(&StoppedGoalChecker::dynamicParametersCallback, this, _1));
 }
 

nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp

@@ -122,7 +122,7 @@ class KinematicsHandler
   std::atomic<KinematicParameters *> kinematics_;
 
   // Dynamic parameters handler
-  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler;
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
   /**
    * @brief Callback executed when a paramter change is detected
    * @param parameters list of changed parameters

nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp

@@ -115,7 +115,7 @@ void KinematicsHandler::initialize(
   kinematics.base_max_vel_theta_ = kinematics.max_vel_theta_;
 
   // Add callback for dynamic parameters
-  dyn_params_handler = nh->add_on_set_parameters_callback(
+  dyn_params_handler_ = nh->add_on_set_parameters_callback(
     std::bind(&KinematicsHandler::dynamicParametersCallback, this, _1));
 
   kinematics.min_speed_xy_sq_ = kinematics.min_speed_xy_ * kinematics.min_speed_xy_;

nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp

@@ -219,7 +219,7 @@ class NavfnPlanner : public nav2_core::GlobalPlanner
   rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
 
   // Dynamic parameters handler
-  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler;
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
 
   /**
    * @brief Callback executed when a paramter change is detected

nav2_navfn_planner/src/navfn_planner.cpp

@@ -104,7 +104,7 @@ NavfnPlanner::activate()
     name_.c_str());
   // Add callback for dynamic parameters
   auto node = node_.lock();
-  dyn_params_handler = node->add_on_set_parameters_callback(
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
     std::bind(&NavfnPlanner::dynamicParametersCallback, this, _1));
 }
 
@@ -114,7 +114,7 @@ NavfnPlanner::deactivate()
   RCLCPP_INFO(
     logger_, "Deactivating plugin %s of type NavfnPlanner",
     name_.c_str());
-  dyn_params_handler.reset();
+  dyn_params_handler_.reset();
 }
 
 void

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp

@@ -20,6 +20,7 @@
 #include <vector>
 #include <memory>
 #include <algorithm>
+#include <mutex>
 
 #include "nav2_core/controller.hpp"
 #include "rclcpp/rclcpp.hpp"
@@ -227,6 +228,14 @@ class RegulatedPurePursuitController : public nav2_core::Controller
    */
   double findDirectionChange(const geometry_msgs::msg::PoseStamped & pose);
 
+  /**
+   * @brief Callback executed when a parameter change is detected
+   * @param event ParameterEvent message
+   */
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
+
+  rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
   std::shared_ptr<tf2_ros::Buffer> tf_;
   std::string plugin_name_;
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
@@ -266,6 +275,10 @@ class RegulatedPurePursuitController : public nav2_core::Controller
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PointStamped>>
   carrot_pub_;
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> carrot_arc_pub_;
+
+  // Dynamic parameters handler
+  std::mutex mutex_;
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
 };
 
 }  // namespace nav2_regulated_pure_pursuit_controller

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

@@ -17,6 +17,7 @@
 #include <string>
 #include <limits>
 #include <memory>
+#include <vector>
 #include <utility>
 
 #include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp"
@@ -32,6 +33,7 @@ using std::abs;
 using nav2_util::declare_parameter_if_not_declared;
 using nav2_util::geometry_utils::euclidean_distance;
 using namespace nav2_costmap_2d;  // NOLINT
+using rcl_interfaces::msg::ParameterType;
 
 namespace nav2_regulated_pure_pursuit_controller
 {
@@ -42,6 +44,7 @@ void RegulatedPurePursuitController::configure(
   const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> & costmap_ros)
 {
   auto node = parent.lock();
+  node_ = parent;
   if (!node) {
     throw nav2_core::PlannerException("Unable to lock node!");
   }
@@ -202,6 +205,12 @@ void RegulatedPurePursuitController::activate()
   global_path_pub_->on_activate();
   carrot_pub_->on_activate();
   carrot_arc_pub_->on_activate();
+  // Add callback for dynamic parameters
+  auto node = node_.lock();
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(
+      &RegulatedPurePursuitController::dynamicParametersCallback,
+      this, std::placeholders::_1));
 }
 
 void RegulatedPurePursuitController::deactivate()
@@ -214,6 +223,7 @@ void RegulatedPurePursuitController::deactivate()
   global_path_pub_->on_deactivate();
   carrot_pub_->on_deactivate();
   carrot_arc_pub_->on_deactivate();
+  dyn_params_handler_.reset();
 }
 
 std::unique_ptr<geometry_msgs::msg::PointStamped> RegulatedPurePursuitController::createCarrotMsg(
@@ -245,6 +255,8 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
   const geometry_msgs::msg::Twist & speed,
   nav2_core::GoalChecker * goal_checker)
 {
+  std::lock_guard<std::mutex> lock_reinit(mutex_);
+
   // Update for the current goal checker's state
   geometry_msgs::msg::Pose pose_tolerance;
   geometry_msgs::msg::Twist vel_tolerance;
@@ -668,6 +680,97 @@ bool RegulatedPurePursuitController::transformPose(
   return false;
 }
 
+
+rcl_interfaces::msg::SetParametersResult
+RegulatedPurePursuitController::dynamicParametersCallback(
+  std::vector<rclcpp::Parameter> parameters)
+{
+  rcl_interfaces::msg::SetParametersResult result;
+  std::lock_guard<std::mutex> lock_reinit(mutex_);
+
+  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) {
+          RCLCPP_WARN(
+            logger_, "The value inflation_cost_scaling_factor is incorrectly set, "
+            "it should be >0. Ignoring parameter update.");
+          continue;
+        }
+        inflation_cost_scaling_factor_ = parameter.as_double();
+      } else if (name == plugin_name_ + ".desired_linear_vel") {
+        desired_linear_vel_ = parameter.as_double();
+        base_desired_linear_vel_ = parameter.as_double();
+      } else if (name == plugin_name_ + ".lookahead_dist") {
+        lookahead_dist_ = parameter.as_double();
+      } else if (name == plugin_name_ + ".max_lookahead_dist") {
+        max_lookahead_dist_ = parameter.as_double();
+      } else if (name == plugin_name_ + ".min_lookahead_dist") {
+        min_lookahead_dist_ = parameter.as_double();
+      } else if (name == plugin_name_ + ".lookahead_time") {
+        lookahead_time_ = parameter.as_double();
+      } else if (name == plugin_name_ + ".rotate_to_heading_angular_vel") {
+        rotate_to_heading_angular_vel_ = parameter.as_double();
+      } else if (name == plugin_name_ + ".max_linear_accel") {
+        max_linear_accel_ = parameter.as_double();
+      } else if (name == plugin_name_ + ".max_linear_decel") {
+        max_linear_decel_ = parameter.as_double();
+      } else if (name == plugin_name_ + ".min_approach_linear_velocity") {
+        min_approach_linear_velocity_ = parameter.as_double();
+      } else if (name == plugin_name_ + ".max_allowed_time_to_collision") {
+        max_allowed_time_to_collision_ = parameter.as_double();
+      } else if (name == plugin_name_ + ".cost_scaling_dist") {
+        cost_scaling_dist_ = parameter.as_double();
+      } else if (name == plugin_name_ + ".cost_scaling_gain") {
+        cost_scaling_gain_ = parameter.as_double();
+      } else if (name == plugin_name_ + ".regulated_linear_scaling_min_radius") {
+        regulated_linear_scaling_min_radius_ = parameter.as_double();
+      } else if (name == plugin_name_ + ".transform_tolerance") {
+        double transform_tolerance = parameter.as_double();
+        transform_tolerance_ = tf2::durationFromSec(transform_tolerance);
+      } else if (name == plugin_name_ + ".regulated_linear_scaling_min_speed") {
+        regulated_linear_scaling_min_speed_ = parameter.as_double();
+      } else if (name == plugin_name_ + ".max_angular_accel") {
+        max_angular_accel_ = parameter.as_double();
+      } else if (name == plugin_name_ + ".rotate_to_heading_min_angle") {
+        rotate_to_heading_min_angle_ = parameter.as_double();
+      }
+    } else if (type == ParameterType::PARAMETER_BOOL) {
+      if (name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") {
+        use_velocity_scaled_lookahead_dist_ = parameter.as_bool();
+      } else if (name == plugin_name_ + ".use_regulated_linear_velocity_scaling") {
+        use_regulated_linear_velocity_scaling_ = parameter.as_bool();
+      } else if (name == plugin_name_ + ".use_cost_regulated_linear_velocity_scaling") {
+        use_cost_regulated_linear_velocity_scaling_ = parameter.as_bool();
+      } else if (name == plugin_name_ + ".use_approach_vel_scaling") {
+        use_approach_vel_scaling_ = parameter.as_bool();
+      } else if (name == plugin_name_ + ".use_rotate_to_heading") {
+        if (parameter.as_bool() && allow_reversing_) {
+          RCLCPP_WARN(
+            logger_, "Both use_rotate_to_heading and allow_reversing "
+            "parameter cannot be set to true. Rejecting parameter update.");
+          continue;
+        }
+        use_rotate_to_heading_ = parameter.as_bool();
+      } else if (name == plugin_name_ + ".allow_reversing") {
+        if (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;
+        }
+        allow_reversing_ = parameter.as_bool();
+      }
+    }
+  }
+
+  result.successful = true;
+  return result;
+}
+
 }  // namespace nav2_regulated_pure_pursuit_controller
 
 // Register this controller as a nav2_core plugin

nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp

@@ -382,3 +382,75 @@ TEST(RegulatedPurePursuitTest, applyConstraints)
   //   dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel);
   // EXPECT_NEAR(linear_vel, 0.5, 0.01);
 }
+
+TEST(RegulatedPurePursuitTest, testDynamicParameter)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("Smactest");
+  auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("global_costmap");
+  costmap->on_configure(rclcpp_lifecycle::State());
+  auto ctrl =
+    std::make_unique<nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController>();
+  auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+  ctrl->configure(node, "test", tf, costmap);
+  ctrl->activate();
+
+  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(
+    {rclcpp::Parameter("test.desired_linear_vel", 1.0),
+      rclcpp::Parameter("test.lookahead_dist", 7.0),
+      rclcpp::Parameter("test.max_lookahead_dist", 7.0),
+      rclcpp::Parameter("test.min_lookahead_dist", 6.0),
+      rclcpp::Parameter("test.lookahead_time", 1.8),
+      rclcpp::Parameter("test.rotate_to_heading_angular_vel", 18.0),
+      rclcpp::Parameter("test.max_linear_accel", 0.5),
+      rclcpp::Parameter("test.max_linear_decel", 0.5),
+      rclcpp::Parameter("test.min_approach_linear_velocity", 1.0),
+      rclcpp::Parameter("test.max_allowed_time_to_collision", 2.0),
+      rclcpp::Parameter("test.cost_scaling_dist", 2.0),
+      rclcpp::Parameter("test.cost_scaling_gain", 4.0),
+      rclcpp::Parameter("test.regulated_linear_scaling_min_radius", 10.0),
+      rclcpp::Parameter("test.transform_tolerance", 30.0),
+      rclcpp::Parameter("test.max_angular_accel", 3.0),
+      rclcpp::Parameter("test.rotate_to_heading_min_angle", 0.7),
+      rclcpp::Parameter("test.regulated_linear_scaling_min_speed", 4.0),
+      rclcpp::Parameter("test.use_velocity_scaled_lookahead_dist", false),
+      rclcpp::Parameter("test.use_regulated_linear_velocity_scaling", false),
+      rclcpp::Parameter("test.use_cost_regulated_linear_velocity_scaling", false),
+      rclcpp::Parameter("test.use_approach_linear_velocity_scaling", false),
+      rclcpp::Parameter("test.allow_reversing", false),
+      rclcpp::Parameter("test.use_rotate_to_heading", false)});
+
+  rclcpp::spin_until_future_complete(
+    node->get_node_base_interface(),
+    results);
+
+  EXPECT_EQ(node->get_parameter("test.desired_linear_vel").as_double(), 1.0);
+  EXPECT_EQ(node->get_parameter("test.lookahead_dist").as_double(), 7.0);
+  EXPECT_EQ(node->get_parameter("test.max_lookahead_dist").as_double(), 7.0);
+  EXPECT_EQ(node->get_parameter("test.min_lookahead_dist").as_double(), 6.0);
+  EXPECT_EQ(node->get_parameter("test.lookahead_time").as_double(), 1.8);
+  EXPECT_EQ(node->get_parameter("test.rotate_to_heading_angular_vel").as_double(), 18.0);
+  EXPECT_EQ(node->get_parameter("test.max_linear_accel").as_double(), 0.5);
+  EXPECT_EQ(node->get_parameter("test.max_linear_decel").as_double(), 0.5);
+  EXPECT_EQ(node->get_parameter("test.min_approach_linear_velocity").as_double(), 1.0);
+  EXPECT_EQ(node->get_parameter("test.max_allowed_time_to_collision").as_double(), 2.0);
+  EXPECT_EQ(node->get_parameter("test.cost_scaling_dist").as_double(), 2.0);
+  EXPECT_EQ(node->get_parameter("test.cost_scaling_gain").as_double(), 4.0);
+  EXPECT_EQ(node->get_parameter("test.regulated_linear_scaling_min_radius").as_double(), 10.0);
+  EXPECT_EQ(node->get_parameter("test.transform_tolerance").as_double(), 30.0);
+  EXPECT_EQ(node->get_parameter("test.max_angular_accel").as_double(), 3.0);
+  EXPECT_EQ(node->get_parameter("test.rotate_to_heading_min_angle").as_double(), 0.7);
+  EXPECT_EQ(node->get_parameter("test.regulated_linear_scaling_min_speed").as_double(), 4.0);
+  EXPECT_EQ(node->get_parameter("test.use_velocity_scaled_lookahead_dist").as_bool(), false);
+  EXPECT_EQ(node->get_parameter("test.use_regulated_linear_velocity_scaling").as_bool(), false);
+  EXPECT_EQ(
+    node->get_parameter(
+      "test.use_cost_regulated_linear_velocity_scaling").as_bool(), false);
+  EXPECT_EQ(node->get_parameter("test.use_approach_linear_velocity_scaling").as_bool(), false);
+  EXPECT_EQ(node->get_parameter("test.allow_reversing").as_bool(), false);
+  EXPECT_EQ(node->get_parameter("test.use_rotate_to_heading").as_bool(), false);
+}

nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp

@@ -120,7 +120,7 @@ class SmacPlanner2D : public nav2_core::GlobalPlanner
   rclcpp_lifecycle::LifecycleNode::WeakPtr _node;
 
   // Dynamic parameters handler
-  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler;
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
 };
 
 }  // namespace nav2_smac_planner

nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp

@@ -121,7 +121,7 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner
   rclcpp_lifecycle::LifecycleNode::WeakPtr _node;
 
   // Dynamic parameters handler
-  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler;
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
 };
 
 }  // namespace nav2_smac_planner

nav2_smac_planner/src/smac_planner_2d.cpp

@@ -168,7 +168,7 @@ void SmacPlanner2D::activate()
   }
   auto node = _node.lock();
   // Add callback for dynamic parameters
-  dyn_params_handler = node->add_on_set_parameters_callback(
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
     std::bind(&SmacPlanner2D::dynamicParametersCallback, this, _1));
 }
 
@@ -181,7 +181,7 @@ void SmacPlanner2D::deactivate()
   if (_costmap_downsampler) {
     _costmap_downsampler->on_deactivate();
   }
-  dyn_params_handler.reset();
+  dyn_params_handler_.reset();
 }
 
 void SmacPlanner2D::cleanup()