[pull] main from autowarefoundation:main

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<double>::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);

planning/motion_velocity_smoother/CMakeLists.txt

@@ -25,7 +25,6 @@ set(SMOOTHER_SRC
   src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp
   src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp
   src/trajectory_utils.cpp
-  src/linear_interpolation.cpp
   src/resample.cpp
 )
 

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<Float32Stamped>::SharedPtr pub_closest_merged_velocity_;
 
   // helper functions
-  boost::optional<size_t> findNearestIndex(
-    const TrajectoryPoints & points, const geometry_msgs::msg::Pose & p) const;
-  boost::optional<size_t> 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();

planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp

@@ -45,13 +45,13 @@ struct ResampleParam
 
 boost::optional<TrajectoryPoints> 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<TrajectoryPoints> 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
 

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<TrajectoryPoints> 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<TrajectoryPoints> applyLateralAccelerationFilter(
     const TrajectoryPoints & input, [[maybe_unused]] const double v0,

planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp

@@ -16,7 +16,6 @@
 #define MOTION_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_  // NOLINT
 
 #include "motion_utils/trajectory/trajectory.hpp"
-#include "motion_velocity_smoother/linear_interpolation.hpp"
 #include "rclcpp/rclcpp.hpp"
 
 #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"

planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp

@@ -48,7 +48,8 @@ class JerkFilteredSmoother : public SmootherBase
 
   boost::optional<TrajectoryPoints> 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;

planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp

@@ -46,7 +46,7 @@ class L2PseudoJerkSmoother : public SmootherBase
 
   boost::optional<TrajectoryPoints> 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;

planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp

@@ -46,7 +46,7 @@ class LinfPseudoJerkSmoother : public SmootherBase
 
   boost::optional<TrajectoryPoints> 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;

planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp

@@ -58,7 +58,7 @@ class SmootherBase
 
   virtual boost::optional<TrajectoryPoints> 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<TrajectoryPoints> applyLateralAccelerationFilter(
     const TrajectoryPoints & input, [[maybe_unused]] const double v0 = 0.0,

planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp

@@ -38,7 +38,7 @@ using TrajectoryPoints = std::vector<TrajectoryPoint>;
 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,

planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml

@@ -1,6 +1,7 @@
 <launch>
   <!-- common param -->
   <arg name="common_param_path" default="$(find-pkg-share motion_velocity_smoother)/config/default_common.param.yaml"/>
+  <arg name="nearest_search_param_path"/>
 
   <!-- input/output -->
   <arg name="input_trajectory" default="/planning/scenario_planning/scenario_selector/trajectory"/>
@@ -16,6 +17,7 @@
 
   <node pkg="motion_velocity_smoother" exec="motion_velocity_smoother" name="motion_velocity_smoother" output="screen">
     <param from="$(var common_param_path)"/>
+    <param from="$(var nearest_search_param_path)"/>
     <param from="$(var param_path)"/>
     <param from="$(var smoother_param_path)"/>
     <param name="publish_debug_trajs" value="$(var publish_debug_trajs)"/>