From 3ea1a92f9070c2a5a4ab898d132e78f6cca1d6ab Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Mon, 18 Sep 2023 14:40:01 +0300 Subject: [PATCH] fix(pid_longitudinal_controller): add virtual last point to avoid wrong find wrong nearest_idx Signed-off-by: Berkay Karaman --- .../longitudinal_controller_utils.hpp | 8 +++++++ .../src/longitudinal_controller_utils.cpp | 22 +++++++++++++++++++ .../src/pid_longitudinal_controller.cpp | 6 ++++- 3 files changed, 35 insertions(+), 1 deletion(-) diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp index 3dd8feed11426..664334f0dcfef 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp @@ -155,6 +155,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 diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 73f9a56a4f6bb..52d6d4986a4d2 100644 --- a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/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 diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index f69127380b23d..fcefd307607b6 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/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(