From 07e93249822b6323a2249ee32fb83f807d3511bf Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 22 Oct 2021 12:08:58 -0700 Subject: [PATCH] backporting kinematic limitation removal to foxy (#2631) --- .../regulated_pure_pursuit_controller.hpp | 2 -- .../src/regulated_pure_pursuit_controller.cpp | 12 +----------- 2 files changed, 1 insertion(+), 13 deletions(-) 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 eb2026a429..7cb3fafa97 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 @@ -220,8 +220,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 bf9f784688..f71b519751 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 @@ -76,10 +76,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( @@ -125,8 +121,6 @@ void RegulatedPurePursuitController::configure( node, plugin_name_ + ".goal_dist_tol", rclcpp::ParameterValue(0.25)); node->get_parameter(plugin_name_ + ".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_); @@ -416,7 +410,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 curvature_vel = linear_vel; @@ -464,10 +458,6 @@ void RegulatedPurePursuitController::applyConstraints( } // Limit linear velocities to be valid and kinematically feasible, v = v0 + a * dt - 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 = clamp(linear_vel, min_feasible_linear_speed, max_feasible_linear_speed); linear_vel = clamp(linear_vel, 0.0, desired_linear_vel_); }