diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 9ad7f72d6af4e..0e0a6b123be12 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -1415,6 +1415,39 @@ void StartPlannerModule::setDebugData() add(start_pose_text_marker_array); } + // visualize the footprint from pull_out_start pose to pull_out_end pose along the path + { + MarkerArray pull_out_path_footprint_marker_array{}; + const auto pink = createMarkerColor(1.0, 0.0, 1.0, 0.99); + Marker pull_out_path_footprint_marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "shift_path_footprint", 0, Marker::LINE_STRIP, + createMarkerScale(0.2, 0.2, 0.2), pink); + pull_out_path_footprint_marker.lifetime = rclcpp::Duration::from_seconds(1.5); + PathWithLaneId path_shift_start_to_end{}; + const auto shift_path = status_.pull_out_path.partial_paths.front(); + { + const size_t pull_out_start_idx = motion_utils::findNearestIndex( + shift_path.points, status_.pull_out_path.start_pose.position); + const size_t pull_out_end_idx = + motion_utils::findNearestIndex(shift_path.points, status_.pull_out_path.end_pose.position); + + path_shift_start_to_end.points.insert( + path_shift_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx, + shift_path.points.begin() + pull_out_end_idx + 1); + } + + for (size_t i = 0; i < path_shift_start_to_end.points.size(); ++i) { + pull_out_path_footprint_marker.id = i; + pull_out_path_footprint_marker.points.clear(); + addFootprintMarker( + pull_out_path_footprint_marker, path_shift_start_to_end.points.at(i).point.pose, + vehicle_info_); + pull_out_path_footprint_marker_array.markers.push_back(pull_out_path_footprint_marker); + } + + add(pull_out_path_footprint_marker_array); + } + // safety check if (parameters_->safety_check_params.enable_safety_check) { if (start_planner_data_.ego_predicted_path.size() > 0) {