From cc4a4f6244ad9d86ab7d4df836d7e1397166cebe Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Fri, 10 Nov 2023 18:41:13 +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 | 18 ++++++++++++++++++ .../src/pid_longitudinal_controller.cpp | 6 +++++- 3 files changed, 31 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 d0d012633b4c0..cf9f26bbec906 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 @@ -158,6 +158,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 71ef37f241954..ac78320227e63 100644 --- a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -196,5 +196,23 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance( } return p; } + +TrajectoryPoint getExtendTrajectoryPoint( + const double extend_distance, const TrajectoryPoint & goal_point) +{ + const auto extend_pose = + tier4_autoware_utils::calcOffsetPose(goal_point.pose, extend_distance, 0.0, 0.0); + + TrajectoryPoint extend_trajectory_point; + extend_trajectory_point.pose = extend_pose; + // TODO[someone]: Transform functions creates point at inverse Z direction for non-zero pitch + // value + extend_trajectory_point.pose.position.z = 2 * goal_point.pose.position.z - extend_pose.position.z; + 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 9aab6b319f805..2ba7742aad4a0 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/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::getExtendTrajectoryPoint( + virtual_point_distance, m_trajectory.points.back())); } rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallback(