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 9a2212c822583..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,9 +45,8 @@ 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_lim_deg: 40.0 # steering angle limit [deg] - steer_rate_lim_dps: 5.0 # steering angle rate limit [deg/s] - acceleration_limit: 0.9 # acceleration limit for trajectory velocity modification [m/ss] + 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] # -- lowpass filter for noise reduction -- diff --git a/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml b/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml index bdcae92e9ea0a..b6e1c3a38c799 100644 --- a/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml +++ b/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml @@ -60,12 +60,12 @@ emergency_jerk: -3.0 # acceleration limit - max_acc: 0.9 - min_acc: -0.9 + max_acc: 3.0 + min_acc: -5.0 # jerk limit - max_jerk: 0.6 - min_jerk: -0.6 + max_jerk: 2.0 + min_jerk: -5.0 # pitch use_trajectory_for_pitch_calculation: false diff --git a/launch/tier4_planning_launch/config/scenario_planning/common/common.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/common/common.param.yaml index d7041fda955ad..a23570a5fc791 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/common/common.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/common/common.param.yaml @@ -3,13 +3,13 @@ # constraints param for normal driving normal: min_acc: -0.5 # min deceleration [m/ss] - max_acc: 0.5 # max acceleration [m/ss] - min_jerk: -0.4 # min jerk [m/sss] - max_jerk: 0.4 # max jerk [m/sss] + max_acc: 1.0 # max acceleration [m/ss] + min_jerk: -0.5 # min jerk [m/sss] + max_jerk: 1.0 # max jerk [m/sss] # constraints to be observed limit: - min_acc: -0.9 # min deceleration limit [m/ss] - max_acc: 0.9 # max acceleration limit [m/ss] - min_jerk: -0.6 # min jerk limit [m/sss] - max_jerk: 0.6 # max jerk limit [m/sss] + min_acc: -2.5 # min deceleration limit [m/ss] + max_acc: 1.0 # max acceleration limit [m/ss] + min_jerk: -1.5 # min jerk limit [m/sss] + max_jerk: 1.5 # max jerk limit [m/sss] 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 dc0d2b50db076..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 @@ -8,8 +8,8 @@ margin_to_insert_external_velocity_limit: 0.3 # margin distance to insert external velocity limit [m] # curve parameters - max_lateral_accel: 0.3 # max lateral acceleration limit [m/ss] - min_curve_velocity: 0.5 # min velocity at lateral acceleration limit [m/ss] + max_lateral_accel: 0.8 # max 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,8 +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] - # vehicle model parameters - max_steering_angle_rate: 0.0872665 # steering angle rate [rad/s] + # 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 diff --git a/map/map_loader/config/lanelet2_map_loader.param.yaml b/map/map_loader/config/lanelet2_map_loader.param.yaml index f7eaf3571ab9c..55f810b0e9ff9 100755 --- a/map/map_loader/config/lanelet2_map_loader.param.yaml +++ b/map/map_loader/config/lanelet2_map_loader.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: - lanelet2_map_projector_type: UTM # Options: MGRS, UTM - latitude: 40.81187906 # Latitude of map_origin, using in UTM - longitude: 29.35810110 # Longitude of map_origin, using in UTM + lanelet2_map_projector_type: MGRS # Options: MGRS, UTM + latitude: 40.816617984672746 # Latitude of map_origin, using in UTM + longitude: 29.360491808334285 # Longitude of map_origin, using in UTM center_line_resolution: 5.0 # [m] diff --git a/planning/motion_velocity_smoother/README.md b/planning/motion_velocity_smoother/README.md index fab01c175cdbc..947dda3212046 100644 --- a/planning/motion_velocity_smoother/README.md +++ b/planning/motion_velocity_smoother/README.md @@ -34,6 +34,10 @@ It calculates the velocity limit from the curvature of the reference trajectory 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 @@ -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,16 @@ 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 5b987c01e4667..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 @@ -86,10 +86,6 @@ class MotionVelocitySmootherNode : public rclcpp::Node double external_velocity_limit_dist_{0.0}; // distance to set external velocity limit double wheelbase_; // wheelbase - // 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 - TrajectoryPoints prev_output_; // previously published trajectory // previous trajectory point closest to ego vehicle 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 31df51fe05c2b..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,9 +20,9 @@ #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" -#include "vehicle_info_util/vehicle_info_util.hpp" #include "boost/optional.hpp" @@ -49,9 +49,14 @@ 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 max_steering_angle_rate; // max steering angle rate [degree/s] double wheel_base; // wheel base [m] - resampling::ResampleParam resample_param; + 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; }; explicit SmootherBase(rclcpp::Node & node); @@ -69,9 +74,7 @@ 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; - + boost::optional applySteeringRateLimit(const TrajectoryPoints & input) const; double getMaxAccel() const; double getMinDecel() const; @@ -83,10 +86,6 @@ class SmootherBase protected: BaseParam base_param_; - const double points_interval = 0.1; // constant interval distance of trajectory - // for lateral acceleration calculation and - // limit steering rate. [m] - }; } // namespace motion_velocity_smoother 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 2550abdd03005..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 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 6ef0c1fa852b0..f2c05792e839e 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -18,6 +18,7 @@ #include "motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" #include "motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" + #include #include @@ -75,12 +76,12 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions default: throw std::domain_error("[MotionVelocitySmootherNode] invalid algorithm"); } - //Initialize the wheelbase + // Initialize the wheelbase auto p = smoother_->getBaseParam(); p.wheel_base = wheelbase_; smoother_->setParam(p); - // publishers, subscribers + // publishers, subscribers pub_trajectory_ = create_publisher("~/output/trajectory", 1); pub_velocity_limit_ = create_publisher( "~/output/current_velocity_limit_mps", rclcpp::QoS{1}.transient_local()); @@ -179,7 +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); } @@ -496,24 +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_WARN(get_logger(), "Fail to do 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); + const auto traj_steering_rate_limited = + smoother_->applySteeringRateLimit(*traj_lateral_acc_filtered); if (!traj_steering_rate_limited) { - RCLCPP_WARN(get_logger(), "Fail to do traj_steering_rate_limited"); + RCLCPP_ERROR(get_logger(), "Fail to do traj_steering_rate_limited"); return false; } - RCLCPP_WARN(get_logger(), "AAAAAAAAA CREATED"); - // 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) { @@ -573,9 +576,9 @@ bool MotionVelocitySmootherNode::smoothVelocity( pub_trajectory_resampled_->publish(toTrajectoryMsg(tmp)); } { - auto tmp = *traj_resampled; + auto tmp = *traj_steering_rate_limited; if (is_reverse_) flipVelocity(tmp); - pub_trajectory_resampled_->publish(toTrajectoryMsg(tmp)); + pub_trajectory_steering_rate_limited_->publish(toTrajectoryMsg(tmp)); } if (!debug_trajectories.empty()) { diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp index bccb24cd69727..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 @@ -27,7 +29,6 @@ namespace motion_velocity_smoother { using vehicle_info_util::VehicleInfoUtil; - SmootherBase::SmootherBase(rclcpp::Node & node) { auto & p = base_param_; @@ -37,7 +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); @@ -52,18 +56,17 @@ SmootherBase::SmootherBase(rclcpp::Node & node) node.declare_parameter("sparse_min_interval_distance", 4.0); } -void SmootherBase::setParam(const BaseParam & param) {base_param_ = param;} - -SmootherBase::BaseParam SmootherBase::getBaseParam() const {return base_param_;} +void SmootherBase::setParam(const BaseParam & param) { base_param_ = param; } -double SmootherBase::getMaxAccel() const {return base_param_.max_accel;} +SmootherBase::BaseParam SmootherBase::getBaseParam() const { return base_param_; } -double SmootherBase::getMinDecel() const {return base_param_.min_decel;} +double SmootherBase::getMaxAccel() const { return base_param_.max_accel; } -double SmootherBase::getMaxJerk() const {return base_param_.max_jerk;} +double SmootherBase::getMinDecel() const { return base_param_.min_decel; } -double SmootherBase::getMinJerk() const {return base_param_.min_jerk;} +double SmootherBase::getMaxJerk() const { return base_param_.max_jerk; } +double SmootherBase::getMinJerk() const { return base_param_.min_jerk; } boost::optional SmootherBase::applyLateralAccelerationFilter( const TrajectoryPoints & input, [[maybe_unused]] const double v0, @@ -74,10 +77,11 @@ boost::optional SmootherBase::applyLateralAccelerationFilter( } if (input.size() < 3) { - return boost::optional(input); // cannot calculate lateral acc. do nothing. + return boost::optional(input); // cannot calculate lateral acc. do nothing. } // Interpolate with constant interval distance for lateral acceleration calculation. + constexpr double points_interval = 0.1; // [m] std::vector out_arclength; const auto traj_length = motion_utils::calcArcLength(input); for (double s = 0; s < traj_length; s += points_interval) { @@ -88,7 +92,7 @@ boost::optional SmootherBase::applyLateralAccelerationFilter( auto output = motion_utils::convertToTrajectoryPointArray(output_traj); output.back() = input.back(); // keep the final speed. - constexpr double curvature_calc_dist = 5.0; // [m] calc curvature with 5m away points + constexpr double curvature_calc_dist = 5.0; // [m] calc curvature with 5m away points const size_t idx_dist = static_cast(std::max(static_cast((curvature_calc_dist) / points_interval), 1)); @@ -116,9 +120,9 @@ boost::optional SmootherBase::applyLateralAccelerationFilter( if (j >= curvature_v.size()) return output; curvature = std::max(curvature, std::fabs(curvature_v.at(j))); } - 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)); @@ -126,33 +130,10 @@ boost::optional SmootherBase::applyLateralAccelerationFilter( if (output.at(i).longitudinal_velocity_mps > v_curvature_max) { output.at(i).longitudinal_velocity_mps = v_curvature_max; } - - /* Fill the steering tire angle w.r.t. orientation - * - * delta_orientation = velocity * tan(steering_tire_angle) / wheelbase - * - * calculate desired steering_tire_angle w.r.t. delta_orientation, velocity, and wheelbase - * - */ - - const size_t length = - static_cast(std::max( - static_cast(output->at(i).longitudinal_velocity_mps / - points_interval), 1)); - if (i < output->size() - length) { - output->at(i).front_wheel_angle_rad = - static_cast(std::atan( - (tf2::getYaw(output->at(i + length).pose.orientation) - - tf2::getYaw(output->at(i).pose.orientation)) * base_param_.wheel_base / - (static_cast(length) * points_interval))); - } else { - output->at(i).front_wheel_angle_rad = 0.0; - } } return output; } - boost::optional SmootherBase::applySteeringRateLimit( const TrajectoryPoints & input) const { @@ -160,52 +141,64 @@ boost::optional SmootherBase::applySteeringRateLimit( return boost::none; } - if (input.size() < 2) { - return boost::optional(input); // cannot calculate the desired velocity. do nothing. + if (input.size() < 3) { + return boost::optional( + input); // cannot calculate the desired velocity. do nothing. } - - auto output = boost::optional(input); - if (output->empty()) { - return boost::none; + // 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); } - for (size_t i = output->size() - 1; 0 < i; i--) { - const size_t length = - static_cast(std::max( - static_cast(output->at(i).longitudinal_velocity_mps / - points_interval), 1)); - double ds = static_cast(length) * points_interval; - // double prev_vel = output->at(i-1).longitudinal_velocity_mps; - // double cur_vel = output->at(i).longitudinal_velocity_mps; - if (i < output->size() - length) { - double mean_vel = - (output->at(i).longitudinal_velocity_mps + - output->at(i + length).longitudinal_velocity_mps) / 2.0; - double dt = std::max(ds / mean_vel, std::numeric_limits::epsilon()); - double dt_steering = std::max( - std::fabs( - (output->at(i).front_wheel_angle_rad - - output->at(i + length).front_wheel_angle_rad)) / - base_param_.max_steering_angle_rate, std::numeric_limits::epsilon()); - if (dt_steering > dt) { - // change velocity of p0 + 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. - output->at(i).longitudinal_velocity_mps = (ds / dt_steering) * 2.0 - - output->at(i + length).longitudinal_velocity_mps; - if (output->at(i).longitudinal_velocity_mps < base_param_.min_curve_velocity) { - output->at(i).longitudinal_velocity_mps = base_param_.min_curve_velocity; + 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; + } } - RCLCPP_INFO( - rclcpp::get_logger("limsteer").get_child( - "smoother_base"), - "wheelbase: %f min curv vel: %f max_steeringang_rate: %f", - base_param_.wheel_base, base_param_.min_curve_velocity, - base_param_.max_steering_angle_rate); } } - } - return boost::optional(output); + + return output; } } // namespace motion_velocity_smoother