From 1a60db7a61b2cde7dc9f45054f957f5a09c39115 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 9 Aug 2024 01:29:28 +0900 Subject: [PATCH] fix(autoware_mpc_lateral_controller): add timestamp and frame ID to published trajectory (#8164) (#1452) add timestamp and frame ID to published trajectory Signed-off-by: kyoichi-sugahara Co-authored-by: Kyoichi Sugahara --- control/autoware_mpc_lateral_controller/src/mpc.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index 79748f23e7aa0..c42072a186f4d 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -804,8 +804,10 @@ Trajectory MPC::calculatePredictedTrajectory( const auto frenet = m_vehicle_model_ptr->calculatePredictedTrajectoryInFrenetCoordinate( mpc_matrix.Aex, mpc_matrix.Bex, mpc_matrix.Cex, mpc_matrix.Wex, x0, Uex, reference_trajectory, dt); - const auto frenet_clipped = MPCUtils::convertToAutowareTrajectory( + auto frenet_clipped = MPCUtils::convertToAutowareTrajectory( MPCUtils::clipTrajectoryByLength(frenet, predicted_length)); + frenet_clipped.header.stamp = m_clock->now(); + frenet_clipped.header.frame_id = "map"; m_debug_frenet_predicted_trajectory_pub->publish(frenet_clipped); }