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

…ng find wrong nearest_idx

Signed-off-by: Berkay Karaman <[email protected]>

control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp

@@ -154,6 +154,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 getExtendTrajectoryPoint(
+  const double extend_distance, const TrajectoryPoint & goal_point);
+
 }  // namespace longitudinal_utils
 }  // namespace autoware::motion::control::pid_longitudinal_controller
 

control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp

@@ -183,5 +183,27 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance(
   }
   return p;
 }
+
+TrajectoryPoint getExtendTrajectoryPoint(
+  const double extend_distance, const TrajectoryPoint & goal_point)
+{
+  tf2::Transform map2goal;
+  tf2::fromMsg(goal_point.pose, map2goal);
+  tf2::Transform local_extend_point;
+  local_extend_point.setOrigin(tf2::Vector3(extend_distance, 0.0, 0.0));
+  tf2::Quaternion q;
+  q.setRPY(0, 0, 0);
+  local_extend_point.setRotation(q);
+  const auto map2extend_point = map2goal * local_extend_point;
+  geometry_msgs::msg::Pose extend_pose;
+  tf2::toMsg(map2extend_point, extend_pose);
+  TrajectoryPoint extend_trajectory_point;
+  extend_trajectory_point.pose = extend_pose;
+  extend_trajectory_point.longitudinal_velocity_mps = goal_point.longitudinal_velocity_mps;
+  extend_trajectory_point.lateral_velocity_mps = goal_point.lateral_velocity_mps;
+  extend_trajectory_point.acceleration_mps2 = goal_point.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

@@ -220,8 +220,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::getExtendTrajectoryPoint(
+    virtual_point_distance, m_trajectory.points.back()));
 }
 
 rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallback(