Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

removing kinematic limiting from RPP #2631

Merged
merged 2 commits into from
Oct 20, 2021
Merged

removing kinematic limiting from RPP #2631

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
70be8b0
removing kinematic limiting from RPP
SteveMacenski Oct 20, 2021
e842aff
update tests
SteveMacenski Oct 20, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 0 additions & 2 deletions ...ller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -249,8 +249,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_;
Expand Down
19 changes: 2 additions & 17 deletions nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,10 +62,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(
Expand Down Expand Up @@ -113,8 +109,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_);
Expand Down Expand Up @@ -488,7 +482,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;
Expand Down Expand Up @@ -535,12 +529,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;
}
Expand Down Expand Up @@ -714,10 +703,6 @@ RegulatedPurePursuitController::dynamicParametersCallback(
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") {
Expand Down
4 changes: 0 additions & 4 deletions nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -406,8 +406,6 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter)
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),
Expand All @@ -434,8 +432,6 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter)
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);
Expand Down