fix(trajectory_follower): add max_dist/max_yaw for nearest index search

control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp

@@ -22,6 +22,7 @@
 #include "motion_common/motion_common.hpp"
 #include "motion_common/trajectory_common.hpp"
 #include "tf2/utils.h"
+#include "tier4_autoware_utils/trajectory/trajectory.hpp"
 #include "trajectory_follower/visibility_control.hpp"
 
 #include <experimental/optional>  // NOLINT
@@ -60,8 +61,9 @@ TRAJECTORY_FOLLOWER_PUBLIC bool8_t isValidTrajectory(const Trajectory & traj);
 /**
  * @brief calculate distance to stopline from current vehicle position where velocity is 0
  */
-TRAJECTORY_FOLLOWER_PUBLIC float64_t
-calcStopDistance(const Point & current_pos, const Trajectory & traj);
+TRAJECTORY_FOLLOWER_PUBLIC float64_t calcStopDistance(
+  const Pose & current_pose, const Trajectory & traj, const float64_t max_dist,
+  const float64_t max_yaw);
 
 /**
  * @brief calculate pitch angle from estimated current pose
@@ -106,21 +108,23 @@ lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const float6
  * @param [in] point Interpolated point is nearest to this point.
  */
 template <class T>
-TRAJECTORY_FOLLOWER_PUBLIC TrajectoryPoint
-lerpTrajectoryPoint(const T & points, const Point & point)
+TRAJECTORY_FOLLOWER_PUBLIC TrajectoryPoint lerpTrajectoryPoint(
+  const T & points, const Pose & pose, const float64_t max_dist, const float64_t max_yaw)
 {
   TrajectoryPoint interpolated_point;
-
-  const size_t nearest_seg_idx = trajectory_common::findNearestSegmentIndex(points, point);
+  auto seg_idx = tier4_autoware_utils::findNearestSegmentIndex(points, pose, max_dist, max_yaw);
+  if (!seg_idx) {  // if not fund idx
+    seg_idx = tier4_autoware_utils::findNearestSegmentIndex(points, pose);
+  }
 
   const float64_t len_to_interpolated =
-    trajectory_common::calcLongitudinalOffsetToSegment(points, nearest_seg_idx, point);
+    tier4_autoware_utils::calcLongitudinalOffsetToSegment(points, *seg_idx, pose.position);
   const float64_t len_segment =
-    trajectory_common::calcSignedArcLength(points, nearest_seg_idx, nearest_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 = nearest_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/package.xml

@@ -27,6 +27,7 @@
   <depend>std_msgs</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
+  <depend>tier4_autoware_utils</depend>
 
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>

control/trajectory_follower/src/longitudinal_controller_utils.cpp

@@ -62,17 +62,31 @@ bool isValidTrajectory(const Trajectory & traj)
   return true;
 }
 
-float64_t calcStopDistance(const Point & current_pos, const Trajectory & traj)
+float64_t calcStopDistance(
+  const Pose & current_pose, const Trajectory & traj, const float64_t max_dist,
+  const float64_t max_yaw)
 {
   const std::experimental::optional<size_t> stop_idx_opt =
     trajectory_common::searchZeroVelocityIndex(traj.points);
 
+  auto seg_idx =
+    tier4_autoware_utils::findNearestSegmentIndex(traj.points, current_pose, max_dist, max_yaw);
+  if (!seg_idx) {  // if not fund idx
+    seg_idx = tier4_autoware_utils::findNearestSegmentIndex(traj.points, current_pose);
+  }
+  const float64_t signed_length_src_offset = tier4_autoware_utils::calcLongitudinalOffsetToSegment(
+    traj.points, *seg_idx, current_pose.position);
+
   // If no zero velocity point, return the length between current_pose to the end of trajectory.
   if (!stop_idx_opt) {
-    return trajectory_common::calcSignedArcLength(traj.points, current_pos, traj.points.size() - 1);
+    float64_t signed_length_on_traj =
+      tier4_autoware_utils::calcSignedArcLength(traj.points, *seg_idx, traj.points.size() - 1);
+    return signed_length_on_traj - signed_length_src_offset;
   }
 
-  return trajectory_common::calcSignedArcLength(traj.points, current_pos, *stop_idx_opt);
+  float64_t signed_length_on_traj =
+    tier4_autoware_utils::calcSignedArcLength(traj.points, *seg_idx, *stop_idx_opt);
+  return signed_length_on_traj - signed_length_src_offset;
 }
 
 float64_t getPitchByPose(const Quaternion & quaternion_msg)

control/trajectory_follower/test/test_longitudinal_controller_utils.cpp

@@ -50,20 +50,26 @@ TEST(TestLongitudinalControllerUtils, calcStopDistance)
 {
   using autoware_auto_planning_msgs::msg::Trajectory;
   using autoware_auto_planning_msgs::msg::TrajectoryPoint;
-  using geometry_msgs::msg::Point;
-  Point current_pos;
-  current_pos.x = 0.0;
-  current_pos.y = 0.0;
+  using geometry_msgs::msg::Pose;
+  Pose current_pose;
+  current_pose.position.x = 0.0;
+  current_pose.position.y = 0.0;
+  current_pose.position.z = 0.0;
   Trajectory traj;
+  double max_dist = 3.0;
+  double max_yaw = 0.7;
   // empty trajectory : exception
-  EXPECT_THROW(longitudinal_utils::calcStopDistance(current_pos, traj), std::invalid_argument);
+  EXPECT_THROW(
+    longitudinal_utils::calcStopDistance(current_pose, traj, max_dist, max_yaw),
+    std::invalid_argument);
   // one point trajectory : exception
   TrajectoryPoint point;
   point.pose.position.x = 0.0;
   point.pose.position.y = 0.0;
   point.longitudinal_velocity_mps = 0.0;
   traj.points.push_back(point);
-  EXPECT_THROW(longitudinal_utils::calcStopDistance(current_pos, traj), std::out_of_range);
+  EXPECT_THROW(
+    longitudinal_utils::calcStopDistance(current_pose, traj, max_dist, max_yaw), std::out_of_range);
   traj.points.clear();
   // non stopping trajectory: stop distance = trajectory length
   point.pose.position.x = 0.0;
@@ -78,7 +84,7 @@ TEST(TestLongitudinalControllerUtils, calcStopDistance)
   point.pose.position.y = 0.0;
   point.longitudinal_velocity_mps = 1.0;
   traj.points.push_back(point);
-  EXPECT_EQ(longitudinal_utils::calcStopDistance(current_pos, traj), 2.0);
+  EXPECT_EQ(longitudinal_utils::calcStopDistance(current_pose, traj, max_dist, max_yaw), 2.0);
   // stopping trajectory: stop distance = length until stopping point
   point.pose.position.x = 3.0;
   point.pose.position.y = 0.0;
@@ -92,7 +98,7 @@ TEST(TestLongitudinalControllerUtils, calcStopDistance)
   point.pose.position.y = 0.0;
   point.longitudinal_velocity_mps = 0.0;
   traj.points.push_back(point);
-  EXPECT_EQ(longitudinal_utils::calcStopDistance(current_pos, traj), 3.0);
+  EXPECT_EQ(longitudinal_utils::calcStopDistance(current_pose, traj, max_dist, max_yaw), 3.0);
 }
 
 TEST(TestLongitudinalControllerUtils, getPitchByPose)
@@ -319,7 +325,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation)
 TEST(TestLongitudinalControllerUtils, lerpTrajectoryPoint)
 {
   using autoware_auto_planning_msgs::msg::TrajectoryPoint;
-  using geometry_msgs::msg::Point;
+  using geometry_msgs::msg::Pose;
   const double abs_err = 1e-15;
   decltype(autoware_auto_planning_msgs::msg::Trajectory::points) points;
   TrajectoryPoint p;
@@ -344,72 +350,73 @@ TEST(TestLongitudinalControllerUtils, lerpTrajectoryPoint)
   p.acceleration_mps2 = 40.0;
   points.push_back(p);
   TrajectoryPoint result;
-  Point point;
-
+  Pose pose;
+  double max_dist = 3.0;
+  double max_yaw = 0.7;
   // Points on the trajectory gives back the original trajectory points values
-  point.x = 0.0;
-  point.y = 0.0;
-  result = longitudinal_utils::lerpTrajectoryPoint(points, point);
-  EXPECT_NEAR(result.pose.position.x, point.x, abs_err);
-  EXPECT_NEAR(result.pose.position.y, point.y, abs_err);
+  pose.position.x = 0.0;
+  pose.position.y = 0.0;
+  result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
+  EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err);
+  EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err);
   EXPECT_NEAR(result.longitudinal_velocity_mps, 10.0, abs_err);
   EXPECT_NEAR(result.acceleration_mps2, 10.0, abs_err);
 
-  point.x = 1.0;
-  point.y = 0.0;
-  result = longitudinal_utils::lerpTrajectoryPoint(points, point);
-  EXPECT_NEAR(result.pose.position.x, point.x, abs_err);
-  EXPECT_NEAR(result.pose.position.y, point.y, abs_err);
+  pose.position.x = 1.0;
+  pose.position.y = 0.0;
+  result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
+  EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err);
+  EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err);
   EXPECT_NEAR(result.longitudinal_velocity_mps, 20.0, abs_err);
   EXPECT_NEAR(result.acceleration_mps2, 20.0, abs_err);
 
-  point.x = 1.0;
-  point.y = 1.0;
-  result = longitudinal_utils::lerpTrajectoryPoint(points, point);
-  EXPECT_NEAR(result.pose.position.x, point.x, abs_err);
-  EXPECT_NEAR(result.pose.position.y, point.y, abs_err);
+  pose.position.x = 1.0;
+  pose.position.y = 1.0;
+  result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
+  EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err);
+  EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err);
   EXPECT_NEAR(result.longitudinal_velocity_mps, 30.0, abs_err);
   EXPECT_NEAR(result.acceleration_mps2, 30.0, abs_err);
 
-  point.x = 2.0;
-  point.y = 1.0;
-  result = longitudinal_utils::lerpTrajectoryPoint(points, point);
-  EXPECT_NEAR(result.pose.position.x, point.x, abs_err);
-  EXPECT_NEAR(result.pose.position.y, point.y, abs_err);
+  pose.position.x = 2.0;
+  pose.position.y = 1.0;
+  result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
+  EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err);
+  EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err);
   EXPECT_NEAR(result.longitudinal_velocity_mps, 40.0, abs_err);
   EXPECT_NEAR(result.acceleration_mps2, 40.0, abs_err);
 
   // Interpolate between trajectory points
-  point.x = 0.5;
-  point.y = 0.0;
-  result = longitudinal_utils::lerpTrajectoryPoint(points, point);
-  EXPECT_NEAR(result.pose.position.x, point.x, abs_err);
-  EXPECT_NEAR(result.pose.position.y, point.y, abs_err);
+  pose.position.x = 0.5;
+  pose.position.y = 0.0;
+  result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
+  EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err);
+  EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err);
   EXPECT_NEAR(result.longitudinal_velocity_mps, 15.0, abs_err);
   EXPECT_NEAR(result.acceleration_mps2, 15.0, abs_err);
-  point.x = 0.75;
-  point.y = 0.0;
-  result = longitudinal_utils::lerpTrajectoryPoint(points, point);
+  pose.position.x = 0.75;
+  pose.position.y = 0.0;
+  result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
 
-  EXPECT_NEAR(result.pose.position.x, point.x, abs_err);
-  EXPECT_NEAR(result.pose.position.y, point.y, abs_err);
+  EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err);
+  EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err);
   EXPECT_NEAR(result.longitudinal_velocity_mps, 17.5, abs_err);
   EXPECT_NEAR(result.acceleration_mps2, 17.5, abs_err);
 
   // Interpolate away from the trajectory (interpolated point is projected)
-  point.x = 0.5;
-  point.y = -1.0;
-  result = longitudinal_utils::lerpTrajectoryPoint(points, point);
-  EXPECT_NEAR(result.pose.position.x, point.x, abs_err);
+  pose.position.x = 0.5;
+  pose.position.y = -1.0;
+  result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
+  EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err);
   EXPECT_NEAR(result.pose.position.y, 0.0, abs_err);
   EXPECT_NEAR(result.longitudinal_velocity_mps, 15.0, abs_err);
   EXPECT_NEAR(result.acceleration_mps2, 15.0, abs_err);
 
   // Ambiguous projections: possibility with the lowest index is used
-  point.x = 0.5;
-  point.y = 0.5;
-  result = longitudinal_utils::lerpTrajectoryPoint(points, point);
-  EXPECT_NEAR(result.pose.position.x, point.x, abs_err);
+  pose.position.x = 0.5;
+  pose.position.y = 0.5;
+  result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
+  EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err);
   EXPECT_NEAR(result.pose.position.y, 0.0, abs_err);
   EXPECT_NEAR(result.longitudinal_velocity_mps, 15.0, abs_err);
   EXPECT_NEAR(result.acceleration_mps2, 15.0, abs_err);

control/trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp

@@ -326,7 +326,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC LongitudinalController : public rclcpp::Node
    */
   autoware_auto_planning_msgs::msg::TrajectoryPoint calcInterpolatedTargetValue(
     const autoware_auto_planning_msgs::msg::Trajectory & traj,
-    const geometry_msgs::msg::Point & point, const size_t nearest_idx) const;
+    const geometry_msgs::msg::Pose & pose, const size_t nearest_idx) const;
 
   /**
    * @brief calculate predicted velocity after time delay based on past control commands

control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp

@@ -425,7 +425,7 @@ LongitudinalController::ControlData LongitudinalController::getControlData(
 
   // distance to stopline
   control_data.stop_dist = trajectory_follower::longitudinal_utils::calcStopDistance(
-    current_pose.position, *m_trajectory_ptr);
+    current_pose, *m_trajectory_ptr, max_dist, max_yaw);
 
   // pitch
   const float64_t raw_pitch =
@@ -553,7 +553,7 @@ LongitudinalController::Motion LongitudinalController::calcCtrlCmd(
     const auto target_pose = trajectory_follower::longitudinal_utils::calcPoseAfterTimeDelay(
       current_pose, m_delay_compensation_time, current_vel);
     const auto target_interpolated_point =
-      calcInterpolatedTargetValue(*m_trajectory_ptr, target_pose.position, nearest_idx);
+      calcInterpolatedTargetValue(*m_trajectory_ptr, target_pose, nearest_idx);
     target_motion = Motion{
       target_interpolated_point.longitudinal_velocity_mps,
       target_interpolated_point.acceleration_mps2};
@@ -800,8 +800,8 @@ LongitudinalController::Motion LongitudinalController::keepBrakeBeforeStop(
 
 autoware_auto_planning_msgs::msg::TrajectoryPoint
 LongitudinalController::calcInterpolatedTargetValue(
-  const autoware_auto_planning_msgs::msg::Trajectory & traj,
-  const geometry_msgs::msg::Point & point, const size_t nearest_idx) const
+  const autoware_auto_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose,
+  const size_t nearest_idx) const
 {
   if (traj.points.size() == 1) {
     return traj.points.at(0);
@@ -810,18 +810,21 @@ LongitudinalController::calcInterpolatedTargetValue(
   // If the current position is not within the reference trajectory, enable the edge value.
   // Else, apply linear interpolation
   if (nearest_idx == 0) {
-    if (motion_common::calcSignedArcLength(traj.points, point, 0) > 0) {
+    if (motion_common::calcSignedArcLength(traj.points, pose.position, 0) > 0) {
       return traj.points.at(0);
     }
   }
   if (nearest_idx == traj.points.size() - 1) {
-    if (motion_common::calcSignedArcLength(traj.points, point, traj.points.size() - 1) < 0) {
+    if (
+      motion_common::calcSignedArcLength(traj.points, pose.position, traj.points.size() - 1) < 0) {
       return traj.points.at(traj.points.size() - 1);
     }
   }
 
   // apply linear interpolation
-  return trajectory_follower::longitudinal_utils::lerpTrajectoryPoint(traj.points, point);
+  return trajectory_follower::longitudinal_utils::lerpTrajectoryPoint(
+    traj.points, pose, m_state_transition_params.emergency_state_traj_trans_dev,
+    m_state_transition_params.emergency_state_traj_rot_dev);
 }
 
 float64_t LongitudinalController::predictedVelocityInTargetPoint(
@@ -920,7 +923,7 @@ void LongitudinalController::updateDebugVelAcc(
   const size_t nearest_idx = control_data.nearest_idx;
 
   const auto interpolated_point =
-    calcInterpolatedTargetValue(*m_trajectory_ptr, current_pose.position, nearest_idx);
+    calcInterpolatedTargetValue(*m_trajectory_ptr, current_pose, nearest_idx);
 
   m_debug_values.setValues(DebugValues::TYPE::CURRENT_VEL, current_vel);
   m_debug_values.setValues(DebugValues::TYPE::TARGET_VEL, target_motion.vel);