fix(trajectory_follower): fix mpc trajectory z pos (backport autoware…

…foundation#2482) (#353)
tier4 · Apr 13, 2023 · 7123a7f · 7123a7f
1 parent 859af0a
commit 7123a7f
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/control/trajectory_follower/src/mpc_utils.cpp b/control/trajectory_follower/src/mpc_utils.cpp
@@ -248,7 +248,7 @@ bool8_t convertToMPCTrajectory(
   for (const autoware_auto_planning_msgs::msg::TrajectoryPoint & p : input.points) {
     const float64_t x = p.pose.position.x;
     const float64_t y = p.pose.position.y;
-    const float64_t z = 0.0;
+    const float64_t z = p.pose.position.z;
     const float64_t yaw = ::motion::motion_common::to_angle(p.pose.orientation);
     const float64_t vx = p.longitudinal_velocity_mps;
     const float64_t k = 0.0;