chore(lane_change): add z-pos in debug marker (autowarefoundation#4074)

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
tier4 · Jul 4, 2023 · bc2c305 · bc2c305
1 parent 7cc06e9
commit bc2c305
Showing 1 changed file with 3 additions and 2 deletions.
diff --git a/planning/behavior_path_planner/src/marker_util/lane_change/debug.cpp b/planning/behavior_path_planner/src/marker_util/lane_change/debug.cpp
@@ -185,11 +185,12 @@ MarkerArray showPolygon(
     }
     const auto & color = colors.at(idx);
     const auto & ego_polygon = info.ego_polygon.outer();
+    const auto poly_z = info.current_pose.position.z;  // temporally
     ego_marker.id = ++id;
     ego_marker.color = createMarkerColor(color[0], color[1], color[2], 0.8);
     ego_marker.points.reserve(ego_polygon.size());
     for (const auto & p : ego_polygon) {
-      ego_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0));
+      ego_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z));
     }
     marker_array.markers.push_back(ego_marker);
 
@@ -198,7 +199,7 @@ MarkerArray showPolygon(
     obj_marker.color = createMarkerColor(color[0], color[1], color[2], 0.8);
     obj_marker.points.reserve(obj_polygon.size());
     for (const auto & p : obj_polygon) {
-      obj_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0));
+      obj_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z));
     }
     marker_array.markers.push_back(obj_marker);
     ++idx;