Skip to content

Commit 67d5637

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 27d25df commit 67d5637

File tree

4 files changed

+86
-1
lines changed

4 files changed

+86
-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

+30
Original file line numberDiff line numberDiff line change
@@ -186,5 +186,35 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance(
186186
}
187187
return p;
188188
}
189+
190+
TrajectoryPoint getExtendedTrajectoryPoint(
191+
const double extend_distance, const Trajectory & trajectory)
192+
{
193+
constexpr double eps = 1e-3;
194+
195+
const auto extend_pose =
196+
tier4_autoware_utils::calcOffsetPose(trajectory.points.back().pose, extend_distance, 0.0, 0.0);
197+
198+
// To avoid 0 slope, extend the point with the same slope as the last two points
199+
const double z_change_ratio =
200+
(trajectory.points.at(trajectory.points.size() - 2).pose.position.z -
201+
trajectory.points.back().pose.position.z) /
202+
std::max(
203+
tier4_autoware_utils::calcDistance2d(
204+
trajectory.points.at(trajectory.points.size() - 2).pose.position,
205+
trajectory.points.back().pose.position),
206+
eps);
207+
TrajectoryPoint extend_trajectory_point;
208+
extend_trajectory_point.pose = extend_pose;
209+
210+
extend_trajectory_point.pose.position.z =
211+
trajectory.points.back().pose.position.z - extend_distance * z_change_ratio;
212+
extend_trajectory_point.longitudinal_velocity_mps =
213+
trajectory.points.back().longitudinal_velocity_mps;
214+
extend_trajectory_point.lateral_velocity_mps = trajectory.points.back().lateral_velocity_mps;
215+
extend_trajectory_point.acceleration_mps2 = trajectory.points.back().acceleration_mps2;
216+
return extend_trajectory_point;
217+
}
218+
189219
} // namespace longitudinal_utils
190220
} // 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(

control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp

+43
Original file line numberDiff line numberDiff line change
@@ -562,3 +562,46 @@ TEST(TestLongitudinalControllerUtils, findTrajectoryPoseAfterDistance)
562562
EXPECT_NEAR(result.position.y, 1.0, abs_err);
563563
EXPECT_NEAR(result.position.z, 2.0, abs_err);
564564
}
565+
566+
TEST(TestLongitudinalControllerUtils, getExtendedTrajectoryPoint)
567+
{
568+
using autoware_auto_planning_msgs::msg::Trajectory;
569+
using autoware_auto_planning_msgs::msg::TrajectoryPoint;
570+
const double abs_err = 1e-5;
571+
Trajectory traj;
572+
TrajectoryPoint point;
573+
point.pose.position.x = 0.0;
574+
point.pose.position.y = 0.0;
575+
point.pose.position.z = 0.0;
576+
traj.points.push_back(point);
577+
point.pose.position.x = 1.0;
578+
point.pose.position.y = 0.0;
579+
point.pose.position.z = 0.0;
580+
traj.points.push_back(point);
581+
point.pose.position.x = 1.0;
582+
point.pose.position.y = 1.0;
583+
point.pose.position.z = 1.0;
584+
traj.points.push_back(point);
585+
point.pose.position.x = 2.0;
586+
point.pose.position.y = 1.0;
587+
point.pose.position.z = 2.0;
588+
traj.points.push_back(point);
589+
double extend_distance = 0.0;
590+
TrajectoryPoint result = longitudinal_utils::getExtendedTrajectoryPoint(extend_distance, traj);
591+
EXPECT_NEAR(result.pose.position.x, 2.0, abs_err);
592+
EXPECT_NEAR(result.pose.position.y, 1.0, abs_err);
593+
EXPECT_NEAR(result.pose.position.z, 2.0, abs_err);
594+
EXPECT_NEAR(result.longitudinal_velocity_mps, 0.0, abs_err);
595+
extend_distance = 1.0;
596+
result = longitudinal_utils::getExtendedTrajectoryPoint(extend_distance, traj);
597+
EXPECT_NEAR(result.pose.position.x, 3.0, abs_err);
598+
EXPECT_NEAR(result.pose.position.y, 1.0, abs_err);
599+
EXPECT_NEAR(result.pose.position.z, 3.0, abs_err);
600+
EXPECT_NEAR(result.longitudinal_velocity_mps, 0.0, abs_err);
601+
extend_distance = 2.0;
602+
result = longitudinal_utils::getExtendedTrajectoryPoint(extend_distance, traj);
603+
EXPECT_NEAR(result.pose.position.x, 4.0, abs_err);
604+
EXPECT_NEAR(result.pose.position.y, 1.0, abs_err);
605+
EXPECT_NEAR(result.pose.position.z, 4.0, abs_err);
606+
EXPECT_NEAR(result.longitudinal_velocity_mps, 0.0, abs_err);
607+
}

0 commit comments

Comments
 (0)