diff --git a/planning/motion_velocity_smoother/README.md b/planning/motion_velocity_smoother/README.md index 0254b53fc7eb..fab01c175cdb 100644 --- a/planning/motion_velocity_smoother/README.md +++ b/planning/motion_velocity_smoother/README.md @@ -30,9 +30,11 @@ This function is used to approach near the obstacle or improve the accuracy of s #### Apply lateral acceleration limit It applies the velocity limit to decelerate at the curve. -It calculate the velocity limit from the curvature of the reference trajectory and the maximum lateral acceleration `max_lateral_accel`. +It calculates the velocity limit from the curvature of the reference trajectory and the maximum lateral acceleration `max_lateral_accel`. 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. + #### Resample trajectory It resamples the points on the reference trajectory with designated time interval. 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 27bcef0855e3..4b29bb821c4a 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 @@ -157,7 +157,9 @@ class MotionVelocitySmootherNode : public rclcpp::Node TrajectoryPoints calcTrajectoryVelocity(const TrajectoryPoints & input) const; - bool smoothVelocity(const TrajectoryPoints & input, TrajectoryPoints & traj_smoothed) const; + bool smoothVelocity( + const TrajectoryPoints & input, const size_t input_closest, + TrajectoryPoints & traj_smoothed) const; std::tuple calcInitialMotion( const TrajectoryPoints & input_traj, const size_t input_closest, diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp index 0cbc7f3ed17d..5a02e48afb91 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp @@ -75,7 +75,9 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase const TrajectoryPoints & input, const double v_current, const int closest_id) const override; boost::optional applyLateralAccelerationFilter( - const TrajectoryPoints & input) const override; + const TrajectoryPoints & input, [[maybe_unused]] const double v0, + [[maybe_unused]] const double a0, + [[maybe_unused]] const bool enable_smooth_limit) const override; void setParam(const Param & param); Param getParam() const; 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 6650c418267c..e34487b767b4 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 @@ -60,7 +60,9 @@ class SmootherBase const TrajectoryPoints & input, const double v_current, const int closest_id) const = 0; virtual boost::optional applyLateralAccelerationFilter( - const TrajectoryPoints & input) const; + const TrajectoryPoints & input, [[maybe_unused]] const double v0 = 0.0, + [[maybe_unused]] const double a0 = 0.0, + [[maybe_unused]] const bool enable_smooth_limit = false) const; double getMaxAccel() const; double getMinDecel() const; diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp index f7d77c5ceb4c..b6b90491b14a 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp @@ -85,6 +85,10 @@ boost::optional applyDecelFilterWithJerkConstraint( boost::optional> updateStateWithJerkConstraint( const double v0, const double a0, const std::map & jerk_profile, const double t); +std::vector calcVelocityProfileWithConstantJerkAndAccelerationLimit( + const TrajectoryPoints & trajectory, const double v0, const double a0, const double jerk, + const double acc_max, const double acc_min); + } // namespace trajectory_utils } // namespace motion_velocity_smoother 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 a9395e82668a..4e8f16bbd8de 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -478,7 +478,7 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( } // Smoothing velocity - if (!smoothVelocity(*traj_extracted, output)) { + if (!smoothVelocity(*traj_extracted, *traj_extracted_closest, output)) { return prev_output_; } @@ -493,10 +493,18 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( } bool MotionVelocitySmootherNode::smoothVelocity( - const TrajectoryPoints & input, TrajectoryPoints & traj_smoothed) const + const TrajectoryPoints & input, const size_t input_closest, + TrajectoryPoints & traj_smoothed) const { + // Calculate initial motion for smoothing + double initial_vel{}; + double initial_acc{}; + InitializeType type{}; + std::tie(initial_vel, initial_acc, type) = calcInitialMotion(input, input_closest, prev_output_); + // Lateral acceleration limit - const auto traj_lateral_acc_filtered = smoother_->applyLateralAccelerationFilter(input); + const auto traj_lateral_acc_filtered = + smoother_->applyLateralAccelerationFilter(input, initial_vel, initial_acc, true); if (!traj_lateral_acc_filtered) { return false; } @@ -505,6 +513,11 @@ bool MotionVelocitySmootherNode::smoothVelocity( const auto traj_pre_resampled_closest = motion_utils::findNearestIndex( *traj_lateral_acc_filtered, current_pose_ptr_->pose, std::numeric_limits::max(), node_param_.delta_yaw_threshold); + if (!traj_pre_resampled_closest) { + RCLCPP_WARN( + get_logger(), "Cannot find closest waypoint for traj_lateral_acc_filtered trajectory"); + return false; + } auto traj_resampled = smoother_->resampleTrajectory( *traj_lateral_acc_filtered, current_odometry_ptr_->twist.twist.linear.x, *traj_pre_resampled_closest); @@ -512,6 +525,13 @@ bool MotionVelocitySmootherNode::smoothVelocity( RCLCPP_WARN(get_logger(), "Fail to do resampling before the optimization"); return false; } + const auto traj_resampled_closest = motion_utils::findNearestIndex( + *traj_resampled, current_pose_ptr_->pose, std::numeric_limits::max(), + node_param_.delta_yaw_threshold); + if (!traj_resampled_closest) { + RCLCPP_WARN(get_logger(), "Cannot find closest waypoint for resampled trajectory"); + return false; + } // Set 0[m/s] in the terminal point if (!traj_resampled->empty()) { @@ -521,20 +541,6 @@ bool MotionVelocitySmootherNode::smoothVelocity( // Publish Closest Resample Trajectory Velocity publishClosestVelocity(*traj_resampled, current_pose_ptr_->pose, debug_closest_max_velocity_); - // Calculate initial motion for smoothing - double initial_vel{}; - double initial_acc{}; - InitializeType type{}; - const auto traj_resampled_closest = motion_utils::findNearestIndex( - *traj_resampled, current_pose_ptr_->pose, std::numeric_limits::max(), - node_param_.delta_yaw_threshold); - if (!traj_resampled_closest) { - RCLCPP_WARN(get_logger(), "Cannot find closest waypoint for resampled trajectory"); - return false; - } - std::tie(initial_vel, initial_acc, type) = - calcInitialMotion(*traj_resampled, *traj_resampled_closest, prev_output_); - // Clip trajectory from closest point TrajectoryPoints clipped; clipped.insert( diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index e862f699c47e..56019abab894 100644 --- a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -273,7 +273,8 @@ boost::optional AnalyticalJerkConstrainedSmoother::resampleTra } boost::optional AnalyticalJerkConstrainedSmoother::applyLateralAccelerationFilter( - const TrajectoryPoints & input) const + const TrajectoryPoints & input, [[maybe_unused]] const double v0, + [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit) const { if (input.empty()) { return boost::none; diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp index 31c6420a6787..52aedeb9f7be 100644 --- a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp @@ -59,7 +59,8 @@ 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) const + const TrajectoryPoints & input, [[maybe_unused]] const double v0, + [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit) const { if (input.empty()) { return boost::none; @@ -102,6 +103,12 @@ boost::optional SmootherBase::applyLateralAccelerationFilter( static_cast(std::round(base_param_.decel_distance_after_curve / points_interval)); const double max_lateral_accel_abs = std::fabs(base_param_.max_lateral_accel); + const auto latacc_min_vel_arr = + enable_smooth_limit + ? trajectory_utils::calcVelocityProfileWithConstantJerkAndAccelerationLimit( + *output, v0, a0, base_param_.min_jerk, base_param_.max_accel, base_param_.min_decel) + : std::vector{}; + for (size_t i = 0; i < output->size(); ++i) { double curvature = 0.0; const size_t start = i > after_decel_index ? i - after_decel_index : 0; @@ -111,6 +118,9 @@ 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) { + v_curvature_max = std::max(v_curvature_max, latacc_min_vel_arr.at(i)); + } if (output->at(i).longitudinal_velocity_mps > v_curvature_max) { output->at(i).longitudinal_velocity_mps = v_curvature_max; } diff --git a/planning/motion_velocity_smoother/src/trajectory_utils.cpp b/planning/motion_velocity_smoother/src/trajectory_utils.cpp index c2b907eec29c..0dfe4a0c8e6c 100644 --- a/planning/motion_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/motion_velocity_smoother/src/trajectory_utils.cpp @@ -639,5 +639,32 @@ boost::optional> updateStateWithJerkC return {}; } +std::vector calcVelocityProfileWithConstantJerkAndAccelerationLimit( + const TrajectoryPoints & trajectory, const double v0, const double a0, const double jerk, + const double acc_max, const double acc_min) +{ + if (trajectory.empty()) return {}; + + std::vector velocities(trajectory.size()); + velocities.at(0) = v0; + auto curr_v = v0; + auto curr_a = a0; + + const auto intervals = calcTrajectoryIntervalDistance(trajectory); + + if (intervals.size() + 1 != trajectory.size()) { + throw std::logic_error("interval calculation result has unexpected array size."); + } + + for (size_t i = 0; i < intervals.size(); ++i) { + const auto t = intervals.at(i) / std::max(velocities.at(i), 1.0e-5); + curr_v = integ_v(curr_v, curr_a, jerk, t); + velocities.at(i + 1) = curr_v; + curr_a = std::clamp(integ_a(curr_a, jerk, t), acc_min, acc_max); + } + + return velocities; +} + } // namespace trajectory_utils } // namespace motion_velocity_smoother