@@ -1492,18 +1492,8 @@ void StartPlannerModule::setDebugData()
1492
1492
info_marker_);
1493
1493
auto marker = createDefaultMarker (
1494
1494
" map" , rclcpp::Clock{RCL_ROS_TIME}.now (), " static_collision_check_end_polygon" , 0 ,
1495
- Marker::LINE_LIST, createMarkerScale (0.1 , 0.1 , 0.1 ), red_color);
1496
- const auto footprint = transformVector (
1497
- local_footprint, tier4_autoware_utils::pose2transform (*collision_check_end_pose));
1498
- const double ego_z = planner_data_->self_odometry ->pose .pose .position .z ;
1499
- for (size_t i = 0 ; i < footprint.size (); i++) {
1500
- const auto & current_point = footprint.at (i);
1501
- const auto & next_point = footprint.at ((i + 1 ) % footprint.size ());
1502
- marker.points .push_back (
1503
- tier4_autoware_utils::createPoint (current_point.x (), current_point.y (), ego_z));
1504
- marker.points .push_back (
1505
- tier4_autoware_utils::createPoint (next_point.x (), next_point.y (), ego_z));
1506
- }
1495
+ Marker::LINE_STRIP, createMarkerScale (0.1 , 0.1 , 0.1 ), red_color);
1496
+ addFootprintMarker (marker, *collision_check_end_pose, vehicle_info_);
1507
1497
marker.lifetime = life_time;
1508
1498
info_marker_.markers .push_back (marker);
1509
1499
}
0 commit comments