fix(pid_longitudinal_controller): add virtual last point to avoid wro…

…ng find wrong nearest_idx

Signed-off-by: Berkay Karaman <brkay54@gmail.com>

control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp

@@ -152,6 +152,14 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance(
   const size_t src_idx, const double distance,
   const autoware_auto_planning_msgs::msg::Trajectory & trajectory);
 
+/**
+ * @brief calculates the translated position of the goal point with respect to extend_distance
+ * @param [in] extend_distance current index
+ * @param [in] goal_point distance to project
+ */
+TrajectoryPoint getExtendedTrajectoryPoint(
+  const double extend_distance, const Trajectory & trajectory);
+
 }  // namespace longitudinal_utils
 }  // namespace autoware::motion::control::pid_longitudinal_controller
 

control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp

@@ -186,5 +186,35 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance(
   }
   return p;
 }
+
+TrajectoryPoint getExtendedTrajectoryPoint(
+  const double extend_distance, const Trajectory & trajectory)
+{
+  constexpr double eps = 1e-3;
+
+  const auto extend_pose =
+    tier4_autoware_utils::calcOffsetPose(trajectory.points.back().pose, extend_distance, 0.0, 0.0);
+
+  // To avoid 0 slope, extend the point with the same slope as the last two points
+  const double z_change_ratio =
+    (trajectory.points.at(trajectory.points.size() - 2).pose.position.z -
+     trajectory.points.back().pose.position.z) /
+    std::max(
+      tier4_autoware_utils::calcDistance2d(
+        trajectory.points.at(trajectory.points.size() - 2).pose.position,
+        trajectory.points.back().pose.position),
+      eps);
+  TrajectoryPoint extend_trajectory_point;
+  extend_trajectory_point.pose = extend_pose;
+
+  extend_trajectory_point.pose.position.z =
+    trajectory.points.back().pose.position.z - extend_distance * z_change_ratio;
+  extend_trajectory_point.longitudinal_velocity_mps =
+    trajectory.points.back().longitudinal_velocity_mps;
+  extend_trajectory_point.lateral_velocity_mps = trajectory.points.back().lateral_velocity_mps;
+  extend_trajectory_point.acceleration_mps2 = trajectory.points.back().acceleration_mps2;
+  return extend_trajectory_point;
+}
+
 }  // namespace longitudinal_utils
 }  // namespace autoware::motion::control::pid_longitudinal_controller

control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

@@ -241,8 +241,12 @@ void PidLongitudinalController::setTrajectory(
     RCLCPP_WARN_THROTTLE(logger_, *clock_, 3000, "Unexpected trajectory size < 2. Ignored.");
     return;
   }
-
+  // If the vehicle pass the last point of trajectory, it causes errors on control_data calculation.
+  // To handle this, we add a virtual point after the last point.
+  constexpr double virtual_point_distance = 5.0;
   m_trajectory = msg;
+  m_trajectory.points.push_back(
+    longitudinal_utils::getExtendedTrajectoryPoint(virtual_point_distance, m_trajectory));
 }
 
 rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallback(

control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp

@@ -562,3 +562,46 @@ TEST(TestLongitudinalControllerUtils, findTrajectoryPoseAfterDistance)
   EXPECT_NEAR(result.position.y, 1.0, abs_err);
   EXPECT_NEAR(result.position.z, 2.0, abs_err);
 }
+
+TEST(TestLongitudinalControllerUtils, getExtendedTrajectoryPoint)
+{
+  using autoware_auto_planning_msgs::msg::Trajectory;
+  using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+  const double abs_err = 1e-5;
+  Trajectory traj;
+  TrajectoryPoint point;
+  point.pose.position.x = 0.0;
+  point.pose.position.y = 0.0;
+  point.pose.position.z = 0.0;
+  traj.points.push_back(point);
+  point.pose.position.x = 1.0;
+  point.pose.position.y = 0.0;
+  point.pose.position.z = 0.0;
+  traj.points.push_back(point);
+  point.pose.position.x = 1.0;
+  point.pose.position.y = 1.0;
+  point.pose.position.z = 1.0;
+  traj.points.push_back(point);
+  point.pose.position.x = 2.0;
+  point.pose.position.y = 1.0;
+  point.pose.position.z = 2.0;
+  traj.points.push_back(point);
+  double extend_distance = 0.0;
+  TrajectoryPoint result = longitudinal_utils::getExtendedTrajectoryPoint(extend_distance, traj);
+  EXPECT_NEAR(result.pose.position.x, 2.0, abs_err);
+  EXPECT_NEAR(result.pose.position.y, 1.0, abs_err);
+  EXPECT_NEAR(result.pose.position.z, 2.0, abs_err);
+  EXPECT_NEAR(result.longitudinal_velocity_mps, 0.0, abs_err);
+  extend_distance = 1.0;
+  result = longitudinal_utils::getExtendedTrajectoryPoint(extend_distance, traj);
+  EXPECT_NEAR(result.pose.position.x, 3.0, abs_err);
+  EXPECT_NEAR(result.pose.position.y, 1.0, abs_err);
+  EXPECT_NEAR(result.pose.position.z, 3.0, abs_err);
+  EXPECT_NEAR(result.longitudinal_velocity_mps, 0.0, abs_err);
+  extend_distance = 2.0;
+  result = longitudinal_utils::getExtendedTrajectoryPoint(extend_distance, traj);
+  EXPECT_NEAR(result.pose.position.x, 4.0, abs_err);
+  EXPECT_NEAR(result.pose.position.y, 1.0, abs_err);
+  EXPECT_NEAR(result.pose.position.z, 4.0, abs_err);
+  EXPECT_NEAR(result.longitudinal_velocity_mps, 0.0, abs_err);
+}