Skip to content

removing kinematic limiting from RPP (backport #2631) #2635

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

Closed
mergify wants to merge 1 commit into from
Closed

removing kinematic limiting from RPP (backport #2631) #2635

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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 @@ -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_;
Expand Down
105 changes: 92 additions & 13 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 @@ -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(
Expand Down Expand Up @@ -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_);
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -668,6 +657,96 @@ bool RegulatedPurePursuitController::transformPose(
return false;
}

<<<<<<< HEAD
=======

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_ + ".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
Expand Down
71 changes: 71 additions & 0 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 @@ -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<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.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))