diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp index e1ff447436aa6..fb7dcbb858f03 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp @@ -30,12 +30,8 @@ using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; // MarkerArray showAllValidLaneChangePath( // const std::vector & lanes, std::string && ns); using visualization_msgs::msg::MarkerArray; -MarkerArray showObjectInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); MarkerArray showAllValidLaneChangePath( const std::vector & lanes, std::string && ns); -MarkerArray showLerpedPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); -MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); -MarkerArray showPolygonPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); MarkerArray createLaneChangingVirtualWallMarker( const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name, const rclcpp::Time & now, const std::string & ns); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp index 62779859e8f10..74803679db21a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -180,10 +180,10 @@ struct SafetyCheckParams struct CollisionCheckDebug { std::string unsafe_reason; ///< Reason indicating unsafe situation. - Pose current_pose{}; ///< Ego vehicle's current pose. Twist current_twist{}; ///< Ego vehicle's current velocity and rotation. - Twist object_twist{}; ///< Detected object's velocity and rotation. Pose expected_ego_pose{}; ///< Predicted future pose of ego vehicle. + Pose current_obj_pose{}; ///< Detected object's current pose. + Twist object_twist{}; ///< Detected object's velocity and rotation. Pose expected_obj_pose{}; ///< Predicted future pose of object. double rss_longitudinal{0.0}; ///< Longitudinal RSS measure. double inter_vehicle_distance{0.0}; ///< Distance between ego vehicle and object. @@ -191,9 +191,10 @@ struct CollisionCheckDebug double extended_polygon_lat_offset{0.0}; ///< Lateral offset for extended polygon. bool is_front{false}; ///< True if object is in front of ego vehicle. bool is_safe{false}; ///< True if situation is deemed safe. - std::vector lerped_path; ///< Interpolated ego vehicle path. - Polygon2d extended_ego_polygon{}; ///< Ego vehicle's extended collision polygon. - Polygon2d extended_obj_polygon{}; ///< Detected object's extended collision polygon. + std::vector ego_predicted_path; ///< ego vehicle's predicted path. + std::vector obj_predicted_path; ///< object's predicted path. + Polygon2d extended_ego_polygon{}; ///< Ego vehicle's extended collision polygon. + Polygon2d extended_obj_polygon{}; ///< Detected object's extended collision polygon. }; using CollisionCheckDebugPair = std::pair; using CollisionCheckDebugMap = diff --git a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp index aab2be6f3054e..7493982db78b1 100644 --- a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp @@ -36,40 +36,6 @@ using geometry_msgs::msg::Point; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerScale; -MarkerArray showObjectInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) -{ - Marker obj_marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, - createMarkerScale(0.5, 0.5, 0.5), colors::aqua()); - - MarkerArray marker_array; - int32_t id{0}; - - marker_array.markers.reserve(obj_debug_vec.size()); - - int idx{0}; - - for (const auto & [uuid, info] : obj_debug_vec) { - obj_marker.id = ++id; - obj_marker.pose = info.current_pose; - - std::ostringstream ss; - - ss << "Idx: " << ++idx << "\nReason: " << info.unsafe_reason - << "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal - << "\nEgo to obj: " << info.inter_vehicle_distance - << "\nExtended polygon lateral offset: " << info.extended_polygon_lat_offset - << "\nExtended polygon longitudinal offset: " << info.extended_polygon_lon_offset - << "\nPosition: " << (info.is_front ? "front" : "back") - << "\nSafe: " << (info.is_safe ? "Yes" : "No"); - - obj_marker.text = ss.str(); - - marker_array.markers.push_back(obj_marker); - } - return marker_array; -} - MarkerArray showAllValidLaneChangePath(const std::vector & lanes, std::string && ns) { if (lanes.empty()) { @@ -103,121 +69,6 @@ MarkerArray showAllValidLaneChangePath(const std::vector & lanes return marker_array; } -MarkerArray showLerpedPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) -{ - MarkerArray marker_array; - int32_t id{0}; - const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; - marker_array.markers.reserve(obj_debug_vec.size()); - - for (const auto & [uuid, info] : obj_debug_vec) { - Marker marker = createDefaultMarker( - "map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.3, 0.3, 0.3), - colors::magenta()); - marker.points.reserve(info.lerped_path.size()); - - for (const auto & point : info.lerped_path) { - marker.points.push_back(point.position); - } - - marker_array.markers.push_back(marker); - } - return marker_array; -} - -MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) -{ - if (obj_debug_vec.empty()) { - return MarkerArray{}; - } - - constexpr float scale_val{0.2}; - int32_t id{0}; - const auto now = rclcpp::Clock{RCL_ROS_TIME}.now(); - Marker ego_marker = createDefaultMarker( - "map", now, ns, id, Marker::LINE_STRIP, createMarkerScale(scale_val, scale_val, scale_val), - colors::green()); - Marker obj_marker = ego_marker; - - auto text_marker = createDefaultMarker( - "map", now, ns + "_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(1.5, 1.5, 1.5), colors::white()); - - MarkerArray marker_array; - - const auto reserve_size = obj_debug_vec.size(); - - marker_array.markers.reserve(reserve_size * 4); - - int32_t idx = {0}; - - for (const auto & [uuid, info] : obj_debug_vec) { - const auto color = info.is_safe ? colors::green() : colors::red(); - const auto & ego_polygon = info.extended_ego_polygon.outer(); - const auto poly_z = info.current_pose.position.z; // temporally - ego_marker.id = ++id; - ego_marker.color = color; - ego_marker.points.reserve(ego_polygon.size()); - for (const auto & p : ego_polygon) { - ego_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z)); - } - marker_array.markers.push_back(ego_marker); - - std::ostringstream ss; - text_marker.id = ego_marker.id; - ss << ++idx; - text_marker.text = ss.str(); - text_marker.pose = info.expected_ego_pose; - - marker_array.markers.push_back(text_marker); - - const auto & obj_polygon = info.extended_obj_polygon.outer(); - obj_marker.id = ++id; - obj_marker.color = color; - obj_marker.points.reserve(obj_polygon.size()); - for (const auto & p : obj_polygon) { - obj_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z)); - } - marker_array.markers.push_back(obj_marker); - - text_marker.id = obj_marker.id; - text_marker.pose = info.expected_obj_pose; - marker_array.markers.push_back(text_marker); - - ego_marker.points.clear(); - obj_marker.points.clear(); - } - - return marker_array; -} - -MarkerArray showPolygonPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) -{ - const auto colors = colors::colors_list(); - const auto loop_size = std::min(colors.size(), obj_debug_vec.size()); - MarkerArray marker_array; - int32_t id{0}; - size_t idx{0}; - const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; - marker_array.markers.reserve(obj_debug_vec.size()); - - for (const auto & [uuid, info] : obj_debug_vec) { - const auto & color = colors.at(idx); - Marker marker = createDefaultMarker( - "map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.2, 0.2, 0.2), color); - marker.points.reserve(2); - marker.points.push_back(info.expected_ego_pose.position); - marker.points.push_back(info.expected_obj_pose.position); - marker_array.markers.push_back(marker); - ++idx; - if (idx >= loop_size) { - break; - } - } - - return marker_array; -} - MarkerArray createLaneChangingVirtualWallMarker( const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name, const rclcpp::Time & now, const std::string & ns) diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp index 6253c6bb372ab..1548b8ba99933 100644 --- a/planning/behavior_path_planner/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/src/marker_utils/utils.cpp @@ -14,6 +14,7 @@ #include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/marker_utils/colors.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -37,7 +38,7 @@ using visualization_msgs::msg::Marker; CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj) { CollisionCheckDebug debug; - debug.current_pose = obj.initial_pose.pose; + debug.current_obj_pose = obj.initial_pose.pose; debug.current_twist = obj.initial_twist.twist; return {tier4_autoware_utils::toHexString(obj.uuid), debug}; } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index a099ae9ff75e8..42c460e15c50c 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -323,6 +323,11 @@ void LaneChangeInterface::setObjectDebugVisualization() const add(showPolygon(debug_data, "ego_and_target_polygon_relation")); } + if (!debug_after_approval.empty()) { + add(showSafetyCheckInfo(debug_after_approval, "object_debug_info_after_approval")); + add(showPredictedPath(debug_after_approval, "ego_predicted_path_after_approval")); + add(showPolygon(debug_after_approval, "ego_and_target_polygon_relation_after_approval")); + } } std::shared_ptr LaneChangeInterface::get_debug_msg_array() const