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

chore(motion_velocity_smoother): add enable curve filtering param #5064

Merged
merged 2 commits into from
Sep 25, 2023
Merged

chore(motion_velocity_smoother): add enable curve filtering param #5064

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
07d0997
chore(motion_velocity_smoother): add enable curve filtering param
TakaHoribe Sep 21, 2023
15a4ec1
update readme
TakaHoribe Sep 21, 2023
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
14 changes: 8 additions & 6 deletions planning/motion_velocity_smoother/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -140,6 +140,7 @@ After the optimization, a resampling called `post resampling` is performed befor

| Name | Type | Description | Default value |
| :------------------------------------- | :------- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
| `enable_lateral_acc_limit` | `bool` | To toggle the lateral acceleration filter on and off. You can switch it dynamically at runtime. | true |
| `max_lateral_accel` | `double` | Max lateral acceleration limit [m/ss] | 0.5 |
| `min_curve_velocity` | `double` | Min velocity at lateral acceleration limit [m/ss] | 2.74 |
| `decel_distance_before_curve` | `double` | Distance to slowdown before a curve for lateral acceleration limit [m] | 3.5 |
Expand Down Expand Up @@ -197,12 +198,13 @@ After the optimization, a resampling called `post resampling` is performed befor

### Limit steering angle rate parameters

| Name | Type | Description | Default value |
| :------------------------------- | :------- | :----------------------------------------------------------------------- | :------------ |
| `max_steering_angle_rate` | `double` | Maximum steering angle rate [degree/s] | 40.0 |
| `resample_ds` | `double` | Distance between trajectory points [m] | 0.1 |
| `curvature_threshold` | `double` | If curvature > curvature_threshold, steeringRateLimit is triggered [1/m] | 0.02 |
| `curvature_calculation_distance` | `double` | Distance of points while curvature is calculating [m] | 1.0 |
| Name | Type | Description | Default value |
| :------------------------------- | :------- | :------------------------------------------------------------------------------------ | :------------ |
| `enable_steering_rate_limit` | `bool` | To toggle the steer rate filter on and off. You can switch it dynamically at runtime. | true |
| `max_steering_angle_rate` | `double` | Maximum steering angle rate [degree/s] | 40.0 |
| `resample_ds` | `double` | Distance between trajectory points [m] | 0.1 |
| `curvature_threshold` | `double` | If curvature > curvature_threshold, steeringRateLimit is triggered [1/m] | 0.02 |
| `curvature_calculation_distance` | `double` | Distance of points while curvature is calculating [m] | 1.0 |

### Weights for optimization

Expand Down
2 changes: 2 additions & 0 deletions planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
margin_to_insert_external_velocity_limit: 0.3 # margin distance to insert external velocity limit [m]

# curve parameters
enable_lateral_acc_limit: true # To toggle the lateral acc filter on and off. You can switch it dynamically at runtime.
max_lateral_accel: 0.8 # max lateral acceleration limit [m/ss]
min_curve_velocity: 2.74 # min velocity at lateral acceleration limit [m/ss]
decel_distance_before_curve: 3.5 # slow speed distance before a curve for lateral acceleration limit
Expand Down Expand Up @@ -49,6 +50,7 @@
post_sparse_min_interval_distance: 1.0 # minimum points-interval length for sparse sampling [m]

# steering angle rate limit parameters
enable_steering_rate_limit: true # To toggle the steer rate filter on and off. You can switch it dynamically at runtime.
max_steering_angle_rate: 40.0 # maximum steering angle rate [degree/s]
resample_ds: 0.1 # distance between trajectory points [m]
curvature_threshold: 0.02 # if curvature > curvature_threshold, steeringRateLimit is triggered [1/m]
Expand Down
3 changes: 3 additions & 0 deletions ...tion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -123,6 +123,9 @@ class MotionVelocitySmootherNode : public rclcpp::Node

struct Param
{
bool enable_lateral_acc_limit;
bool enable_steering_rate_limit;

double max_velocity; // max velocity [m/s]
double margin_to_insert_external_velocity_limit; // for external velocity limit [m]
double replan_vel_deviation; // if speed error exceeds this [m/s],
Expand Down
17 changes: 14 additions & 3 deletions planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -165,6 +165,9 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter

{
auto & p = node_param_;
update_param_bool("enable_lateral_acc_limit", p.enable_lateral_acc_limit);
update_param_bool("enable_steering_rate_limit", p.enable_steering_rate_limit);

update_param("max_velocity", p.max_velocity);
update_param(
"margin_to_insert_external_velocity_limit", p.margin_to_insert_external_velocity_limit);
Expand Down Expand Up @@ -266,6 +269,9 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter
void MotionVelocitySmootherNode::initCommonParam()
{
auto & p = node_param_;
p.enable_lateral_acc_limit = declare_parameter<bool>("enable_lateral_acc_limit");
p.enable_steering_rate_limit = declare_parameter<bool>("enable_steering_rate_limit");

p.max_velocity = declare_parameter<double>("max_velocity"); // 72.0 kmph
p.margin_to_insert_external_velocity_limit =
declare_parameter<double>("margin_to_insert_external_velocity_limit");
Expand Down Expand Up @@ -572,12 +578,17 @@ bool MotionVelocitySmootherNode::smoothVelocity(
// Lateral acceleration limit
constexpr bool enable_smooth_limit = true;
constexpr bool use_resampling = true;
const auto traj_lateral_acc_filtered = smoother_->applyLateralAccelerationFilter(
input, initial_motion.vel, initial_motion.acc, enable_smooth_limit, use_resampling);
const auto traj_lateral_acc_filtered =
node_param_.enable_lateral_acc_limit
? smoother_->applyLateralAccelerationFilter(
input, initial_motion.vel, initial_motion.acc, enable_smooth_limit, use_resampling)
: input;

// Steering angle rate limit (Note: set use_resample = false since it is resampled above)
const auto traj_steering_rate_limited =
smoother_->applySteeringRateLimit(traj_lateral_acc_filtered, false);
node_param_.enable_steering_rate_limit
? smoother_->applySteeringRateLimit(traj_lateral_acc_filtered, false)
: traj_lateral_acc_filtered;

// Resample trajectory with ego-velocity based interval distance
auto traj_resampled = smoother_->resampleTrajectory(
Expand Down