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 b4674b64b2507..f1821bfd5f843 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 @@ -46,8 +46,8 @@ 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: 600.0 # steering angle rate limit [deg/s] - acceleration_limit: 2.0 # acceleration limit for trajectory velocity modification [m/ss] + steer_rate_lim_dps: 5.0 # steering angle rate limit [deg/s] + acceleration_limit: 0.9 # 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 28fa297afbecb..c3deef24e8a36 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: 3.0 - min_acc: -5.0 + max_acc: 0.9 + min_acc: -0.9 # jerk limit - max_jerk: 2.0 - min_jerk: -5.0 + max_jerk: 0.6 + min_jerk: -0.6 # 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 a23570a5fc791..d7041fda955ad 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: 1.0 # max acceleration [m/ss] - min_jerk: -0.5 # min jerk [m/sss] - max_jerk: 1.0 # max jerk [m/sss] + max_acc: 0.5 # max acceleration [m/ss] + min_jerk: -0.4 # min jerk [m/sss] + max_jerk: 0.4 # max jerk [m/sss] # constraints to be observed limit: - 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] + 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] 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..dc0d2b50db076 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.8 # max lateral acceleration limit [m/ss] - min_curve_velocity: 2.74 # min velocity at lateral acceleration limit [m/ss] + max_lateral_accel: 0.3 # max lateral acceleration limit [m/ss] + min_curve_velocity: 0.5 # min velocity at lateral acceleration limit [m/ss] 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,8 @@ 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] + # 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/map/map_loader/config/lanelet2_map_loader.param.yaml b/map/map_loader/config/lanelet2_map_loader.param.yaml index 55f810b0e9ff9..f7eaf3571ab9c 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: MGRS # Options: MGRS, UTM - latitude: 40.816617984672746 # Latitude of map_origin, using in UTM - longitude: 29.360491808334285 # Longitude of map_origin, using in UTM + 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 center_line_resolution: 5.0 # [m] 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 4b29bb821c4ac..463602223b815 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 @@ -82,6 +82,7 @@ class MotionVelocitySmootherNode : public rclcpp::Node 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 boost::optional prev_closest_point_{}; // previous trajectory point @@ -199,6 +200,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 e34487b767b45..2598e38853b1d 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 @@ -22,6 +22,7 @@ #include "tier4_autoware_utils/geometry/geometry.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" #include "boost/optional.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,7 +49,9 @@ 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 - resampling::ResampleParam resample_param; + double max_steering_angle_rate; // max steering angle rate [degree/s] + double wheel_base; // wheel base [m] + resampling::ResampleParam resample_param; }; explicit SmootherBase(rclcpp::Node & node); @@ -64,6 +68,10 @@ 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; @@ -74,6 +82,10 @@ 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/package.xml b/planning/motion_velocity_smoother/package.xml index a634752ba84c3..269fc9c2cd4e0 100644 --- a/planning/motion_velocity_smoother/package.xml +++ b/planning/motion_velocity_smoother/package.xml @@ -27,6 +27,7 @@ tier4_autoware_utils tier4_debug_msgs tier4_planning_msgs + vehicle_info_util ament_lint_auto autoware_lint_common 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 4e8f16bbd8deb..2a30a6d099fb5 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 #include @@ -37,6 +38,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,8 +75,12 @@ 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 + // publishers, subscribers pub_trajectory_ = create_publisher("~/output/trajectory", 1); pub_velocity_limit_ = create_publisher( "~/output/current_velocity_limit_mps", rclcpp::QoS{1}.transient_local()); @@ -93,7 +100,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 +111,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 @@ -167,6 +176,7 @@ 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("max_steering_angle_rate", p.max_steering_angle_rate); smoother_->setParam(p); } @@ -506,8 +516,20 @@ bool MotionVelocitySmootherNode::smoothVelocity( const auto traj_lateral_acc_filtered = smoother_->applyLateralAccelerationFilter(input, initial_vel, initial_acc, true); if (!traj_lateral_acc_filtered) { + RCLCPP_WARN(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_WARN(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 const auto traj_pre_resampled_closest = motion_utils::findNearestIndex( @@ -519,7 +541,7 @@ bool MotionVelocitySmootherNode::smoothVelocity( return false; } 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, *traj_pre_resampled_closest); if (!traj_resampled) { RCLCPP_WARN(get_logger(), "Fail to do resampling before the optimization"); @@ -574,6 +596,8 @@ bool MotionVelocitySmootherNode::smoothVelocity( if (publish_debug_trajs_) { pub_trajectory_latacc_filtered_->publish( toTrajectoryMsg(*traj_lateral_acc_filtered, base_traj_raw_ptr_->header)); + pub_trajectory_steering_rate_limited_->publish( + toTrajectoryMsg(*traj_steering_rate_limited, base_traj_raw_ptr_->header)); pub_trajectory_resampled_->publish( toTrajectoryMsg(*traj_resampled, base_traj_raw_ptr_->header)); diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp index 52aedeb9f7be3..bade33d1408a0 100644 --- a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp @@ -23,6 +23,9 @@ namespace motion_velocity_smoother { +using vehicle_info_util::VehicleInfoUtil; + + SmootherBase::SmootherBase(rclcpp::Node & node) { auto & p = base_param_; @@ -32,6 +35,7 @@ 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.max_steering_angle_rate = node.declare_parameter("max_steering_angle_rate", 5.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); @@ -46,17 +50,18 @@ SmootherBase::SmootherBase(rclcpp::Node & node) node.declare_parameter("sparse_min_interval_distance", 4.0); } -void SmootherBase::setParam(const BaseParam & param) { base_param_ = param; } +void SmootherBase::setParam(const BaseParam & param) {base_param_ = param;} + +SmootherBase::BaseParam SmootherBase::getBaseParam() const {return base_param_;} -SmootherBase::BaseParam SmootherBase::getBaseParam() const { return base_param_; } +double SmootherBase::getMaxAccel() const {return base_param_.max_accel;} -double SmootherBase::getMaxAccel() const { return base_param_.max_accel; } +double SmootherBase::getMinDecel() const {return base_param_.min_decel;} -double SmootherBase::getMinDecel() const { return base_param_.min_decel; } +double SmootherBase::getMaxJerk() const {return base_param_.max_jerk;} -double SmootherBase::getMaxJerk() const { return base_param_.max_jerk; } +double SmootherBase::getMinJerk() const {return base_param_.min_jerk;} -double SmootherBase::getMinJerk() const { return base_param_.min_jerk; } boost::optional SmootherBase::applyLateralAccelerationFilter( const TrajectoryPoints & input, [[maybe_unused]] const double v0, @@ -67,11 +72,10 @@ 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 std::vector in_arclength = trajectory_utils::calcArclengthArray(input); for (double s = 0; s < in_arclength.back(); s += points_interval) { @@ -84,9 +88,9 @@ boost::optional SmootherBase::applyLateralAccelerationFilter( "interpolation failed at lateral acceleration filter."); return boost::none; } - output->back() = input.back(); // keep the final speed. + 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,6 +120,7 @@ boost::optional SmootherBase::applyLateralAccelerationFilter( for (size_t j = start; j < end; ++j) { 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) { @@ -124,8 +129,86 @@ 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 +{ + if (input.empty()) { + return boost::none; + } + + if (input.size() < 2) { + return boost::optional(input); // cannot calculate the desired velocity. do nothing. + } + + auto output = boost::optional(input); + if (output->empty()) { + return boost::none; + } + 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 + + 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; + + } + 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); +} + } // namespace motion_velocity_smoother