Skip to content

Commit 27ae345

Browse files
kyoichi-sugaharatakayuki5168
authored andcommitted
fix(autoware_mpc_lateral_controller): add timestamp and frame ID to published trajectory (autowarefoundation#8164)
add timestamp and frame ID to published trajectory Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
1 parent 069d98c commit 27ae345

File tree

1 file changed

+3
-1
lines changed
  • control/autoware_mpc_lateral_controller/src

1 file changed

+3
-1
lines changed

control/autoware_mpc_lateral_controller/src/mpc.cpp

+3-1
Original file line numberDiff line numberDiff line change
@@ -804,8 +804,10 @@ Trajectory MPC::calculatePredictedTrajectory(
804804
const auto frenet = m_vehicle_model_ptr->calculatePredictedTrajectoryInFrenetCoordinate(
805805
mpc_matrix.Aex, mpc_matrix.Bex, mpc_matrix.Cex, mpc_matrix.Wex, x0, Uex, reference_trajectory,
806806
dt);
807-
const auto frenet_clipped = MPCUtils::convertToAutowareTrajectory(
807+
auto frenet_clipped = MPCUtils::convertToAutowareTrajectory(
808808
MPCUtils::clipTrajectoryByLength(frenet, predicted_length));
809+
frenet_clipped.header.stamp = m_clock->now();
810+
frenet_clipped.header.frame_id = "map";
809811
m_debug_frenet_predicted_trajectory_pub->publish(frenet_clipped);
810812
}
811813

0 commit comments

Comments
 (0)