@@ -186,5 +186,31 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance(
186
186
}
187
187
return p;
188
188
}
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
+
189
215
} // namespace longitudinal_utils
190
216
} // namespace autoware::motion::control::pid_longitudinal_controller
0 commit comments