Skip to content

Commit 208ca0a

Browse files
replace util function for footprint
Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
1 parent c004f94 commit 208ca0a

File tree

1 file changed

+2
-12
lines changed

1 file changed

+2
-12
lines changed

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

+2-12
Original file line numberDiff line numberDiff line change
@@ -1492,18 +1492,8 @@ void StartPlannerModule::setDebugData()
14921492
info_marker_);
14931493
auto marker = createDefaultMarker(
14941494
"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_);
15071497
marker.lifetime = life_time;
15081498
info_marker_.markers.push_back(marker);
15091499
}

0 commit comments

Comments
 (0)