fix(motion_velocity_smoother): use common ego nearest search (autowar…

…efoundation#1584) * fix(motion_velocity_smoother): use common ego nearest search Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix build error and modify launch Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
kenji-miyake · Aug 31, 2022 · f1a8851 · f1a8851
1 parent 5c64f8a
commit f1a8851
Show file tree

Hide file tree

Showing 17 changed files with 128 additions and 110 deletions.
diff --git 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<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);

diff --git a/...tion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/...tion_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();

diff --git 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<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
 

diff --git a/...er/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/...er/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,

diff --git a/...on_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp b/...on_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;

diff --git a/...n_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/...n_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;

diff --git a/...velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/...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;

diff --git a/...ning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp b/...ning/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,

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
@@ -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,

diff --git 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 @@
 <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)"/>