diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index d09ff2b4bfd..690462d962d 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -240,8 +240,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller double max_lookahead_dist_; double min_lookahead_dist_; double lookahead_time_; - double max_linear_accel_; - double max_linear_decel_; bool use_velocity_scaled_lookahead_dist_; tf2::Duration transform_tolerance_; bool use_approach_vel_scaling_; diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 6cda1b6faf7..4e804046e33 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -59,10 +59,6 @@ void RegulatedPurePursuitController::configure( declare_parameter_if_not_declared( node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".max_linear_accel", rclcpp::ParameterValue(2.5)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".max_linear_decel", rclcpp::ParameterValue(2.5)); declare_parameter_if_not_declared( node, plugin_name_ + ".lookahead_dist", rclcpp::ParameterValue(0.6)); declare_parameter_if_not_declared( @@ -110,8 +106,6 @@ void RegulatedPurePursuitController::configure( node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_); base_desired_linear_vel_ = desired_linear_vel_; - node->get_parameter(plugin_name_ + ".max_linear_accel", max_linear_accel_); - node->get_parameter(plugin_name_ + ".max_linear_decel", max_linear_decel_); node->get_parameter(plugin_name_ + ".lookahead_dist", lookahead_dist_); node->get_parameter(plugin_name_ + ".min_lookahead_dist", min_lookahead_dist_); node->get_parameter(plugin_name_ + ".max_lookahead_dist", max_lookahead_dist_); @@ -476,7 +470,7 @@ double RegulatedPurePursuitController::costAtPose(const double & x, const double void RegulatedPurePursuitController::applyConstraints( const double & dist_error, const double & lookahead_dist, - const double & curvature, const geometry_msgs::msg::Twist & curr_speed, + const double & curvature, const geometry_msgs::msg::Twist & /*curr_speed*/, const double & pose_cost, double & linear_vel, double & sign) { double curvature_vel = linear_vel; @@ -523,12 +517,7 @@ void RegulatedPurePursuitController::applyConstraints( linear_vel = std::min(linear_vel, approach_vel); } - // Limit linear velocities to be valid and kinematically feasible, v = v0 + a * dt - linear_vel = sign * linear_vel; - double & dt = control_duration_; - const double max_feasible_linear_speed = curr_speed.linear.x + max_linear_accel_ * dt; - const double min_feasible_linear_speed = curr_speed.linear.x - max_linear_decel_ * dt; - linear_vel = std::clamp(linear_vel, min_feasible_linear_speed, max_feasible_linear_speed); + // Limit linear velocities to be valid linear_vel = std::clamp(fabs(linear_vel), 0.0, desired_linear_vel_); linear_vel = sign * linear_vel; } @@ -668,6 +657,96 @@ bool RegulatedPurePursuitController::transformPose( return false; } +<<<<<<< HEAD +======= + +rcl_interfaces::msg::SetParametersResult +RegulatedPurePursuitController::dynamicParametersCallback( + std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + std::lock_guard 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_ + ".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; +} + +>>>>>>> c65532f7 (removing kinematic limiting from RPP (#2631)) } // namespace nav2_regulated_pure_pursuit_controller // Register this controller as a nav2_core plugin diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 0371f496a90..c8237097f26 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -382,3 +382,74 @@ TEST(RegulatedPurePursuitTest, applyConstraints) // dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); // EXPECT_NEAR(linear_vel, 0.5, 0.01); } +<<<<<<< HEAD +======= + +TEST(RegulatedPurePursuitTest, testDynamicParameter) +{ + auto node = std::make_shared("Smactest"); + auto costmap = std::make_shared("global_costmap"); + costmap->on_configure(rclcpp_lifecycle::State()); + auto ctrl = + std::make_unique(); + auto tf = std::make_shared(node->get_clock()); + ctrl->configure(node, "test", tf, costmap); + ctrl->activate(); + + auto rec_param = std::make_shared( + 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.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.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); +} +>>>>>>> c65532f7 (removing kinematic limiting from RPP (#2631))