Skip to content

Commit df05c22

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 870fc09 commit df05c22

File tree

3 files changed

+39
-1
lines changed

3 files changed

+39
-1
lines changed

control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp

+8
Original file line numberDiff line numberDiff line change
@@ -152,6 +152,14 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance(
152152
const size_t src_idx, const double distance,
153153
const autoware_auto_planning_msgs::msg::Trajectory & trajectory);
154154

155+
/**
156+
* @brief calculates the translated position of the goal point with respect to extend_distance
157+
* @param [in] extend_distance current index
158+
* @param [in] goal_point distance to project
159+
*/
160+
TrajectoryPoint getExtendedTrajectoryPoint(
161+
const double extend_distance, const Trajectory & trajectory);
162+
155163
} // namespace longitudinal_utils
156164
} // namespace autoware::motion::control::pid_longitudinal_controller
157165

control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp

+26
Original file line numberDiff line numberDiff line change
@@ -186,5 +186,31 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance(
186186
}
187187
return p;
188188
}
189+
190+
TrajectoryPoint getExtendedTrajectoryPoint(
191+
const double extend_distance, const Trajectory & trajectory)
192+
{
193+
const auto extend_pose =
194+
tier4_autoware_utils::calcOffsetPose(trajectory.points.back().pose, extend_distance, 0.0, 0.0);
195+
196+
// To avoid 0 slope, extend the point with the same slope as the last two points
197+
const double z_change_ratio =
198+
(trajectory.points.at(trajectory.points.size() - 2).pose.position.z -
199+
trajectory.points.back().pose.position.z) /
200+
tier4_autoware_utils::calcDistance3d(
201+
trajectory.points.at(trajectory.points.size() - 2).pose.position,
202+
trajectory.points.back().pose.position);
203+
TrajectoryPoint extend_trajectory_point;
204+
extend_trajectory_point.pose = extend_pose;
205+
206+
extend_trajectory_point.pose.position.z =
207+
trajectory.points.back().pose.position.z - extend_distance * z_change_ratio;
208+
extend_trajectory_point.longitudinal_velocity_mps =
209+
trajectory.points.back().longitudinal_velocity_mps;
210+
extend_trajectory_point.lateral_velocity_mps = trajectory.points.back().lateral_velocity_mps;
211+
extend_trajectory_point.acceleration_mps2 = trajectory.points.back().acceleration_mps2;
212+
return extend_trajectory_point;
213+
}
214+
189215
} // namespace longitudinal_utils
190216
} // 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
@@ -241,8 +241,12 @@ void PidLongitudinalController::setTrajectory(
241241
RCLCPP_WARN_THROTTLE(logger_, *clock_, 3000, "Unexpected trajectory size < 2. Ignored.");
242242
return;
243243
}
244-
244+
// If the vehicle pass the last point of trajectory, it causes errors on control_data calculation.
245+
// To handle this, we add a virtual point after the last point.
246+
constexpr double virtual_point_distance = 5.0;
245247
m_trajectory = msg;
248+
m_trajectory.points.push_back(
249+
longitudinal_utils::getExtendedTrajectoryPoint(virtual_point_distance, m_trajectory));
246250
}
247251

248252
rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallback(

0 commit comments

Comments
 (0)