From c0f3507f0b5e990c2cae8fa007197793a57a892e Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Wed, 7 Sep 2022 14:25:10 +0300 Subject: [PATCH] feat(motion_velocity_smoother): add steering rate limit while planning velocity (#1071) * feat(motion_velocity_smoother): add steering rate limit while planning velocity (#1071) function added, not turning fix the always positive curvature problem added lower velocity limit added vehicle parameters functions created Signed-off-by: Berkay * Update readme update svg readme updated with test params change sample rate calculate accurate dt test Signed-off-by: Berkay fix trajectory size update readme change map loader params Signed-off-by: Berkay clear unnecessary comment Signed-off-by: Berkay change the min and max index Signed-off-by: Berkay ci(pre-commit): autofix removed unnecessary params and comments Signed-off-by: Berkay ci(pre-commit): autofix all velocities in lookup distance is changed Signed-off-by: Berkay ci(pre-commit): autofix works ci(pre-commit): autofix changed calculations with const lookupdistance ci(pre-commit): autofix not work peak points written with constant distances added param ci(pre-commit): autofix update ci(pre-commit): autofix update steering angle calculation method ci(pre-commit): autofix changed curvature calculation of steeringAngleLimit func Signed-off-by: Berkay Karaman changed default parameter values Signed-off-by: Berkay update readme Signed-off-by: Berkay update engage velocity parameter Signed-off-by: Berkay * ci(pre-commit): autofix Signed-off-by: Berkay Co-authored-by: Berkay Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../lateral_controller.param.yaml | 2 +- .../motion_velocity_smoother.param.yaml | 10 +- planning/motion_velocity_smoother/README.md | 14 + .../motion_velocity_smoother_node.hpp | 9 +- .../smoother/smoother_base.hpp | 11 + .../motion_velocity_smoother_flow.drawio.svg | 4846 ++++++++++++++++- planning/motion_velocity_smoother/package.xml | 2 + .../src/motion_velocity_smoother_node.cpp | 34 +- .../src/smoother/smoother_base.cpp | 76 + 9 files changed, 4992 insertions(+), 12 deletions(-) diff --git a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml b/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml index c786daee07d24..76b5da140bfaa 100644 --- a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml +++ b/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml @@ -45,7 +45,7 @@ vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics input_delay: 0.24 # steering input delay time for delay compensation vehicle_model_steer_tau: 0.3 # steering dynamics time constant (1d approximation) [s] - steer_rate_lim_dps: 600.0 # steering angle rate limit [deg/s] + steer_rate_lim_dps: 40.0 # steering angle rate limit [deg/s] acceleration_limit: 2.0 # acceleration limit for trajectory velocity modification [m/ss] velocity_time_constant: 0.3 # velocity dynamics time constant for trajectory velocity modification [s] diff --git a/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml index 016d313e9b646..91bc39b336e58 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml @@ -9,7 +9,7 @@ # curve parameters max_lateral_accel: 0.8 # max lateral acceleration limit [m/ss] - min_curve_velocity: 2.74 # min velocity at lateral acceleration limit [m/ss] + min_curve_velocity: 0.5 # min velocity at lateral acceleration limit and steering angle rate limit [m/s] decel_distance_before_curve: 3.5 # slow speed distance before a curve for lateral acceleration limit decel_distance_after_curve: 2.0 # slow speed distance after a curve for lateral acceleration limit @@ -47,5 +47,11 @@ post_sparse_resample_dt: 0.1 # resample time interval for sparse sampling [s] post_sparse_min_interval_distance: 1.0 # minimum points-interval length for sparse sampling [m] + # steering angle rate limit parameters + 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] + curvature_calculation_distance: 1.0 # distance of points while curvature is calculating [m] + # system - over_stop_velocity_warn_thr: 1.389 # used to check if the optimization exceeds the input velocity on the stop point + over_stop_velocity_warn_thr: 1.389 # used to check if the optimization exceeds the input velocity on the stop point diff --git a/planning/motion_velocity_smoother/README.md b/planning/motion_velocity_smoother/README.md index fab01c175cdbc..5a3a0467fcbaf 100644 --- a/planning/motion_velocity_smoother/README.md +++ b/planning/motion_velocity_smoother/README.md @@ -35,6 +35,10 @@ The velocity limit is set as not to fall under `min_curve_velocity`. Note: velocity limit that requests larger than `nominal.jerk` is not applied. In other words, even if a sharp curve is planned just in front of the ego, no deceleration is performed. +#### Apply steering rate limit + +It calculates the desired steering angles of trajectory points. and it applies the steering rate limit. If the (`steering_angle_rate` > `max_steering_angle_rate`), it decreases the velocity of the trajectory point to acceptable velocity. + #### Resample trajectory It resamples the points on the reference trajectory with designated time interval. @@ -108,6 +112,7 @@ After the optimization, a resampling called `post resampling` is performed befor | `~/debug/trajectory_raw` | `autoware_auto_planning_msgs/Trajectory` | Extracted trajectory (for debug) | | `~/debug/trajectory_external_velocity_limited` | `autoware_auto_planning_msgs/Trajectory` | External velocity limited trajectory (for debug) | | `~/debug/trajectory_lateral_acc_filtered` | `autoware_auto_planning_msgs/Trajectory` | Lateral acceleration limit filtered trajectory (for debug) | +| `~/debug/trajectory_steering_rate_limited` | `autoware_auto_planning_msgs/Trajectory` | Steering angle rate limit filtered trajectory (for debug) | | `~/debug/trajectory_time_resampled` | `autoware_auto_planning_msgs/Trajectory` | Time resampled trajectory (for debug) | | `~/distance_to_stopline` | `std_msgs/Float32` | Distance to stop line from current ego pose (max 50 m) (for debug) | | `~/stop_speed_exceeded` | `std_msgs/Bool` | It publishes `true` if planned velocity on the point which the maximum velocity is zero is over threshold | @@ -189,6 +194,15 @@ After the optimization, a resampling called `post resampling` is performed befor | `post_sparse_dt` | `double` | resample time interval for sparse sampling [s] | 0.1 | | `post_sparse_min_interval_distance` | `double` | minimum points-interval length for sparse sampling [m] | 1.0 | +### 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 | + ### Weights for optimization #### JerkFiltered diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp index dcc5079c9f9fb..ea0ea72a648d2 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp @@ -81,10 +81,10 @@ class MotionVelocitySmootherNode : public rclcpp::Node Odometry::ConstSharedPtr current_odometry_ptr_; // current odometry Trajectory::ConstSharedPtr base_traj_raw_ptr_; // current base_waypoints double external_velocity_limit_; // current external_velocity_limit - - // maximum velocity with deceleration for external velocity limit - double max_velocity_with_deceleration_; - double external_velocity_limit_dist_{0.0}; // distance to set external velocity limit + double max_velocity_with_deceleration_; // maximum velocity with deceleration + // for external velocity limit + double external_velocity_limit_dist_{0.0}; // distance to set external velocity limit + double wheelbase_; // wheelbase TrajectoryPoints prev_output_; // previously published trajectory @@ -213,6 +213,7 @@ class MotionVelocitySmootherNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_velocity_limit_; rclcpp::Publisher::SharedPtr pub_trajectory_vel_lim_; rclcpp::Publisher::SharedPtr pub_trajectory_latacc_filtered_; + rclcpp::Publisher::SharedPtr pub_trajectory_steering_rate_limited_; rclcpp::Publisher::SharedPtr pub_trajectory_resampled_; rclcpp::Publisher::SharedPtr debug_closest_velocity_; rclcpp::Publisher::SharedPtr debug_closest_acc_; diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp index 0ec6330394a56..38c11bcf1c2e9 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp @@ -20,6 +20,7 @@ #include "motion_velocity_smoother/trajectory_utils.hpp" #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" @@ -32,6 +33,7 @@ namespace motion_velocity_smoother { using autoware_auto_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; +using vehicle_info_util::VehicleInfoUtil; class SmootherBase { @@ -47,6 +49,13 @@ class SmootherBase double min_curve_velocity; // min velocity at curve [m/s] double decel_distance_before_curve; // distance before slow down for lateral acc at a curve double decel_distance_after_curve; // distance after slow down for lateral acc at a curve + double max_steering_angle_rate; // max steering angle rate [degree/s] + double wheel_base; // wheel base [m] + double sample_ds; // distance between trajectory points [m] + double curvature_threshold; // look-up distance of Trajectory point for calculation of steering + // angle limit [m] + double curvature_calculation_distance; // threshold steering degree limit to trigger + // steeringRateLimit [degree] resampling::ResampleParam resample_param; }; @@ -65,6 +74,8 @@ class SmootherBase [[maybe_unused]] const double a0 = 0.0, [[maybe_unused]] const bool enable_smooth_limit = false) const; + boost::optional applySteeringRateLimit(const TrajectoryPoints & input) const; + double getMaxAccel() const; double getMinDecel() const; double getMaxJerk() const; diff --git a/planning/motion_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg b/planning/motion_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg index 4d8ffd9eb8ae2..e44399fdd7eb9 100644 --- a/planning/motion_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg +++ b/planning/motion_velocity_smoother/media/motion_velocity_smoother_flow.drawio.svg @@ -1,3 +1,4843 @@ - - -
Extract trajectory
Extract trajectory
Apply external velocity limit
Apply external velocity limit
Apply lateral acceleration limit
Apply lateral acceleration limit
Resample trajectory
Resample trajectory
Calculate initial state
Calculate initial state
Smooth velocity
Smooth velocity
Postprocess
PostprocessReference trajectoryOutput trajectory
onCurrentTrajectory
onCurrentTrajectory
Calculate travel distance
Calculate travel distance
Apply stop approaching velocity
Apply stop approaching velocity
extract_behind_dist
extract_behind_dist
extract_ahead_dist
extract_ahead_dist
Extract trajectory
Extract trajecto...
Apply external velocity limit
Apply external velocity li...
default velocity limit
default velocity lim...
external velocity limit
external velocity li...
velocity
velocity
distance
distance
applied velocity limit
applied velocity lim...
distance required for deceleration under jerk constraints
distance required for decelera...
current pose
current p...
Apply stopping velocity limit
Apply stopping velocity li...
stopping velocity
stopping velocity
velocity
velocity
applied velocity limit
applied velocity lim...
stopping
distance
stopping...
current pose
current p...
velocity limit with external velocity limit
velocity limit with external veloci...
Apply lateral acceleration limit
Apply lateral acceleration lim...
velocity
velocity
applied velocity limit
applied velocity lim...
current pose
current p...
velocity limit with stopping velocity
velocity limit with stopping veloci...
ego car
ego car
distance
distance
distance
distance
curvature
curvature
distance
distance
Resample trajectory
Resample trajectory
trajectory
trajectory
velocity
velocity
current pose
current p...
distance
distance
resample time * current velocity
resample time * current veloc...
dense sampling
dense sampling
sparse sampling
sparse sampling
Smooth velocity
Smooth velocity
velocity
velocity
smoothed velocity
smoothed velocity
velocity limit
velocity limit
distance
distanceViewer does not support full SVG 1.1 \ No newline at end of file + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Extract trajectory + + + + Extract trajectory + + + + + + + + + + + Apply external velocity limit + + + + Apply external velocity limit + + + + + + + + + + + Apply lateral acceleration limit + + + + Apply lateral acceleration limit + + + + + + + + + + + Resample trajectory + + + + Resample trajectory + + + + + + + + + + + Calculate initial state + + + + Calculate initial state + + + + + + + + + + + Smooth velocity + + + + Smooth velocity + + + + + + + + + + + Postprocess + + + + Postprocess + + + + + Reference trajectory + + + + + + Output trajectory + + + + + + + + onCurrentTrajectory + + + + onCurrentTrajectory + + + + + + + + + + + Calculate travel distance + + + + Calculate travel distance + + + + + + + Apply steering rate limit + + + + + + + + + + Apply stop approaching velocity + + + + Apply stop approaching velocity + + + + + + + + + + + + + + + + + + + extract_behind_dist + + + + extract_behind_dist + + + + + + + + + extract_ahead_dist + + + + extract_ahead_dist + + + + + + + + + + Extract trajectory + + + + Extract trajecto... + + + + + + + + + Apply external velocity limit + + + + Apply external velocity li... + + + + + + + + + + + + + default velocity limit + + + + default velocity lim... + + + + + + + + + external velocity limit + + + + external velocity li... + + + + + + + + + + + + + velocity + + + + velocity + + + + + + + + + distance + + + + distance + + + + + + + + + + + + applied velocity limit + + + + applied velocity lim... + + + + + + + + + + + distance required for deceleration under jerk constraints + + + + distance required for decelera... + + + + + + + + + current pose + + + + current p... + + + + + + + + + Apply stopping velocity limit + + + + Apply stopping velocity li... + + + + + + + + + + + + + + + + + + stopping velocity + + + + stopping velocity + + + + + + + + + + + + + velocity + + + + velocity + + + + + + + + + + + + applied velocity limit + + + + applied velocity lim... + + + + + + + + + + stopping +distance + + + + stopping... + + + + + + + + + current pose + + + + current p... + + + + + + + + + + + + + velocity limit with external velocity limit + + + + velocity limit with external veloci... + + + + + + + + + + + + + + + + + Apply lateral acceleration limit + + + + Apply lateral acceleration lim... + + + + + + + + + + + + + + velocity + + + + velocity + + + + + + + + + + + applied velocity limit + + + + applied velocity lim... + + + + + + + + + current pose + + + + current p... + + + + + + + + + velocity limit with stopping velocity + + + + velocity limit with stopping veloci... + + + + + + + + + + + + + + ego car + + + + ego car + + + + + + + + + + + + + + + + + + distance + + + + distance + + + + + + + + + distance + + + + distance + + + + + + + + + curvature + + + + curvature + + + + + + + + + distance + + + + distance + + + + + + + + + + + trajectory + + + + trajectory + + + + + + + + + + + Resample trajectory + + + + Resample trajectory + + + + + + + + + + + + + + velocity + + + + velocity + + + + + + + + + current pose + + + + current p... + + + + + + + + + distance + + + + distance + + + + + + + + + + + + + + + + + + + + + + + + + + resample time * current velocity + + + + resample time * current veloc... + + + + + + + + + + + dense sampling + + + + dense sampling + + + + + + + + + sparse sampling + + + + sparse sampling + + + + + + + + + + + + + + + Smooth velocity + + + + Smooth velocity + + + + + + + + + + + + + + + + + + + + + + + Apply steering rate limit + + + + Apply steering rate limit + + + + + + + + + + + + + + velocity + + + + velocity + + + + + + + + + + + applied velocity limit + + + + applied velocity lim... + + + + + + + + + current pose + + + + current p... + + + + + + + + + velocity limit with lateral acceleration limit + + + + velocity with lateral acceleration limit + + + + + + + + + + + + + + ego car + + + + ego car + + + + + + + + + + + + + + + + + + distance + + + + distance + + + + + + + + + curvature + + + + curvature + + + + + + + + + distance + + + + distance + + + + + + + + + + + trajectory + + + + trajectory + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + smoothed velocity + + + + smoothed velocity + + + + + + + + + velocity limit + + + + velocity limit + + + + + + Viewer does not support full SVG 1.1 + + + + + + + + + + + + + + + + + + + + + + + + + velocity + + + + velocity + + + + + + + + + + current pose + + + + current p... + + + + + + + + + distance + + + + distance + + + diff --git a/planning/motion_velocity_smoother/package.xml b/planning/motion_velocity_smoother/package.xml index 620db1717a454..98bbb2512217e 100644 --- a/planning/motion_velocity_smoother/package.xml +++ b/planning/motion_velocity_smoother/package.xml @@ -22,6 +22,7 @@ geometry_msgs interpolation libboost-dev + motion_common motion_utils nav_msgs osqp_interface @@ -31,6 +32,7 @@ tier4_autoware_utils tier4_debug_msgs tier4_planning_msgs + vehicle_info_util ament_cmake_ros ament_lint_auto diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index ba0018e26ce77..f2c05792e839e 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -19,6 +19,8 @@ #include "motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" +#include + #include #include #include @@ -37,6 +39,8 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions using std::placeholders::_1; // set common params + const auto vehicle_info = VehicleInfoUtil(*this).getVehicleInfo(); + wheelbase_ = vehicle_info.wheel_base_m; initCommonParam(); over_stop_velocity_warn_thr_ = declare_parameter("over_stop_velocity_warn_thr", tier4_autoware_utils::kmph2mps(5.0)); @@ -72,6 +76,10 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions default: throw std::domain_error("[MotionVelocitySmootherNode] invalid algorithm"); } + // Initialize the wheelbase + auto p = smoother_->getBaseParam(); + p.wheel_base = wheelbase_; + smoother_->setParam(p); // publishers, subscribers pub_trajectory_ = create_publisher("~/output/trajectory", 1); @@ -93,7 +101,7 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions std::bind(&MotionVelocitySmootherNode::onParameter, this, _1)); // debug - publish_debug_trajs_ = declare_parameter("publish_debug_trajs", false); + publish_debug_trajs_ = declare_parameter("publish_debug_trajs", true); debug_closest_velocity_ = create_publisher("~/closest_velocity", 1); debug_closest_acc_ = create_publisher("~/closest_acceleration", 1); debug_closest_jerk_ = create_publisher("~/closest_jerk", 1); @@ -104,6 +112,8 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions create_publisher("~/debug/trajectory_external_velocity_limited", 1); pub_trajectory_latacc_filtered_ = create_publisher("~/debug/trajectory_lateral_acc_filtered", 1); + pub_trajectory_steering_rate_limited_ = + create_publisher("~/debug/trajectory_steering_rate_limited", 1); pub_trajectory_resampled_ = create_publisher("~/debug/trajectory_time_resampled", 1); // Wait for first self pose @@ -170,6 +180,10 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter update_param("min_interval_distance", p.resample_param.dense_min_interval_distance); update_param("sparse_resample_dt", p.resample_param.sparse_resample_dt); update_param("sparse_min_interval_distance", p.resample_param.sparse_min_interval_distance); + update_param("resample_ds", p.sample_ds); + update_param("curvature_threshold", p.curvature_threshold); + update_param("max_steering_angle_rate", p.max_steering_angle_rate); + update_param("curvature_calculation_distance", p.curvature_calculation_distance); smoother_->setParam(p); } @@ -486,12 +500,23 @@ bool MotionVelocitySmootherNode::smoothVelocity( const auto traj_lateral_acc_filtered = smoother_->applyLateralAccelerationFilter(input, initial_motion.vel, initial_motion.acc, true); if (!traj_lateral_acc_filtered) { + RCLCPP_ERROR(get_logger(), "Fail to do traj_lateral_acc_filtered"); + + return false; + } + + // Steering angle rate limit + const auto traj_steering_rate_limited = + smoother_->applySteeringRateLimit(*traj_lateral_acc_filtered); + if (!traj_steering_rate_limited) { + RCLCPP_ERROR(get_logger(), "Fail to do traj_steering_rate_limited"); + return false; } // Resample trajectory with ego-velocity based interval distance auto traj_resampled = smoother_->resampleTrajectory( - *traj_lateral_acc_filtered, current_odometry_ptr_->twist.twist.linear.x, + *traj_steering_rate_limited, current_odometry_ptr_->twist.twist.linear.x, current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold); if (!traj_resampled) { @@ -550,6 +575,11 @@ bool MotionVelocitySmootherNode::smoothVelocity( if (is_reverse_) flipVelocity(tmp); pub_trajectory_resampled_->publish(toTrajectoryMsg(tmp)); } + { + auto tmp = *traj_steering_rate_limited; + if (is_reverse_) flipVelocity(tmp); + pub_trajectory_steering_rate_limited_->publish(toTrajectoryMsg(tmp)); + } if (!debug_trajectories.empty()) { for (auto & debug_trajectory : debug_trajectories) { diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp index fa357948e24af..a002163a69bda 100644 --- a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp @@ -14,10 +14,12 @@ #include "motion_velocity_smoother/smoother/smoother_base.hpp" +#include "motion_common/motion_common.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/tmp_conversion.hpp" #include "motion_velocity_smoother/resample.hpp" #include "motion_velocity_smoother/trajectory_utils.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" #include #include @@ -25,6 +27,8 @@ namespace motion_velocity_smoother { +using vehicle_info_util::VehicleInfoUtil; + SmootherBase::SmootherBase(rclcpp::Node & node) { auto & p = base_param_; @@ -34,6 +38,10 @@ SmootherBase::SmootherBase(rclcpp::Node & node) p.max_jerk = node.declare_parameter("normal.max_jerk", 0.3); p.min_jerk = node.declare_parameter("normal.min_jerk", -0.1); p.max_lateral_accel = node.declare_parameter("max_lateral_accel", 0.2); + p.sample_ds = node.declare_parameter("resample_ds", 0.5); + p.curvature_threshold = node.declare_parameter("curvature_threshold", 0.2); + p.max_steering_angle_rate = node.declare_parameter("max_steering_angle_rate", 5.0); + p.curvature_calculation_distance = node.declare_parameter("curvature_calculation_distance", 1.0); p.decel_distance_before_curve = node.declare_parameter("decel_distance_before_curve", 3.5); p.decel_distance_after_curve = node.declare_parameter("decel_distance_after_curve", 0.0); p.min_curve_velocity = node.declare_parameter("min_curve_velocity", 1.38); @@ -114,6 +122,7 @@ boost::optional SmootherBase::applyLateralAccelerationFilter( } double v_curvature_max = std::sqrt(max_lateral_accel_abs / std::max(curvature, 1.0E-5)); v_curvature_max = std::max(v_curvature_max, base_param_.min_curve_velocity); + if (enable_smooth_limit) { if (i >= latacc_min_vel_arr.size()) return output; v_curvature_max = std::max(v_curvature_max, latacc_min_vel_arr.at(i)); @@ -125,4 +134,71 @@ boost::optional SmootherBase::applyLateralAccelerationFilter( return output; } +boost::optional SmootherBase::applySteeringRateLimit( + const TrajectoryPoints & input) const +{ + if (input.empty()) { + return boost::none; + } + + if (input.size() < 3) { + return boost::optional( + input); // cannot calculate the desired velocity. do nothing. + } + // Interpolate with constant interval distance for lateral acceleration calculation. + std::vector out_arclength; + const auto traj_length = motion_utils::calcArcLength(input); + for (double s = 0; s < traj_length; s += base_param_.sample_ds) { + out_arclength.push_back(s); + } + const auto output_traj = + motion_utils::resampleTrajectory(motion_utils::convertToTrajectory(input), out_arclength); + auto output = motion_utils::convertToTrajectoryPointArray(output_traj); + output.back() = input.back(); // keep the final speed. + + const size_t idx_dist = static_cast(std::max( + static_cast((base_param_.curvature_calculation_distance) / base_param_.sample_ds), 1)); + + // Calculate curvature assuming the trajectory points interval is constant + const auto curvature_v = trajectory_utils::calcTrajectoryCurvatureFrom3Points(output, idx_dist); + + for (size_t i = 0; i + 1 < output.size(); i++) { + if (fabs(curvature_v.at(i)) > base_param_.curvature_threshold) { + // calculate the just 2 steering angle + output.at(i).front_wheel_angle_rad = std::atan(base_param_.wheel_base * curvature_v.at(i)); + output.at(i + 1).front_wheel_angle_rad = + std::atan(base_param_.wheel_base * curvature_v.at(i + 1)); + + const double mean_vel = + (output.at(i).longitudinal_velocity_mps + output.at(i + 1).longitudinal_velocity_mps) / 2.0; + const double dt = + std::max(base_param_.sample_ds / mean_vel, std::numeric_limits::epsilon()); + const double steering_diff = + fabs(output.at(i).front_wheel_angle_rad - output.at(i + 1).front_wheel_angle_rad); + const double dt_steering = + steering_diff / tier4_autoware_utils::deg2rad(base_param_.max_steering_angle_rate); + + if (dt_steering > dt) { + const double target_mean_vel = (base_param_.sample_ds / dt_steering); + for (size_t k = 0; k < 2; k++) { + const double temp_vel = + output.at(i + k).longitudinal_velocity_mps * (target_mean_vel / mean_vel); + if (temp_vel < output.at(i + k).longitudinal_velocity_mps) { + output.at(i + k).longitudinal_velocity_mps = temp_vel; + } else { + if (target_mean_vel < output.at(i + k).longitudinal_velocity_mps) { + output.at(i + k).longitudinal_velocity_mps = target_mean_vel; + } + } + if (output.at(i + k).longitudinal_velocity_mps < base_param_.min_curve_velocity) { + output.at(i + k).longitudinal_velocity_mps = base_param_.min_curve_velocity; + } + } + } + } + } + + return output; +} + } // namespace motion_velocity_smoother