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 808e76a140761..d337bae17ca6c 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 @@ -26,12 +26,8 @@ namespace marker_utils::lane_change_markers { using behavior_path_planner::LaneChangePath; 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/marker_utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp index 657437615f809..ba8ddd9b19c46 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp @@ -104,6 +104,11 @@ MarkerArray createPredictedPathMarkerArray( const PredictedPath & ego_predicted_path, const vehicle_info_util::VehicleInfo & vehicle_info, std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); +MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); + +MarkerArray showPredictedPath(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); + +MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); } // namespace marker_utils #endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_ 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 87c93f49901f0..93eb0457d7cf4 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 @@ -179,10 +179,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. @@ -190,9 +190,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 f24e772d198c1..d6a237079448f 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}; } @@ -486,4 +487,162 @@ MarkerArray createPredictedPathMarkerArray( return msg; } +MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) +{ + if (obj_debug_vec.empty()) { + return MarkerArray{}; + } + + int32_t id{0}; + const auto now = rclcpp::Clock{RCL_ROS_TIME}.now(); + + constexpr float line_scale_val{0.2}; + const auto line_marker_scale = createMarkerScale(line_scale_val, line_scale_val, line_scale_val); + + auto default_line_marker = [&](const auto & color = colors::green()) { + return createDefaultMarker("map", now, ns, ++id, Marker::LINE_STRIP, line_marker_scale, color); + }; + + constexpr float text_scale_val{1.5}; + const auto text_marker_scale = createMarkerScale(text_scale_val, text_scale_val, text_scale_val); + + auto default_text_marker = [&]() { + return createDefaultMarker( + "map", now, ns + "_text", ++id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + text_marker_scale, colors::white()); + }; + + auto default_cube_marker = + [&](const auto & width, const auto & depth, const auto & color = colors::green()) { + return createDefaultMarker( + "map", now, ns + "_cube", ++id, visualization_msgs::msg::Marker::CUBE, + createMarkerScale(width, depth, 1.0), color); + }; + + MarkerArray marker_array; + marker_array.markers.reserve( + obj_debug_vec.size() * 5); // poly ego, text ego, poly obj, text obj, cube obj + + int32_t idx = {0}; + for (const auto & [uuid, info] : obj_debug_vec) { + const auto color = info.is_safe ? colors::green() : colors::red(); + const auto poly_z = info.current_obj_pose.position.z; // temporally + + const auto insert_polygon_marker = [&](const auto & polygon) { + marker_array.markers.emplace_back(); + auto & polygon_marker = marker_array.markers.back(); + polygon_marker = default_line_marker(color); + polygon_marker.points.reserve(polygon.outer().size()); + for (const auto & p : polygon.outer()) { + polygon_marker.points.push_back(createPoint(p.x(), p.y(), poly_z)); + } + }; + + insert_polygon_marker(info.extended_ego_polygon); + insert_polygon_marker(info.extended_obj_polygon); + + const auto str_idx = std::to_string(++idx); + const auto insert_text_marker = [&](const auto & pose) { + marker_array.markers.emplace_back(); + auto & text_marker = marker_array.markers.back(); + text_marker = default_text_marker(); + text_marker.text = str_idx; + text_marker.pose = pose; + }; + + insert_text_marker(info.expected_ego_pose); + insert_text_marker(info.expected_obj_pose); + + const auto insert_cube_marker = [&](const auto & pose) { + marker_array.markers.emplace_back(); + auto & cube_marker = marker_array.markers.back(); + cube_marker = default_cube_marker(1.0, 1.0, color); + cube_marker.pose = pose; + }; + insert_cube_marker(info.current_obj_pose); + } + return marker_array; +} + +MarkerArray showPredictedPath(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) +{ + int32_t id{0}; + const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; + const auto arrow_marker_scale = createMarkerScale(1.0, 0.3, 0.3); + const auto default_arrow_marker = [&](const auto & color) { + return createDefaultMarker( + "map", current_time, ns, ++id, Marker::ARROW, arrow_marker_scale, color); + }; + + MarkerArray marker_array; + marker_array.markers.reserve(std::accumulate( + obj_debug_vec.cbegin(), obj_debug_vec.cend(), 0UL, + [&](const auto current_sum, const auto & obj_debug) { + const auto & [uuid, info] = obj_debug; + return current_sum + info.ego_predicted_path.size() + info.obj_predicted_path.size() + 2; + })); + + for (const auto & [uuid, info] : obj_debug_vec) { + const auto insert_marker = [&](const auto & path, const auto & color) { + for (const auto & pose : path) { + marker_array.markers.emplace_back(); + auto & marker = marker_array.markers.back(); + marker = default_arrow_marker(color); + marker.pose = pose.pose; + } + }; + + insert_marker(info.ego_predicted_path, colors::aqua()); + insert_marker(info.obj_predicted_path, colors::yellow()); + const auto insert_expected_pose_marker = [&](const auto & pose, const auto & color) { + // instead of checking for distance, inserting a new marker might be more efficient + marker_array.markers.emplace_back(); + auto & marker = marker_array.markers.back(); + marker = default_arrow_marker(color); + marker.pose = pose; + marker.pose.position.z += 0.05; + }; + + insert_expected_pose_marker(info.expected_ego_pose, colors::red()); + insert_expected_pose_marker(info.expected_obj_pose, colors::red()); + } + return marker_array; +} + +MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) +{ + int32_t id{0}; + auto default_text_marker = [&]() { + return createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, ++id, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.5, 0.5, 0.5), colors::aqua()); + }; + + MarkerArray marker_array; + + marker_array.markers.reserve(obj_debug_vec.size()); + + int idx{0}; + + for (const auto & [uuid, info] : obj_debug_vec) { + auto safety_check_info_text = default_text_marker(); + safety_check_info_text.pose = info.current_obj_pose; + + std::ostringstream ss; + + ss << "Idx: " << ++idx << "\nUnsafe reason: " << info.unsafe_reason + << "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal + << "\nEgo to obj: " << info.inter_vehicle_distance + << "\nExtended polygon: " << (info.is_front ? "ego" : "object") + << "\nExtended polygon lateral offset: " << info.extended_polygon_lat_offset + << "\nExtended polygon longitudinal offset: " << info.extended_polygon_lon_offset + << "\nLast checked position: " << (info.is_front ? "obj in front ego" : "obj at back ego") + << "\nSafe: " << (info.is_safe ? "Yes" : "No"); + + safety_check_info_text.text = ss.str(); + marker_array.markers.push_back(safety_check_info_text); + } + return marker_array; +} + } // namespace marker_utils 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 072c1c62ae3d2..0ceab21573316 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 @@ -295,11 +295,10 @@ void LaneChangeInterface::setObjectDebugVisualization() const if (!parameters_->publish_debug_marker) { return; } + using marker_utils::showPolygon; + using marker_utils::showPredictedPath; + using marker_utils::showSafetyCheckInfo; using marker_utils::lane_change_markers::showAllValidLaneChangePath; - using marker_utils::lane_change_markers::showLerpedPose; - using marker_utils::lane_change_markers::showObjectInfo; - using marker_utils::lane_change_markers::showPolygon; - using marker_utils::lane_change_markers::showPolygonPose; const auto debug_data = module_type_->getDebugData(); const auto debug_after_approval = module_type_->getAfterApprovalDebugData(); @@ -312,14 +311,14 @@ void LaneChangeInterface::setObjectDebugVisualization() const add(showAllValidLaneChangePath(debug_valid_path, "lane_change_valid_paths")); if (!debug_data.empty()) { - add(showObjectInfo(debug_data, "object_debug_info")); - add(showLerpedPose(debug_data, "ego_predicted_path")); + add(showSafetyCheckInfo(debug_data, "object_debug_info")); + add(showPredictedPath(debug_data, "ego_predicted_path")); add(showPolygon(debug_data, "ego_and_target_polygon_relation")); } if (!debug_after_approval.empty()) { - add(showObjectInfo(debug_after_approval, "object_debug_info_after_approval")); - add(showLerpedPose(debug_after_approval, "ego_predicted_path_after_approval")); + 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")); } } diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 2939ca29c0a40..1838dc6bdc07b 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -253,7 +253,11 @@ bool checkCollision( const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, double hysteresis_factor, CollisionCheckDebug & debug) { - debug.lerped_path.reserve(target_object_path.path.size()); + { + debug.ego_predicted_path = predicted_ego_path; + debug.obj_predicted_path = target_object_path.path; + debug.current_obj_pose = target_object.initial_pose.pose; + } for (const auto & obj_pose_with_poly : target_object_path.path) { const auto & current_time = obj_pose_with_poly.time; @@ -277,7 +281,6 @@ bool checkCollision( const auto & ego_velocity = interpolated_data->velocity; { - debug.lerped_path.push_back(ego_pose); debug.expected_ego_pose = ego_pose; debug.expected_obj_pose = obj_pose; debug.extended_ego_polygon = ego_polygon;