Skip to content

Commit fe1fdab

Browse files
committed
fix(pid_longitudinal_controller): add virtual last point to avoid wrong find wrong nearest_idx
Signed-off-by: Berkay Karaman <brkay54@gmail.com>
1 parent 0befda2 commit fe1fdab

File tree

3 files changed

+35
-1
lines changed

3 files changed

+35
-1
lines changed

control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp

+8
Original file line numberDiff line numberDiff line change
@@ -158,6 +158,14 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance(
158158
const size_t src_idx, const double distance,
159159
const autoware_auto_planning_msgs::msg::Trajectory & trajectory);
160160

161+
/**
162+
* @brief calculates the translated position of the goal point with respect to extend_distance
163+
* @param [in] extend_distance current index
164+
* @param [in] goal_point distance to project
165+
*/
166+
TrajectoryPoint getExtendTrajectoryPoint(
167+
const double extend_distance, const TrajectoryPoint & goal_point);
168+
161169
} // namespace longitudinal_utils
162170
} // namespace autoware::motion::control::pid_longitudinal_controller
163171

control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp

+22
Original file line numberDiff line numberDiff line change
@@ -202,5 +202,27 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance(
202202
}
203203
return p;
204204
}
205+
206+
TrajectoryPoint getExtendTrajectoryPoint(
207+
const double extend_distance, const TrajectoryPoint & goal_point)
208+
{
209+
tf2::Transform map2goal;
210+
tf2::fromMsg(goal_point.pose, map2goal);
211+
tf2::Transform local_extend_point;
212+
local_extend_point.setOrigin(tf2::Vector3(extend_distance, 0.0, 0.0));
213+
tf2::Quaternion q;
214+
q.setRPY(0, 0, 0);
215+
local_extend_point.setRotation(q);
216+
const auto map2extend_point = map2goal * local_extend_point;
217+
geometry_msgs::msg::Pose extend_pose;
218+
tf2::toMsg(map2extend_point, extend_pose);
219+
TrajectoryPoint extend_trajectory_point;
220+
extend_trajectory_point.pose = extend_pose;
221+
extend_trajectory_point.longitudinal_velocity_mps = goal_point.longitudinal_velocity_mps;
222+
extend_trajectory_point.lateral_velocity_mps = goal_point.lateral_velocity_mps;
223+
extend_trajectory_point.acceleration_mps2 = goal_point.acceleration_mps2;
224+
return extend_trajectory_point;
225+
}
226+
205227
} // namespace longitudinal_utils
206228
} // namespace autoware::motion::control::pid_longitudinal_controller

control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

+5-1
Original file line numberDiff line numberDiff line change
@@ -220,8 +220,12 @@ void PidLongitudinalController::setTrajectory(
220220
RCLCPP_WARN_THROTTLE(logger_, *clock_, 3000, "Unexpected trajectory size < 2. Ignored.");
221221
return;
222222
}
223-
223+
// If the vehicle pass the last point of trajectory, it causes errors on control_data calculation.
224+
// To handle this, we add a virtual point after the last point.
225+
constexpr double virtual_point_distance = 5.0;
224226
m_trajectory = msg;
227+
m_trajectory.points.push_back(longitudinal_utils::getExtendTrajectoryPoint(
228+
virtual_point_distance, m_trajectory.points.back()));
225229
}
226230

227231
rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallback(

0 commit comments

Comments
 (0)