diff --git a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp index 228cd0768f465..3f6474ffbf895 100644 --- a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp +++ b/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp @@ -105,7 +105,8 @@ inline bool smoothPath( // Resample trajectory with ego-velocity based interval distances auto traj_resampled = smoother->resampleTrajectory( - *traj_lateral_acc_filtered, v0, current_pose, std::numeric_limits::max()); + *traj_lateral_acc_filtered, v0, current_pose, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold); const size_t traj_resampled_closest = motion_utils::findFirstNearestIndexWithSoftConstraints( *traj_resampled, current_pose, planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold); 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 6ea62a101e39b..dcc5079c9f9fb 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 @@ -125,7 +125,9 @@ class MotionVelocitySmootherNode : public rclcpp::Node double extract_ahead_dist; // forward waypoints distance from current position [m] double extract_behind_dist; // backward waypoints distance from current position [m] double stop_dist_to_prohibit_engage; // prevent to move toward close stop point - double delta_yaw_threshold; // for closest index calculation + double ego_nearest_dist_threshold; // for ego's closest index calculation + double ego_nearest_yaw_threshold; // for ego's closest index calculation + resampling::ResampleParam post_resample_param; AlgorithmType algorithm_type; // Option : JerkFiltered, Linf, L2 } node_param_{}; @@ -225,9 +227,7 @@ class MotionVelocitySmootherNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_closest_merged_velocity_; // helper functions - boost::optional findNearestIndex( - const TrajectoryPoints & points, const geometry_msgs::msg::Pose & p) const; - boost::optional findNearestIndexFromEgo(const TrajectoryPoints & points) const; + size_t findNearestIndexFromEgo(const TrajectoryPoints & points) const; bool isReverse(const TrajectoryPoints & points) const; void flipVelocity(TrajectoryPoints & points) const; void publishStopWatchTime(); diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp index 76fffd5f95901..97e4c974f9ad7 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp @@ -45,13 +45,13 @@ struct ResampleParam boost::optional resampleTrajectory( const TrajectoryPoints & input, const double v_current, - const geometry_msgs::msg::Pose & current_pose, const double delta_yaw_threshold, - const ResampleParam & param, const bool use_zoh_for_v = true); + const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold, + const double nearest_yaw_threshold, const ResampleParam & param, const bool use_zoh_for_v = true); boost::optional resampleTrajectory( const TrajectoryPoints & input, const geometry_msgs::msg::Pose & current_pose, - const double delta_yaw_threshold, const ResampleParam & param, const double nominal_ds, - const bool use_zoh_for_v = true); + const double nearest_dist_threshold, const double nearest_yaw_threshold, + const ResampleParam & param, const double nominal_ds, const bool use_zoh_for_v = true); } // namespace resampling } // namespace motion_velocity_smoother 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 32e97b5511827..0603c8305cee9 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 @@ -74,7 +74,8 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase boost::optional resampleTrajectory( const TrajectoryPoints & input, [[maybe_unused]] const double v0, [[maybe_unused]] const geometry_msgs::msg::Pose & current_pose, - [[maybe_unused]] const double delta_yaw_threshold) const override; + [[maybe_unused]] const double nearest_dist_threshold, + [[maybe_unused]] const double nearest_yaw_threshold) const override; boost::optional applyLateralAccelerationFilter( const TrajectoryPoints & input, [[maybe_unused]] const double v0, diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp index 0cbf3a2cbd733..d4b65e695bb89 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp @@ -48,7 +48,8 @@ class JerkFilteredSmoother : public SmootherBase boost::optional resampleTrajectory( const TrajectoryPoints & input, [[maybe_unused]] const double v0, - const geometry_msgs::msg::Pose & current_pose, const double delta_yaw_threshold) const override; + const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold, + const double nearest_yaw_threshold) const override; void setParam(const Param & param); Param getParam() const; diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp index c822c2625c29a..de1d3cd4491c3 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp @@ -46,7 +46,7 @@ class L2PseudoJerkSmoother : public SmootherBase boost::optional resampleTrajectory( const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, - const double delta_yaw_threshold) const override; + const double nearest_dist_threshold, const double nearest_yaw_threshold) const override; void setParam(const Param & smoother_param); Param getParam() const; diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp index 145336211228f..dadb314edbb58 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp @@ -46,7 +46,7 @@ class LinfPseudoJerkSmoother : public SmootherBase boost::optional resampleTrajectory( const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, - const double delta_yaw_threshold) const override; + const double nearest_dist_threshold, const double nearest_yaw_threshold) const override; void setParam(const Param & smoother_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 0d401af8e5ebf..0ec6330394a56 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 @@ -58,7 +58,7 @@ class SmootherBase virtual boost::optional resampleTrajectory( const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, - const double delta_yaw_threshold) const = 0; + const double nearest_dist_threshold, const double nearest_yaw_threshold) const = 0; virtual boost::optional applyLateralAccelerationFilter( const TrajectoryPoints & input, [[maybe_unused]] const double v0 = 0.0, 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 8c73329c1808b..a1fee003703e3 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 @@ -38,7 +38,7 @@ using TrajectoryPoints = std::vector; using geometry_msgs::msg::Pose; TrajectoryPoint calcInterpolatedTrajectoryPoint( - const TrajectoryPoints & trajectory, const Pose & target_pose); + const TrajectoryPoints & trajectory, const Pose & target_pose, const size_t seg_idx); TrajectoryPoints extractPathAroundIndex( const TrajectoryPoints & trajectory, const size_t index, const double & ahead_length, diff --git a/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml b/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml index d21c050caf9c5..8a1042fafa1e3 100644 --- a/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml +++ b/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml @@ -1,6 +1,7 @@ + @@ -16,6 +17,7 @@ + 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 bd1065b8b891f..ba0018e26ce77 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -148,7 +148,8 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter update_param("extract_ahead_dist", p.extract_ahead_dist); update_param("extract_behind_dist", p.extract_behind_dist); update_param("stop_dist_to_prohibit_engage", p.stop_dist_to_prohibit_engage); - update_param("delta_yaw_threshold", p.delta_yaw_threshold); + update_param("ego_nearest_dist_threshold", p.ego_nearest_dist_threshold); + update_param("ego_nearest_yaw_threshold", p.ego_nearest_yaw_threshold); } { @@ -245,7 +246,8 @@ void MotionVelocitySmootherNode::initCommonParam() p.extract_ahead_dist = declare_parameter("extract_ahead_dist", 200.0); p.extract_behind_dist = declare_parameter("extract_behind_dist", 3.0); p.stop_dist_to_prohibit_engage = declare_parameter("stop_dist_to_prohibit_engage", 1.5); - p.delta_yaw_threshold = declare_parameter("delta_yaw_threshold", M_PI / 3.0); + p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); + p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); p.post_resample_param.max_trajectory_length = declare_parameter("post_max_trajectory_length", 300.0); p.post_resample_param.min_trajectory_length = @@ -379,7 +381,8 @@ void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstShar // Note that output velocity is resampled by linear interpolation auto output_resampled = resampling::resampleTrajectory( output, current_odometry_ptr_->twist.twist.linear.x, current_pose_ptr_->pose, - node_param_.delta_yaw_threshold, node_param_.post_resample_param, false); + node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold, + node_param_.post_resample_param, false); if (!output_resampled) { RCLCPP_WARN(get_logger(), "Failed to get the resampled output trajectory"); return; @@ -432,16 +435,10 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( TrajectoryPoints output{}; // velocity is optimized by qp solver // Extract trajectory around self-position with desired forward-backward length - const auto input_closest = findNearestIndexFromEgo(traj_input); - - if (!input_closest) { - RCLCPP_WARN_THROTTLE( - get_logger(), *clock_, 5000, "Cannot find the closest point from input trajectory"); - return prev_output_; - } + const size_t input_closest = findNearestIndexFromEgo(traj_input); auto traj_extracted = trajectory_utils::extractPathAroundIndex( - traj_input, *input_closest, node_param_.extract_ahead_dist, node_param_.extract_behind_dist); + traj_input, input_closest, node_param_.extract_ahead_dist, node_param_.extract_behind_dist); if (traj_extracted.empty()) { RCLCPP_WARN(get_logger(), "Fail to extract the path from the input trajectory"); return prev_output_; @@ -458,12 +455,7 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( applyExternalVelocityLimit(traj_extracted); // Change trajectory velocity to zero when current_velocity == 0 & stop_dist is close - const auto traj_extracted_closest = findNearestIndexFromEgo(traj_extracted); - - if (!traj_extracted_closest) { - RCLCPP_WARN(get_logger(), "Cannot find the closest point from extracted trajectory"); - return prev_output_; - } + const size_t traj_extracted_closest = findNearestIndexFromEgo(traj_extracted); // Apply velocity to approach stop point applyStopApproachingVelocity(traj_extracted); @@ -476,7 +468,7 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( } // Smoothing velocity - if (!smoothVelocity(traj_extracted, *traj_extracted_closest, output)) { + if (!smoothVelocity(traj_extracted, traj_extracted_closest, output)) { return prev_output_; } @@ -500,17 +492,13 @@ 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, - current_pose_ptr_->pose, node_param_.delta_yaw_threshold); + current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); if (!traj_resampled) { RCLCPP_WARN(get_logger(), "Fail to do resampling before the optimization"); return false; } - const auto traj_resampled_closest = findNearestIndexFromEgo(*traj_resampled); - - if (!traj_resampled_closest) { - RCLCPP_WARN(get_logger(), "Cannot find closest waypoint for resampled trajectory"); - return false; - } + const size_t traj_resampled_closest = findNearestIndexFromEgo(*traj_resampled); // Set 0[m/s] in the terminal point if (!traj_resampled->empty()) { @@ -523,7 +511,7 @@ bool MotionVelocitySmootherNode::smoothVelocity( // Clip trajectory from closest point TrajectoryPoints clipped; clipped.insert( - clipped.end(), traj_resampled->begin() + *traj_resampled_closest, traj_resampled->end()); + clipped.end(), traj_resampled->begin() + traj_resampled_closest, traj_resampled->end()); std::vector debug_trajectories; if (!smoother_->apply( @@ -536,7 +524,7 @@ bool MotionVelocitySmootherNode::smoothVelocity( traj_smoothed.insert( traj_smoothed.begin(), traj_resampled->begin(), - traj_resampled->begin() + *traj_resampled_closest); + traj_resampled->begin() + traj_resampled_closest); // For the endpoint of the trajectory if (!traj_smoothed.empty()) { @@ -545,10 +533,10 @@ bool MotionVelocitySmootherNode::smoothVelocity( // Max velocity filter for safety trajectory_utils::applyMaximumVelocityLimit( - *traj_resampled_closest, traj_smoothed.size(), node_param_.max_velocity, traj_smoothed); + traj_resampled_closest, traj_smoothed.size(), node_param_.max_velocity, traj_smoothed); // Insert behind velocity for output's consistency - insertBehindVelocity(*traj_resampled_closest, type, traj_smoothed); + insertBehindVelocity(traj_resampled_closest, type, traj_smoothed); RCLCPP_DEBUG(get_logger(), "smoothVelocity : traj_smoothed.size() = %lu", traj_smoothed.size()); if (publish_debug_trajs_) { @@ -567,10 +555,10 @@ bool MotionVelocitySmootherNode::smoothVelocity( for (auto & debug_trajectory : debug_trajectories) { debug_trajectory.insert( debug_trajectory.begin(), traj_resampled->begin(), - traj_resampled->begin() + *traj_resampled_closest); - for (size_t i = 0; i < *traj_resampled_closest; ++i) { + traj_resampled->begin() + traj_resampled_closest); + for (size_t i = 0; i < traj_resampled_closest; ++i) { debug_trajectory.at(i).longitudinal_velocity_mps = - debug_trajectory.at(*traj_resampled_closest).longitudinal_velocity_mps; + debug_trajectory.at(traj_resampled_closest).longitudinal_velocity_mps; } } } @@ -592,8 +580,27 @@ void MotionVelocitySmootherNode::insertBehindVelocity( output.at(i).longitudinal_velocity_mps = output.at(output_closest).longitudinal_velocity_mps; output.at(i).acceleration_mps2 = output.at(output_closest).acceleration_mps2; } else { + // TODO(planning/control team) deal with overlapped lanes with the same direction + const size_t seg_idx = [&]() { + // with distance and yaw thresholds + const auto opt_nearest_seg_idx = motion_utils::findNearestSegmentIndex( + prev_output_, output.at(i).pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + if (opt_nearest_seg_idx) { + return opt_nearest_seg_idx.get(); + } + + // with distance threshold + const auto opt_second_nearest_seg_idx = motion_utils::findNearestSegmentIndex( + prev_output_, output.at(i).pose, node_param_.ego_nearest_dist_threshold); + if (opt_second_nearest_seg_idx) { + return opt_second_nearest_seg_idx.get(); + } + + return motion_utils::findNearestSegmentIndex(prev_output_, output.at(i).pose.position); + }(); const auto prev_output_point = - trajectory_utils::calcInterpolatedTrajectoryPoint(prev_output_, output.at(i).pose); + trajectory_utils::calcInterpolatedTrajectoryPoint(prev_output_, output.at(i).pose, seg_idx); // output should be always positive: TODO(Horibe) think better way output.at(i).longitudinal_velocity_mps = @@ -605,11 +612,7 @@ void MotionVelocitySmootherNode::insertBehindVelocity( void MotionVelocitySmootherNode::publishStopDistance(const TrajectoryPoints & trajectory) const { - const auto closest_optional = findNearestIndexFromEgo(trajectory); - if (!closest_optional) { - return; - } - const auto closest = *closest_optional; + const size_t closest = findNearestIndexFromEgo(trajectory); // stop distance calculation const double stop_dist_lim{50.0}; @@ -645,8 +648,11 @@ MotionVelocitySmootherNode::calcInitialMotion( return std::make_pair(initial_motion, type); } - const auto prev_output_closest_point = - trajectory_utils::calcInterpolatedTrajectoryPoint(prev_traj, current_pose_ptr_->pose); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prev_traj, current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + const auto prev_output_closest_point = trajectory_utils::calcInterpolatedTrajectoryPoint( + prev_traj, current_pose_ptr_->pose, current_seg_idx); // when velocity tracking deviation is large const double desired_vel{prev_output_closest_point.longitudinal_velocity_mps}; @@ -715,7 +721,10 @@ void MotionVelocitySmootherNode::overwriteStopPoint( } // Get Closest Point from Output - const auto nearest_output_point_idx = findNearestIndex(output, input.at(*stop_idx).pose); + // TODO(planning/control team) deal with overlapped lanes with the same directions + const auto nearest_output_point_idx = motion_utils::findNearestIndex( + output, input.at(*stop_idx).pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); // check over velocity bool is_stop_velocity_exceeded{false}; @@ -759,13 +768,10 @@ void MotionVelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & t trajectory_utils::applyMaximumVelocityLimit( 0, traj.size(), max_velocity_with_deceleration_, traj); - const auto closest_idx = findNearestIndexFromEgo(traj); - if (!closest_idx) { - return; - } + const size_t closest_idx = findNearestIndexFromEgo(traj); double dist = 0.0; - for (size_t idx = *closest_idx; idx < traj.size() - 1; ++idx) { + for (size_t idx = closest_idx; idx < traj.size() - 1; ++idx) { dist += tier4_autoware_utils::calcDistance2d(traj.at(idx), traj.at(idx + 1)); if (dist > external_velocity_limit_dist_) { trajectory_utils::applyMaximumVelocityLimit( @@ -822,8 +828,11 @@ void MotionVelocitySmootherNode::publishClosestVelocity( const TrajectoryPoints & trajectory, const Pose & current_pose, const rclcpp::Publisher::SharedPtr pub) const { + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + trajectory, current_pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); const auto closest_point = - trajectory_utils::calcInterpolatedTrajectoryPoint(trajectory, current_pose); + trajectory_utils::calcInterpolatedTrajectoryPoint(trajectory, current_pose, current_seg_idx); Float32Stamped vel_data{}; vel_data.stamp = this->now(); @@ -833,8 +842,11 @@ void MotionVelocitySmootherNode::publishClosestVelocity( void MotionVelocitySmootherNode::publishClosestState(const TrajectoryPoints & trajectory) { - const auto closest_point = - trajectory_utils::calcInterpolatedTrajectoryPoint(trajectory, current_pose_ptr_->pose); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + trajectory, current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + const auto closest_point = trajectory_utils::calcInterpolatedTrajectoryPoint( + trajectory, current_pose_ptr_->pose, current_seg_idx); auto publishFloat = [=](const double data, const auto pub) { Float32Stamped msg{}; @@ -871,8 +883,11 @@ void MotionVelocitySmootherNode::updatePrevValues(const TrajectoryPoints & final { prev_output_ = final_result; - const auto closest_point = - trajectory_utils::calcInterpolatedTrajectoryPoint(final_result, current_pose_ptr_->pose); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + final_result, current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + const auto closest_point = trajectory_utils::calcInterpolatedTrajectoryPoint( + final_result, current_pose_ptr_->pose, current_seg_idx); prev_closest_point_ = closest_point; } @@ -898,8 +913,11 @@ MotionVelocitySmootherNode::AlgorithmType MotionVelocitySmootherNode::getAlgorit double MotionVelocitySmootherNode::calcTravelDistance() const { - const auto closest_point = - trajectory_utils::calcInterpolatedTrajectoryPoint(prev_output_, current_pose_ptr_->pose); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prev_output_, current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + const auto closest_point = trajectory_utils::calcInterpolatedTrajectoryPoint( + prev_output_, current_pose_ptr_->pose, current_seg_idx); if (prev_closest_point_) { const double travel_dist = @@ -925,17 +943,11 @@ Trajectory MotionVelocitySmootherNode::toTrajectoryMsg( return trajectory; } -boost::optional MotionVelocitySmootherNode::findNearestIndexFromEgo( - const TrajectoryPoints & points) const -{ - return findNearestIndex(points, current_pose_ptr_->pose); -} - -boost::optional MotionVelocitySmootherNode::findNearestIndex( - const TrajectoryPoints & points, const geometry_msgs::msg::Pose & p) const +size_t MotionVelocitySmootherNode::findNearestIndexFromEgo(const TrajectoryPoints & points) const { - return motion_utils::findNearestIndex( - points, p, std::numeric_limits::max(), node_param_.delta_yaw_threshold); + return motion_utils::findFirstNearestIndexWithSoftConstraints( + points, current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); } bool MotionVelocitySmootherNode::isReverse(const TrajectoryPoints & points) const diff --git a/planning/motion_velocity_smoother/src/resample.cpp b/planning/motion_velocity_smoother/src/resample.cpp index f04a564bbdeda..674234ab287e4 100644 --- a/planning/motion_velocity_smoother/src/resample.cpp +++ b/planning/motion_velocity_smoother/src/resample.cpp @@ -26,16 +26,15 @@ namespace resampling { boost::optional resampleTrajectory( const TrajectoryPoints & input, const double v_current, - const geometry_msgs::msg::Pose & current_pose, const double delta_yaw_threshold, - const ResampleParam & param, const bool use_zoh_for_v) + const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold, + const double nearest_yaw_threshold, const ResampleParam & param, const bool use_zoh_for_v) { // Arc length from the initial point to the closest point - const auto negative_front_arclength_value = motion_utils::calcSignedArcLength( - input, current_pose, 0, std::numeric_limits::max(), delta_yaw_threshold); - if (!negative_front_arclength_value) { - return {}; - } - const auto front_arclength_value = std::fabs(*negative_front_arclength_value); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + input, current_pose, nearest_dist_threshold, nearest_yaw_threshold); + const double negative_front_arclength_value = motion_utils::calcSignedArcLength( + input, current_pose.position, current_seg_idx, input.at(0).pose.position, 0); + const auto front_arclength_value = std::fabs(negative_front_arclength_value); const auto dist_to_closest_stop_point = motion_utils::calcDistanceToForwardStopPoint(input, current_pose); @@ -145,8 +144,8 @@ boost::optional resampleTrajectory( boost::optional resampleTrajectory( const TrajectoryPoints & input, const geometry_msgs::msg::Pose & current_pose, - const double delta_yaw_threshold, const ResampleParam & param, const double nominal_ds, - const bool use_zoh_for_v) + const double nearest_dist_threshold, const double nearest_yaw_threshold, + const ResampleParam & param, const double nominal_ds, const bool use_zoh_for_v) { // input arclength const double trajectory_length = motion_utils::calcArcLength(input); @@ -169,12 +168,12 @@ boost::optional resampleTrajectory( // Step1. Resample front trajectory // Arc length from the initial point to the closest point - const auto negative_front_arclength_value = motion_utils::calcSignedArcLength( - input, current_pose, 0, std::numeric_limits::max(), delta_yaw_threshold); - if (!negative_front_arclength_value) { - return {}; - } - const auto front_arclength_value = std::fabs(*negative_front_arclength_value); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + input, current_pose, nearest_dist_threshold, nearest_yaw_threshold); + const double negative_front_arclength_value = motion_utils::calcSignedArcLength( + input, current_pose.position, current_seg_idx, input.at(0).pose.position, + static_cast(0)); + const auto front_arclength_value = std::fabs(negative_front_arclength_value); for (double s = 0.0; s <= front_arclength_value; s += nominal_ds) { out_arclength.push_back(s); } 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 e32d37b403a15..a43f67b00b53d 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 @@ -234,7 +234,8 @@ bool AnalyticalJerkConstrainedSmoother::apply( boost::optional AnalyticalJerkConstrainedSmoother::resampleTrajectory( const TrajectoryPoints & input, [[maybe_unused]] const double v0, [[maybe_unused]] const geometry_msgs::msg::Pose & current_pose, - [[maybe_unused]] const double delta_yaw_threshold) const + [[maybe_unused]] const double nearest_dist_threshold, + [[maybe_unused]] const double nearest_yaw_threshold) const { TrajectoryPoints output; if (input.empty()) { diff --git a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index 2fcfc1c77fd8a..d3b8d79e2f376 100644 --- a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -100,10 +100,11 @@ bool JerkFilteredSmoother::apply( debug_trajectories[2] = filtered; // Resample TrajectoryPoints for Optimization + // TODO(planning/control team) deal with overlapped lanes with the same direction const auto initial_traj_pose = filtered.front().pose; auto opt_resampled_trajectory = resampling::resampleTrajectory( filtered, v0, initial_traj_pose, std::numeric_limits::max(), - base_param_.resample_param); + std::numeric_limits::max(), base_param_.resample_param); if (!opt_resampled_trajectory) { RCLCPP_WARN(logger_, "Resample failed!"); @@ -471,10 +472,11 @@ TrajectoryPoints JerkFilteredSmoother::mergeFilteredTrajectory( boost::optional JerkFilteredSmoother::resampleTrajectory( const TrajectoryPoints & input, [[maybe_unused]] const double v0, - const geometry_msgs::msg::Pose & current_pose, const double delta_yaw_threshold) const + const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold, + const double nearest_yaw_threshold) const { return resampling::resampleTrajectory( - input, current_pose, delta_yaw_threshold, base_param_.resample_param, + input, current_pose, nearest_dist_threshold, nearest_yaw_threshold, base_param_.resample_param, smoother_param_.jerk_filter_ds); } diff --git a/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp index da3764e307cc7..6b2ae568f0052 100644 --- a/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp @@ -223,10 +223,11 @@ bool L2PseudoJerkSmoother::apply( boost::optional L2PseudoJerkSmoother::resampleTrajectory( const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, - const double delta_yaw_threshold) const + const double nearest_dist_threshold, const double nearest_yaw_threshold) const { return resampling::resampleTrajectory( - input, v0, current_pose, delta_yaw_threshold, base_param_.resample_param); + input, v0, current_pose, nearest_dist_threshold, nearest_yaw_threshold, + base_param_.resample_param); } } // namespace motion_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp index 8030a0ff5c1da..90e72f8b083d8 100644 --- a/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp @@ -235,10 +235,11 @@ bool LinfPseudoJerkSmoother::apply( boost::optional LinfPseudoJerkSmoother::resampleTrajectory( const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, - const double delta_yaw_threshold) const + const double nearest_dist_threshold, const double nearest_yaw_threshold) const { return resampling::resampleTrajectory( - input, v0, current_pose, delta_yaw_threshold, base_param_.resample_param); + input, v0, current_pose, nearest_dist_threshold, nearest_yaw_threshold, + base_param_.resample_param); } } // namespace motion_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/trajectory_utils.cpp b/planning/motion_velocity_smoother/src/trajectory_utils.cpp index c81809cb62e0c..2f84ff48b1669 100644 --- a/planning/motion_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/motion_velocity_smoother/src/trajectory_utils.cpp @@ -58,7 +58,7 @@ inline double integ_v(double v0, double a0, double j0, double t) inline double integ_a(double a0, double j0, double t) { return a0 + j0 * t; } TrajectoryPoint calcInterpolatedTrajectoryPoint( - const TrajectoryPoints & trajectory, const Pose & target_pose) + const TrajectoryPoints & trajectory, const Pose & target_pose, const size_t seg_idx) { TrajectoryPoint traj_p{}; traj_p.pose = target_pose; @@ -75,17 +75,14 @@ TrajectoryPoint calcInterpolatedTrajectoryPoint( return traj_p; } - const size_t segment_idx = - motion_utils::findNearestSegmentIndex(trajectory, target_pose.position); - - auto v1 = getTransVector3(trajectory.at(segment_idx).pose, trajectory.at(segment_idx + 1).pose); - auto v2 = getTransVector3(trajectory.at(segment_idx).pose, target_pose); + auto v1 = getTransVector3(trajectory.at(seg_idx).pose, trajectory.at(seg_idx + 1).pose); + auto v2 = getTransVector3(trajectory.at(seg_idx).pose, target_pose); // calc internal proportion const double prop{std::max(0.0, std::min(1.0, v1.dot(v2) / v1.length2()))}; { - const auto & seg_pt = trajectory.at(segment_idx); - const auto & next_pt = trajectory.at(segment_idx + 1); + const auto & seg_pt = trajectory.at(seg_idx); + const auto & next_pt = trajectory.at(seg_idx + 1); traj_p.pose = tier4_autoware_utils::calcInterpolatedPose(seg_pt.pose, next_pt.pose, prop); traj_p.longitudinal_velocity_mps = interpolation::lerp( seg_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, prop);