From fa5d7dbb417f62e6944a296dc36da1ffa5336f99 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 16 Nov 2023 00:09:19 +0900 Subject: [PATCH] fix(goal_planner): fix detection_polygons visualiztion (#5596) --- .../src/scene_module/goal_planner/goal_planner_module.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 84b4032cde4ad..6b363f206204c 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -1752,16 +1752,16 @@ void GoalPlannerModule::setDebugData() "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "detection_polygons", 0, Marker::LINE_LIST, tier4_autoware_utils::createMarkerScale(0.01, 0.0, 0.0), tier4_autoware_utils::createMarkerColor(0.0, 0.0, 1.0, 0.999)); - + const double ego_z = planner_data_->self_odometry->pose.pose.position.z; for (const auto & ego_polygon : debug_data_.ego_polygons_expanded) { for (size_t ep_idx = 0; ep_idx < ego_polygon.outer().size(); ++ep_idx) { const auto & current_point = ego_polygon.outer().at(ep_idx); const auto & next_point = ego_polygon.outer().at((ep_idx + 1) % ego_polygon.outer().size()); marker.points.push_back( - tier4_autoware_utils::createPoint(current_point.x(), current_point.y(), 0.0)); + tier4_autoware_utils::createPoint(current_point.x(), current_point.y(), ego_z)); marker.points.push_back( - tier4_autoware_utils::createPoint(next_point.x(), next_point.y(), 0.0)); + tier4_autoware_utils::createPoint(next_point.x(), next_point.y(), ego_z)); } } debug_marker_.markers.push_back(marker);