diff --git a/planning/obstacle_avoidance_planner/src/debug_marker.cpp b/planning/obstacle_avoidance_planner/src/debug_marker.cpp index a8af96e7b49de..2b0ddbed8fbba 100644 --- a/planning/obstacle_avoidance_planner/src/debug_marker.cpp +++ b/planning/obstacle_avoidance_planner/src/debug_marker.cpp @@ -72,7 +72,7 @@ MarkerArray getFootprintsMarkerArray( return marker_array; } -MarkerArray getBoundsLineMarkerArray( +MarkerArray getBoundsWidthMarkerArray( const std::vector & ref_points, const double vehicle_width, const size_t sampling_num) { @@ -141,6 +141,40 @@ MarkerArray getBoundsLineMarkerArray( return marker_array; } +MarkerArray getBoundsLineMarkerArray( + const std::vector & ref_points, const double vehicle_width) +{ + MarkerArray marker_array; + + if (ref_points.empty()) return marker_array; + + auto ub_marker = createDefaultMarker( + "map", rclcpp::Clock().now(), "left_bounds", 0, Marker::LINE_STRIP, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(0.0, 1.0, 1.0, 0.8)); + ub_marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + auto lb_marker = createDefaultMarker( + "map", rclcpp::Clock().now(), "right_bounds", 0, Marker::LINE_STRIP, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(0.0, 1.0, 1.0, 0.8)); + lb_marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + for (size_t i = 0; i < ref_points.size(); i++) { + const geometry_msgs::msg::Pose & pose = ref_points.at(i).pose; + + const double ub_y = ref_points.at(i).bounds.upper_bound + vehicle_width / 2.0; + const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; + ub_marker.points.push_back(ub); + + const double lb_y = ref_points.at(i).bounds.lower_bound - vehicle_width / 2.0; + const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; + lb_marker.points.push_back(lb); + } + marker_array.markers.push_back(ub_marker); + marker_array.markers.push_back(lb_marker); + + return marker_array; +} + MarkerArray getVehicleCircleLinesMarkerArray( const std::vector & ref_points, const std::vector & vehicle_circle_longitudinal_offsets, const double vehicle_width, @@ -308,9 +342,13 @@ MarkerArray getDebugMarker( getFootprintsMarkerArray(optimized_points, vehicle_info, debug_data.mpt_visualize_sampling_num), &marker_array); - // bounds + // bounds lines + appendMarkerArray( + getBoundsLineMarkerArray(debug_data.ref_points, vehicle_info.vehicle_width_m), &marker_array); + + // bounds width appendMarkerArray( - getBoundsLineMarkerArray( + getBoundsWidthMarkerArray( debug_data.ref_points, vehicle_info.vehicle_width_m, debug_data.mpt_visualize_sampling_num), &marker_array);