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 e85e8f25cbc91..c1cabcfa0b752 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,7 +46,7 @@ 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] + steer_rate_lim_dps: 15.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 e3c496a707eb5..09b697504887d 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 @@ -48,10 +48,10 @@ post_sparse_min_interval_distance: 1.0 # minimum points-interval length for sparse sampling [m] # steering angle rate limit parameters - max_steering_angle_rate: 5.0 # steering angle rate [degree/s] + max_steering_angle_rate: 15.0 # maximum steering angle rate [degree/s] resample_ds: 0.1 # distance between trajectory points [m] - lookup_distance: 2.0 # look-up distance of Trajectory point for calculation of steering angle limit [m] - threshold_trigger: 2.0 # threshold steering degree limit to trigger steeringRateLimit [degree] + 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/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 365195c040069..37572cf3bb291 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 @@ -52,10 +52,10 @@ class SmootherBase 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 lookup_dist; // look-up distance of Trajectory point for calculation of steering angle - // limit [m] + double curvature_threshold; // look-up distance of Trajectory point for calculation of steering angle + // limit [m] double - threshold_trigger; // threshold steering degree limit to trigger steeringRateLimit [degree] + curvature_calculation_distance; // threshold steering degree limit to trigger steeringRateLimit [degree] resampling::ResampleParam resample_param; }; 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 a9d10f6494ab9..2ba5c5dc4de90 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -180,9 +180,9 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter 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("lookup_distance", p.lookup_dist); + update_param("curvature_threshold", p.curvature_threshold); update_param("max_steering_angle_rate", p.max_steering_angle_rate); - update_param("threshold_trigger", p.threshold_trigger); + update_param("curvature_calculation_distance", p.curvature_calculation_distance); smoother_->setParam(p); } @@ -524,7 +524,7 @@ bool MotionVelocitySmootherNode::smoothVelocity( // 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_.delta_yaw_threshold); if (!traj_resampled) { RCLCPP_WARN(get_logger(), "Fail to do resampling before the optimization"); diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp index 623f575d30187..e2c188933e92d 100644 --- a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp @@ -39,9 +39,9 @@ SmootherBase::SmootherBase(rclcpp::Node & node) 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.lookup_dist = node.declare_parameter("lookup_distance", 2.0); + 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.threshold_trigger = node.declare_parameter("threshold_trigger", 3.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); @@ -67,7 +67,41 @@ double SmootherBase::getMinDecel() const { return base_param_.min_decel; } double SmootherBase::getMaxJerk() const { return base_param_.max_jerk; } double SmootherBase::getMinJerk() const { return base_param_.min_jerk; } +std::vector calcCurvatureFrom3Points( + const TrajectoryPoints & trajectory, size_t idx_dist) +{ + using tier4_autoware_utils::calcCurvature; + using tier4_autoware_utils::getPoint; + + if (trajectory.size() < 3) { + const std::vector k_arr(trajectory.size(), 0.0); + return k_arr; + } + + // if the idx size is not enough, change the idx_dist + const auto max_idx_dist = static_cast(std::floor((trajectory.size() - 1) / 2.0)); + idx_dist = std::max(1ul, std::min(idx_dist, max_idx_dist)); + + if (idx_dist < 1) { + throw std::logic_error("idx_dist less than 1 is not expected"); + } + + // calculate curvature by circle fitting from three points + std::vector k_arr; + // for first point curvature = 0; + k_arr.push_back(0.0); + for (size_t i = 1; i + 1 < trajectory.size(); i++) { + const auto p0 = getPoint(trajectory.at(i - std::min(idx_dist, i))); + const auto p1 = getPoint(trajectory.at(i)); + const auto p2 = getPoint(trajectory.at(i + std::min(idx_dist, trajectory.size() - 1 - i))); + k_arr.push_back(calcCurvature(p0, p1, p2)); + } + // for last point curvature = 0; + k_arr.push_back(0.0); + + return k_arr; +} boost::optional SmootherBase::applyLateralAccelerationFilter( const TrajectoryPoints & input, [[maybe_unused]] const double v0, [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit) const @@ -122,6 +156,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)); @@ -133,6 +168,8 @@ boost::optional SmootherBase::applyLateralAccelerationFilter( return output; } + + boost::optional SmootherBase::applySteeringRateLimit( const TrajectoryPoints & input) const { @@ -140,112 +177,58 @@ boost::optional SmootherBase::applySteeringRateLimit( return boost::none; } - if (input.size() < 2) { + 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 std::vector in_arclength = trajectory_utils::calcArclengthArray(input); - for (double s = 0; s < in_arclength.back(); s += base_param_.sample_ds) { + 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); } - auto output = trajectory_utils::applyLinearInterpolation(in_arclength, input, out_arclength); - if (!output) { - RCLCPP_WARN( - rclcpp::get_logger("smoother").get_child("smoother_base"), - "interpolation failed at steering angle rate limit."); - return boost::none; - } - - std::vector arc_length_array(output->size() - 1); - - for (size_t i = 0; output->size() > i + 1; i++) { - /* - * calculate the arc length of 2 trajectory points by using orientation of vehicle - */ - - const double heading_diff = - std::fabs(tf2::getYaw(output->at(i + 1).pose.orientation - output->at(i).pose.orientation)); - const double radius = base_param_.sample_ds / (2.0 * sin(heading_diff / 2.0)); - const double arc_length = radius * heading_diff; - arc_length_array.at(i) = arc_length; - - /* Fill the steering tire angle w.r.t. orientation - * - * delta_orientation / delta_time = velocity * tan(steering_tire_angle) / wheelbase - * - * calculate desired steering_tire_angle w.r.t. delta_orientation, velocity, and wheelbase - * - */ - - output->at(i).front_wheel_angle_rad = static_cast(std::atan( - (tf2::getYaw(output->at(i + 1).pose.orientation - output->at(i).pose.orientation) / - arc_length_array.at(i)) * - base_param_.wheel_base)); - } - output->back().front_wheel_angle_rad = 0.0; - - const size_t lookup_index = base_param_.lookup_dist / base_param_.sample_ds; - - for (size_t i = 0; i < output->size(); i++) { - const size_t base = output->size() - 1 - i; - - if (lookup_index > i) { - continue; - } - double ds = 0.0; - double tot_vel = 0.0; - double tot_steering_diff = 0.0; - double tot_steering = 0.0; - - for (size_t k = 0; k < lookup_index; k++) { - ds += arc_length_array.at(base + k); - tot_vel += (output->at(base + k).longitudinal_velocity_mps + - output->at(base + k + 1).longitudinal_velocity_mps) / - 2.0; - tot_steering += (output->at(base + k).front_wheel_angle_rad + - output->at(base + k + 1).front_wheel_angle_rad) / - 2.0; - tot_steering_diff += fabs( - output->at(base + k).front_wheel_angle_rad - - output->at(1 + k + base).front_wheel_angle_rad); - } - - double mean_steer = tot_steering / static_cast(lookup_index); - - if (fabs(mean_steer) > tier4_autoware_utils::deg2rad(base_param_.threshold_trigger)) { - const double mean_vel = tot_vel / static_cast(lookup_index); - const double dt = std::max(ds / mean_vel, std::numeric_limits::epsilon()); - - const double dt_steering = - tot_steering_diff / tier4_autoware_utils::deg2rad(base_param_.max_steering_angle_rate); - const double mean_steering_diff = tot_steering_diff / static_cast(lookup_index); + 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. - if (dt_steering > dt) { - const double target_mean_vel = (ds / dt_steering); + const size_t idx_dist = + static_cast(std::max(static_cast((base_param_.curvature_calculation_distance) / base_param_.sample_ds), 1)); - double tmp_vel = - target_mean_vel * fabs( - mean_steering_diff / (output->at(base).front_wheel_angle_rad - - output->at(base + 1).front_wheel_angle_rad)); - if (tmp_vel < output->at(base).longitudinal_velocity_mps) { - output->at(base).longitudinal_velocity_mps = tmp_vel; - } else { - if (target_mean_vel < output->at(base).longitudinal_velocity_mps) { - output->at(base).longitudinal_velocity_mps = target_mean_vel; + // Calculate curvature assuming the trajectory points interval is constant + const auto curvature_v = calcCurvatureFrom3Points(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++){ + 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; + } } } - // else{ - // if(target_mean_vel < output->at(base + k).longitudinal_velocity_mps) { - // output->at(base + k).longitudinal_velocity_mps = target_mean_vel; - // } - // } - if (output->at(base).longitudinal_velocity_mps < base_param_.min_curve_velocity) { - output->at(base).longitudinal_velocity_mps = base_param_.min_curve_velocity; - } } - } } return output;