fix(velocity_controller, mpc_follower): use common ego nearest search

control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp

@@ -112,19 +112,17 @@ TRAJECTORY_FOLLOWER_PUBLIC TrajectoryPoint lerpTrajectoryPoint(
   const T & points, const Pose & pose, const float64_t max_dist, const float64_t max_yaw)
 {
   TrajectoryPoint interpolated_point;
-  auto seg_idx = motion_utils::findNearestSegmentIndex(points, pose, max_dist, max_yaw);
-  if (!seg_idx) {  // if not fund idx
-    seg_idx = motion_utils::findNearestSegmentIndex(points, pose);
-  }
+  const size_t seg_idx =
+    motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(points, pose, max_dist, max_yaw);
 
   const float64_t len_to_interpolated =
-    motion_utils::calcLongitudinalOffsetToSegment(points, *seg_idx, pose.position);
+    motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, pose.position);
   const float64_t len_segment =
-    trajectory_common::calcSignedArcLength(points, *seg_idx, *seg_idx + 1);
+    trajectory_common::calcSignedArcLength(points, seg_idx, seg_idx + 1);
   const float64_t interpolate_ratio = std::clamp(len_to_interpolated / len_segment, 0.0, 1.0);
 
   {
-    const size_t i = *seg_idx;
+    const size_t i = seg_idx;
 
     interpolated_point.pose.position.x = motion_common::interpolate(
       points.at(i).pose.position.x, points.at(i + 1).pose.position.x, interpolate_ratio);

control/trajectory_follower/include/trajectory_follower/mpc.hpp

@@ -365,6 +365,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC
   /* parameters for path smoothing */
   //!< @brief flag to use predicted steer, not measured steer.
   bool8_t m_use_steer_prediction;
+  //!< @brief parameters for nearest index search
+  double ego_nearest_dist_threshold;
+  double ego_nearest_yaw_threshold;
 
   /**
    * @brief constructor

control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp

@@ -138,6 +138,10 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController
   tf2::BufferCore m_tf_buffer{tf2::BUFFER_CORE_DEFAULT_CACHE_TIME};
   tf2_ros::TransformListener m_tf_listener{m_tf_buffer};
 
+  // ego nearest index search
+  double ego_nearest_dist_threshold_;
+  double ego_nearest_yaw_threshold_;
+
   //!< initialize timer to work in real, simulation, and replay
   void initTimer(float64_t period_s);
   /**

control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp

@@ -176,27 +176,11 @@ TRAJECTORY_FOLLOWER_PUBLIC std::vector<float64_t> calcTrajectoryCurvature(
 TRAJECTORY_FOLLOWER_PUBLIC bool8_t calcNearestPoseInterp(
   const MPCTrajectory & traj, const geometry_msgs::msg::Pose & self_pose,
   geometry_msgs::msg::Pose * nearest_pose, size_t * nearest_index, float64_t * nearest_time,
-  const rclcpp::Logger & logger, rclcpp::Clock & clock);
-/**
- * @brief calculate the index of the trajectory point nearest to the given pose
- * @param [in] traj trajectory to search for the point nearest to the pose
- * @param [in] self_pose pose for which to search the nearest trajectory point
- * @return index of the input trajectory nearest to the pose
- */
-TRAJECTORY_FOLLOWER_PUBLIC int64_t
-calcNearestIndex(const MPCTrajectory & traj, const geometry_msgs::msg::Pose & self_pose);
-/**
- * @brief calculate the index of the trajectory point nearest to the given pose
- * @param [in] traj trajectory to search for the point nearest to the pose
- * @param [in] self_pose pose for which to search the nearest trajectory point
- * @return index of the input trajectory nearest to the pose
- */
-TRAJECTORY_FOLLOWER_PUBLIC int64_t calcNearestIndex(
-  const autoware_auto_planning_msgs::msg::Trajectory & traj,
-  const geometry_msgs::msg::Pose & self_pose);
-/**
- * @brief calculate distance to stopped point
- */
+  const double max_dist, const double max_yaw, const rclcpp::Logger & logger,
+  rclcpp::Clock & clock);
+// /**
+//  * @brief calculate distance to stopped point
+//  */
 TRAJECTORY_FOLLOWER_PUBLIC float64_t calcStopDistance(
   const autoware_auto_planning_msgs::msg::Trajectory & current_trajectory, const int64_t origin);
 }  // namespace MPCUtils

control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp

@@ -186,6 +186,10 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal
   float64_t m_max_pitch_rad;
   float64_t m_min_pitch_rad;
 
+  // ego nearest index search
+  double ego_nearest_dist_threshold_;
+  double ego_nearest_yaw_threshold_;
+
   // 1st order lowpass filter for acceleration
   std::shared_ptr<trajectory_follower::LowpassFilter1d> m_lpf_acc{nullptr};
 
@@ -331,7 +335,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal
    */
   autoware_auto_planning_msgs::msg::TrajectoryPoint calcInterpolatedTargetValue(
     const autoware_auto_planning_msgs::msg::Trajectory & traj,
-    const geometry_msgs::msg::Pose & pose, const size_t nearest_idx) const;
+    const geometry_msgs::msg::Pose & pose) const;
 
   /**
    * @brief calculate predicted velocity after time delay based on past control commands

control/trajectory_follower/src/longitudinal_controller_utils.cpp

@@ -69,28 +69,17 @@ float64_t calcStopDistance(
   const std::experimental::optional<size_t> stop_idx_opt =
     trajectory_common::searchZeroVelocityIndex(traj.points);
 
-  auto seg_idx =
-    motion_utils::findNearestSegmentIndex(traj.points, current_pose, max_dist, max_yaw);
-  if (!seg_idx) {  // if not fund idx
-    seg_idx = motion_utils::findNearestSegmentIndex(traj.points, current_pose);
-  }
-  const float64_t signed_length_src_offset =
-    motion_utils::calcLongitudinalOffsetToSegment(traj.points, *seg_idx, current_pose.position);
-
-  if (std::isnan(signed_length_src_offset)) {
+  const size_t end_idx = stop_idx_opt ? *stop_idx_opt : traj.points.size() - 1;
+  const size_t seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
+    traj.points, current_pose, max_dist, max_yaw);
+  const float64_t signed_length_on_traj = motion_utils::calcSignedArcLength(
+    traj.points, current_pose.position, seg_idx, traj.points.at(end_idx).pose.position,
+    std::min(end_idx, traj.points.size() - 2));
+
+  if (std::isnan(signed_length_on_traj)) {
     return 0.0;
   }
-
-  // If no zero velocity point, return the length between current_pose to the end of trajectory.
-  if (!stop_idx_opt) {
-    float64_t signed_length_on_traj =
-      motion_utils::calcSignedArcLength(traj.points, *seg_idx, traj.points.size() - 1);
-    return signed_length_on_traj - signed_length_src_offset;
-  }
-
-  float64_t signed_length_on_traj =
-    motion_utils::calcSignedArcLength(traj.points, *seg_idx, *stop_idx_opt);
-  return signed_length_on_traj - signed_length_src_offset;
+  return signed_length_on_traj;
 }
 
 float64_t getPitchByPose(const Quaternion & quaternion_msg)

control/trajectory_follower/src/mpc.cpp

@@ -264,8 +264,8 @@ bool8_t MPC::getData(
   static constexpr auto duration = 5000 /*ms*/;
   size_t nearest_idx;
   if (!trajectory_follower::MPCUtils::calcNearestPoseInterp(
-        traj, current_pose, &(data->nearest_pose), &(nearest_idx), &(data->nearest_time), m_logger,
-        *m_clock)) {
+        traj, current_pose, &(data->nearest_pose), &(nearest_idx), &(data->nearest_time),
+        ego_nearest_dist_threshold, ego_nearest_yaw_threshold, m_logger, *m_clock)) {
     // reset previous MPC result
     // Note: When a large deviation from the trajectory occurs, the optimization stops and
     // the vehicle will return to the path by re-planning the trajectory or external operation.
@@ -490,7 +490,11 @@ trajectory_follower::MPCTrajectory MPC::applyVelocityDynamicsFilter(
   const trajectory_follower::MPCTrajectory & input, const geometry_msgs::msg::Pose & current_pose,
   const float64_t v0) const
 {
-  int64_t nearest_idx = trajectory_follower::MPCUtils::calcNearestIndex(input, current_pose);
+  autoware_auto_planning_msgs::msg::Trajectory autoware_traj;
+  autoware::motion::control::trajectory_follower::MPCUtils::convertToAutowareTrajectory(
+    input, autoware_traj);
+  const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
+    autoware_traj.points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold);
   if (nearest_idx < 0) {
     return input;
   }
@@ -783,8 +787,11 @@ float64_t MPC::getPredictionDeletaTime(
   const geometry_msgs::msg::Pose & current_pose) const
 {
   // Calculate the time min_prediction_length ahead from current_pose
-  const size_t nearest_idx =
-    static_cast<size_t>(trajectory_follower::MPCUtils::calcNearestIndex(input, current_pose));
+  autoware_auto_planning_msgs::msg::Trajectory autoware_traj;
+  autoware::motion::control::trajectory_follower::MPCUtils::convertToAutowareTrajectory(
+    input, autoware_traj);
+  const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
+    autoware_traj.points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold);
   float64_t sum_dist = 0;
   const float64_t target_time = [&]() {
     const float64_t t_ext =

control/trajectory_follower/src/mpc_lateral_controller.cpp

@@ -14,6 +14,7 @@
 
 #include "trajectory_follower/mpc_lateral_controller.hpp"
 
+#include "motion_utils/motion_utils.hpp"
 #include "tf2_ros/create_timer_ros.h"
 
 #include <algorithm>
@@ -140,6 +141,18 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node}
     m_mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz);
   }
 
+  // ego nearest index search
+  ego_nearest_dist_threshold_ =
+    node_->has_parameter("ego_nearest_dist_threshold")
+      ? node_->get_parameter("ego_nearest_dist_threshold").as_double()
+      : node_->declare_parameter<double>("ego_nearest_dist_threshold");  // [m]
+  ego_nearest_yaw_threshold_ =
+    node_->has_parameter("ego_nearest_yaw_threshold")
+      ? node_->get_parameter("ego_nearest_yaw_threshold").as_double()
+      : node_->declare_parameter<double>("ego_nearest_yaw_threshold");  // [rad]
+  m_mpc.ego_nearest_dist_threshold = ego_nearest_dist_threshold_;
+  m_mpc.ego_nearest_yaw_threshold = ego_nearest_yaw_threshold_;
+
   m_pub_predicted_traj = node_->create_publisher<autoware_auto_planning_msgs::msg::Trajectory>(
     "~/output/predicted_trajectory", 1);
   m_pub_diagnostic =
@@ -356,17 +369,18 @@ MpcLateralController::getInitialControlCommand() const
 
 bool8_t MpcLateralController::isStoppedState() const
 {
+  // If the nearest index is not found, return false
+  if (m_current_trajectory_ptr->points.empty()) {
+    return false;
+  }
+
   // Note: This function used to take into account the distance to the stop line
   // for the stop state judgement. However, it has been removed since the steering
   // control was turned off when approaching/exceeding the stop line on a curve or
   // emergency stop situation and it caused large tracking error.
-  const int64_t nearest = trajectory_follower::MPCUtils::calcNearestIndex(
-    *m_current_trajectory_ptr, m_current_pose_ptr->pose);
-
-  // If the nearest index is not found, return false
-  if (nearest < 0) {
-    return false;
-  }
+  const size_t nearest = motion_utils::findFirstNearestIndexWithSoftConstraints(
+    m_current_trajectory_ptr->points, m_current_pose_ptr->pose, ego_nearest_dist_threshold_,
+    ego_nearest_yaw_threshold_);
 
   const float64_t current_vel = m_current_odometry_ptr->twist.twist.linear.x;
   const float64_t target_vel =

control/trajectory_follower/src/mpc_utils.cpp

@@ -14,6 +14,8 @@
 
 #include "trajectory_follower/mpc_utils.hpp"
 
+#include "motion_utils/motion_utils.hpp"
+
 #include <algorithm>
 #include <limits>
 #include <string>
@@ -296,84 +298,29 @@ void dynamicSmoothingVelocity(
   calcMPCTrajectoryTime(traj);
 }
 
-int64_t calcNearestIndex(const MPCTrajectory & traj, const geometry_msgs::msg::Pose & self_pose)
-{
-  if (traj.empty()) {
-    return -1;
-  }
-  const float64_t my_yaw = ::motion::motion_common::to_angle(self_pose.orientation);
-  int64_t nearest_idx = -1;
-  float64_t min_dist_squared = std::numeric_limits<float64_t>::max();
-  for (size_t i = 0; i < traj.size(); ++i) {
-    const float64_t dx = self_pose.position.x - traj.x[i];
-    const float64_t dy = self_pose.position.y - traj.y[i];
-    const float64_t dist_squared = dx * dx + dy * dy;
-
-    /* ignore when yaw error is large, for crossing path */
-    const float64_t err_yaw = autoware::common::helper_functions::wrap_angle(my_yaw - traj.yaw[i]);
-    if (std::fabs(err_yaw) > (M_PI / 3.0)) {
-      continue;
-    }
-    if (dist_squared < min_dist_squared) {
-      min_dist_squared = dist_squared;
-      nearest_idx = static_cast<int64_t>(i);
-    }
-  }
-  return nearest_idx;
-}
-
-int64_t calcNearestIndex(
-  const autoware_auto_planning_msgs::msg::Trajectory & traj,
-  const geometry_msgs::msg::Pose & self_pose)
-{
-  if (traj.points.empty()) {
-    return -1;
-  }
-  const float64_t my_yaw = ::motion::motion_common::to_angle(self_pose.orientation);
-  int64_t nearest_idx = -1;
-  float64_t min_dist_squared = std::numeric_limits<float64_t>::max();
-  for (size_t i = 0; i < traj.points.size(); ++i) {
-    const float64_t dx =
-      self_pose.position.x - static_cast<float64_t>(traj.points.at(i).pose.position.x);
-    const float64_t dy =
-      self_pose.position.y - static_cast<float64_t>(traj.points.at(i).pose.position.y);
-    const float64_t dist_squared = dx * dx + dy * dy;
-
-    /* ignore when yaw error is large, for crossing path */
-    const float64_t traj_yaw =
-      ::motion::motion_common::to_angle(traj.points.at(i).pose.orientation);
-    const float64_t err_yaw = autoware::common::helper_functions::wrap_angle(my_yaw - traj_yaw);
-    if (std::fabs(err_yaw) > (M_PI / 3.0)) {
-      continue;
-    }
-    if (dist_squared < min_dist_squared) {
-      min_dist_squared = dist_squared;
-      nearest_idx = static_cast<int64_t>(i);
-    }
-  }
-  return nearest_idx;
-}
-
 bool8_t calcNearestPoseInterp(
   const MPCTrajectory & traj, const geometry_msgs::msg::Pose & self_pose,
   geometry_msgs::msg::Pose * nearest_pose, size_t * nearest_index, float64_t * nearest_time,
-  const rclcpp::Logger & logger, rclcpp::Clock & clock)
+  const double max_dist, const double max_yaw, const rclcpp::Logger & logger, rclcpp::Clock & clock)
 {
   if (traj.empty() || !nearest_pose || !nearest_index || !nearest_time) {
     return false;
   }
-  const int64_t nearest_idx = calcNearestIndex(traj, self_pose);
-  if (nearest_idx == -1) {
+
+  autoware_auto_planning_msgs::msg::Trajectory autoware_traj;
+  convertToAutowareTrajectory(traj, autoware_traj);
+  if (autoware_traj.points.empty()) {
     RCLCPP_WARN_SKIPFIRST_THROTTLE(
-      logger, clock, 5000, "[calcNearestPoseInterp] fail to get nearest. traj.size = %zu",
-      traj.size());
+      logger, clock, 5000,
+      "[calcNearestPoseInterp] fail to get nearest. autoware_traj.points.size = %zu",
+      autoware_traj.points.size());
     return false;
   }
 
+  *nearest_index = motion_utils::findFirstNearestIndexWithSoftConstraints(
+    autoware_traj.points, self_pose, max_dist, max_yaw);
   const int64_t traj_size = static_cast<int64_t>(traj.size());
 
-  *nearest_index = static_cast<size_t>(nearest_idx);
-
   if (traj.size() == 1) {
     nearest_pose->position.x = traj.x[*nearest_index];
     nearest_pose->position.y = traj.y[*nearest_index];
@@ -390,8 +337,10 @@ bool8_t calcNearestPoseInterp(
     };
 
   /* get second nearest index = next to nearest_index */
-  const size_t next = static_cast<size_t>(std::min(nearest_idx + 1, traj_size - 1));
-  const size_t prev = static_cast<size_t>(std::max(nearest_idx - 1, int64_t(0)));
+  const size_t next =
+    static_cast<size_t>(std::min(static_cast<int64_t>(*nearest_index) + 1, traj_size - 1));
+  const size_t prev =
+    static_cast<size_t>(std::max(static_cast<int64_t>(*nearest_index) - 1, int64_t(0)));
   const float64_t dist_to_next = calcSquaredDist(self_pose, traj, next);
   const float64_t dist_to_prev = calcSquaredDist(self_pose, traj, prev);
   const size_t second_nearest_index = (dist_to_next < dist_to_prev) ? next : prev;