feat(motion_velocity_smoother): use resampling function from motion_u…

…tils (autowarefoundation#1489)

* feat(motion_velocity_smoother): use resampling fucntion from motion utils

Signed-off-by: yutaka <purewater0901@gmail.com>

* fix behavior velocity planner

Signed-off-by: yutaka <purewater0901@gmail.com>

planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp

@@ -144,11 +144,10 @@ inline bool smoothPath(
       0, trajectory.size(), external_v_limit->max_velocity, trajectory);
   }
   const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory);
-  auto nearest_idx =
-    motion_utils::findNearestIndex(*traj_lateral_acc_filtered, current_pose.position);
 
   // Resample trajectory with ego-velocity based interval distances
-  auto traj_resampled = smoother->resampleTrajectory(*traj_lateral_acc_filtered, v0, nearest_idx);
+  auto traj_resampled = smoother->resampleTrajectory(
+    *traj_lateral_acc_filtered, v0, current_pose, std::numeric_limits<double>::max());
   const auto traj_resampled_closest = findNearestIndex(*traj_resampled, current_pose, max, M_PI_4);
   if (!traj_resampled_closest) {
     return false;

planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp

@@ -165,8 +165,6 @@ class MotionVelocitySmootherNode : public rclcpp::Node
 
   void updateDataForExternalVelocityLimit();
 
-  boost::optional<TrajectoryPoints> poseResampleTrajectory(const TrajectoryPoints trajectory) const;
-
   AlgorithmType getAlgorithmType(const std::string & algorithm_name) const;
 
   TrajectoryPoints calcTrajectoryVelocity(const TrajectoryPoints & input) const;

planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp

@@ -19,6 +19,7 @@
 #include "motion_velocity_smoother/trajectory_utils.hpp"
 
 #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
+#include <geometry_msgs/msg/pose.hpp>
 
 #include "boost/optional.hpp"
 
@@ -43,12 +44,14 @@ struct ResampleParam
 };
 
 boost::optional<TrajectoryPoints> resampleTrajectory(
-  const TrajectoryPoints & input, const double v_current, const size_t closest_id,
-  const ResampleParam & param);
+  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);
 
 boost::optional<TrajectoryPoints> resampleTrajectory(
-  const TrajectoryPoints & input, const size_t closest_id, const ResampleParam & param,
-  const double nominal_ds);
+  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);
 }  // namespace resampling
 }  // namespace motion_velocity_smoother
 

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

@@ -72,7 +72,9 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase
     TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) override;
 
   boost::optional<TrajectoryPoints> resampleTrajectory(
-    const TrajectoryPoints & input, const double v_current, const int closest_id) const override;
+    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;
 
   boost::optional<TrajectoryPoints> applyLateralAccelerationFilter(
     const TrajectoryPoints & input, [[maybe_unused]] const double v0,

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

@@ -47,7 +47,8 @@ class JerkFilteredSmoother : public SmootherBase
     TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) override;
 
   boost::optional<TrajectoryPoints> resampleTrajectory(
-    const TrajectoryPoints & input, const double v_current, const int closest_id) const override;
+    const TrajectoryPoints & input, [[maybe_unused]] const double v0,
+    const geometry_msgs::msg::Pose & current_pose, const double delta_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

@@ -45,7 +45,8 @@ class L2PseudoJerkSmoother : public SmootherBase
     TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) override;
 
   boost::optional<TrajectoryPoints> resampleTrajectory(
-    const TrajectoryPoints & input, const double v_current, const int closest_id) const override;
+    const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose,
+    const double delta_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

@@ -45,7 +45,8 @@ class LinfPseudoJerkSmoother : public SmootherBase
     TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) override;
 
   boost::optional<TrajectoryPoints> resampleTrajectory(
-    const TrajectoryPoints & input, const double v_current, const int closest_id) const override;
+    const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose,
+    const double delta_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

@@ -57,7 +57,8 @@ class SmootherBase
     TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories) = 0;
 
   virtual boost::optional<TrajectoryPoints> resampleTrajectory(
-    const TrajectoryPoints & input, const double v_current, const int closest_id) const = 0;
+    const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose,
+    const double delta_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

@@ -64,10 +64,6 @@ void applyMaximumVelocityLimit(
 
 boost::optional<size_t> searchZeroVelocityIdx(const TrajectoryPoints & trajectory);
 
-boost::optional<TrajectoryPoints> applyLinearInterpolation(
-  const std::vector<double> & base_index, const TrajectoryPoints & base_trajectory,
-  const std::vector<double> & out_index, const bool use_spline_for_pose = false);
-
 bool calcStopDistWithJerkConstraints(
   const double v0, const double a0, const double jerk_acc, const double jerk_dec,
   const double min_acc, const double target_vel, std::map<double, double> & jerk_profile,

planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp

@@ -376,7 +376,10 @@ void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstShar
     return;
   }
 
-  auto output_resampled = poseResampleTrajectory(output);
+  // 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);
   if (!output_resampled) {
     RCLCPP_WARN(get_logger(), "Failed to get the resampled output trajectory");
     return;
@@ -495,15 +498,9 @@ bool MotionVelocitySmootherNode::smoothVelocity(
   }
 
   // Resample trajectory with ego-velocity based interval distance
-  const auto traj_pre_resampled_closest = findNearestIndexFromEgo(*traj_lateral_acc_filtered);
-  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);
+    current_pose_ptr_->pose, node_param_.delta_yaw_threshold);
   if (!traj_resampled) {
     RCLCPP_WARN(get_logger(), "Fail to do resampling before the optimization");
     return false;
@@ -606,22 +603,6 @@ void MotionVelocitySmootherNode::insertBehindVelocity(
   }
 }
 
-boost::optional<TrajectoryPoints> MotionVelocitySmootherNode::poseResampleTrajectory(
-  const TrajectoryPoints trajectory) const
-{
-  // Get the nearest point
-  const auto closest_idx = findNearestIndexFromEgo(trajectory);
-  if (!closest_idx) {
-    return {};
-  }
-
-  auto trajectory_resampled = resampling::resampleTrajectory(
-    trajectory, current_odometry_ptr_->twist.twist.linear.x, *closest_idx,
-    node_param_.post_resample_param);
-
-  return trajectory_resampled;
-}
-
 void MotionVelocitySmootherNode::publishStopDistance(const TrajectoryPoints & trajectory) const
 {
   const auto closest_optional = findNearestIndexFromEgo(trajectory);

planning/motion_velocity_smoother/src/resample.cpp

@@ -14,6 +14,9 @@
 
 #include "motion_velocity_smoother/resample.hpp"
 
+#include "motion_utils/resample/resample.hpp"
+#include "motion_utils/trajectory/tmp_conversion.hpp"
+
 #include <algorithm>
 #include <vector>
 
@@ -22,23 +25,23 @@ namespace motion_velocity_smoother
 namespace resampling
 {
 boost::optional<TrajectoryPoints> resampleTrajectory(
-  const TrajectoryPoints & input, const double v_current, const size_t closest_id,
-  const ResampleParam & param)
+  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)
 {
   // Arc length from the initial point to the closest point
-  const double front_arclength_value = trajectory_utils::calcArcLength(input, 0, closest_id);
-
-  // Get the nearest point where velocity is zero
-  auto zero_vel_id = motion_utils::searchZeroVelocityIndex(input, closest_id, input.size());
-  // Arc length from the closest point to the point where velocity is zero
-  double zero_vel_arclength_value = param.max_trajectory_length;
-  if (zero_vel_id) {
-    zero_vel_arclength_value = std::min(
-      zero_vel_arclength_value, motion_utils::calcSignedArcLength(input, closest_id, *zero_vel_id));
+  const auto negative_front_arclength_value = motion_utils::calcSignedArcLength(
+    input, current_pose, 0, std::numeric_limits<double>::max(), delta_yaw_threshold);
+  if (!negative_front_arclength_value) {
+    return {};
   }
+  const auto front_arclength_value = std::fabs(*negative_front_arclength_value);
+
+  const auto dist_to_closest_stop_point =
+    motion_utils::calcDistanceToForwardStopPoint(input, current_pose);
 
   // Get the resample size from the closest point
-  const std::vector<double> in_arclength = trajectory_utils::calcArclengthArray(input);
+  const double trajectory_length = motion_utils::calcArcLength(input);
   const double Nt = param.resample_time / std::max(param.dense_resample_dt, 0.001);
   const double ds_nominal =
     std::max(v_current * param.dense_resample_dt, param.dense_min_interval_distance);
@@ -77,7 +80,7 @@ boost::optional<TrajectoryPoints> resampleTrajectory(
     }
 
     // Check if the distance is longer than input arclength
-    if (dist_i + front_arclength_value >= in_arclength.back()) {
+    if (dist_i + front_arclength_value >= trajectory_length) {
       is_endpoint_included = true;  // distance is over input endpoint.
       break;
     }
@@ -95,20 +98,22 @@ boost::optional<TrajectoryPoints> resampleTrajectory(
     }
 
     // Handle Close Stop Point
-    if (dist_i > zero_vel_arclength_value && !is_zero_point_included) {
-      if (std::fabs(dist_i - zero_vel_arclength_value) > 1e-3) {
+    if (
+      dist_to_closest_stop_point && dist_i > *dist_to_closest_stop_point &&
+      !is_zero_point_included) {
+      if (std::fabs(dist_i - *dist_to_closest_stop_point) > 1e-3) {
         // dist_i is much bigger than zero_vel_arclength_value
         if (
           !out_arclength.empty() &&
-          std::fabs(out_arclength.back() - (zero_vel_arclength_value + front_arclength_value)) <
+          std::fabs(out_arclength.back() - (*dist_to_closest_stop_point + front_arclength_value)) <
             1e-3) {
-          out_arclength.back() = zero_vel_arclength_value + front_arclength_value;
+          out_arclength.back() = *dist_to_closest_stop_point + front_arclength_value;
         } else {
-          out_arclength.push_back(zero_vel_arclength_value + front_arclength_value);
+          out_arclength.push_back(*dist_to_closest_stop_point + front_arclength_value);
         }
       } else {
         // dist_i is close to the zero_vel_arclength_value
-        dist_i = zero_vel_arclength_value;
+        dist_i = *dist_to_closest_stop_point;
       }
 
       is_zero_point_included = true;
@@ -117,61 +122,59 @@ boost::optional<TrajectoryPoints> resampleTrajectory(
     out_arclength.push_back(dist_i + front_arclength_value);
   }
 
-  auto output =
-    trajectory_utils::applyLinearInterpolation(in_arclength, input, out_arclength, true);
-  if (!output) {
-    RCLCPP_WARN(
-      rclcpp::get_logger("motion_velocity_smoother").get_child("resample"),
-      "fail trajectory interpolation. size : in_arclength = %lu, "
-      "input = %lu, out_arclength = %lu",
-      in_arclength.size(), input.size(), out_arclength.size());
-    return boost::none;
+  if (input.size() < 2 || out_arclength.size() < 2 || trajectory_length < out_arclength.back()) {
+    return input;
   }
 
+  const auto output_traj = motion_utils::resampleTrajectory(
+    motion_utils::convertToTrajectory(input), out_arclength, false, true, use_zoh_for_v);
+  auto output = motion_utils::convertToTrajectoryPointArray(output_traj);
+
   // add end point directly to consider the endpoint velocity.
   if (is_endpoint_included) {
     constexpr double ep_dist = 1.0E-3;
-    if (tier4_autoware_utils::calcDistance2d(output->back(), input.back()) < ep_dist) {
-      output->back() = input.back();
+    if (tier4_autoware_utils::calcDistance2d(output.back(), input.back()) < ep_dist) {
+      output.back() = input.back();
     } else {
-      output->push_back(input.back());
+      output.push_back(input.back());
     }
   }
 
   return output;
 }
 
 boost::optional<TrajectoryPoints> resampleTrajectory(
-  const TrajectoryPoints & input, const size_t closest_id, const ResampleParam & param,
-  const double nominal_ds)
+  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)
 {
   // input arclength
-  std::vector<double> in_arclength = trajectory_utils::calcArclengthArray(input);
+  const double trajectory_length = motion_utils::calcArcLength(input);
+  const auto dist_to_closest_stop_point =
+    motion_utils::calcDistanceToForwardStopPoint(input, current_pose);
 
-  // Get the nearest point where velocity is zero
-  // to avoid getting closest_id as a stop point, search zero velocity index from closest_id + 1.
-  auto stop_id = motion_utils::searchZeroVelocityIndex(input, closest_id + 1, input.size());
-  // Arc length from the closest point to the point where velocity is zero
+  // distance to stop point
   double stop_arclength_value = param.max_trajectory_length;
-  if (stop_id) {
-    stop_arclength_value = std::min(
-      stop_arclength_value, motion_utils::calcSignedArcLength(input, closest_id, *stop_id));
+  if (dist_to_closest_stop_point) {
+    stop_arclength_value = std::min(*dist_to_closest_stop_point, stop_arclength_value);
   }
-
-  // Do dense resampling before the stop line(3[m] ahead of the stop line)
   if (param.min_trajectory_length < stop_arclength_value) {
     stop_arclength_value = param.min_trajectory_length;
   }
-  if (in_arclength.back() < stop_arclength_value) {
-    stop_arclength_value = in_arclength.back();
-  }
-  double start_stop_arclength_value = std::max(stop_arclength_value - 3.0, 0.0);
+
+  // Do dense resampling before the stop line(3[m] ahead of the stop line)
+  const double start_stop_arclength_value = std::max(stop_arclength_value - 3.0, 0.0);
 
   std::vector<double> out_arclength;
 
   // Step1. Resample front trajectory
   // Arc length from the initial point to the closest point
-  const double front_arclength_value = trajectory_utils::calcArcLength(input, 0, closest_id);
+  const auto negative_front_arclength_value = motion_utils::calcSignedArcLength(
+    input, current_pose, 0, std::numeric_limits<double>::max(), delta_yaw_threshold);
+  if (!negative_front_arclength_value) {
+    return {};
+  }
+  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);
   }
@@ -199,7 +202,7 @@ boost::optional<TrajectoryPoints> resampleTrajectory(
     }
 
     // Check if the distance is longer than input arclength
-    if (dist_i + front_arclength_value >= in_arclength.back()) {
+    if (dist_i + front_arclength_value >= trajectory_length) {
       is_endpoint_included = true;  // distance is over input endpoint.
       break;
     }
@@ -238,23 +241,21 @@ boost::optional<TrajectoryPoints> resampleTrajectory(
     out_arclength.push_back(dist_i + front_arclength_value);
   }
 
-  auto output = trajectory_utils::applyLinearInterpolation(in_arclength, input, out_arclength);
-  if (!output) {
-    RCLCPP_WARN(
-      rclcpp::get_logger("motion_velocity_smoother").get_child("resample"),
-      "fail trajectory interpolation. size : in_arclength = %lu, "
-      "input = %lu, out_arclength = %lu",
-      in_arclength.size(), input.size(), out_arclength.size());
-    return boost::none;
+  if (input.size() < 2 || out_arclength.size() < 2 || trajectory_length < out_arclength.back()) {
+    return input;
   }
 
+  const auto output_traj = motion_utils::resampleTrajectory(
+    motion_utils::convertToTrajectory(input), out_arclength, false, true, use_zoh_for_v);
+  auto output = motion_utils::convertToTrajectoryPointArray(output_traj);
+
   // add end point directly to consider the endpoint velocity.
   if (is_endpoint_included) {
     constexpr double ep_dist = 1.0E-3;
-    if (tier4_autoware_utils::calcDistance2d(output->back(), input.back()) < ep_dist) {
-      output->back() = input.back();
+    if (tier4_autoware_utils::calcDistance2d(output.back(), input.back()) < ep_dist) {
+      output.back() = input.back();
     } else {
-      output->push_back(input.back());
+      output.push_back(input.back());
     }
   }