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 d337bae17ca6c..1d0c9f5c2ce65 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 @@ -17,6 +17,7 @@ #include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include @@ -25,11 +26,16 @@ namespace marker_utils::lane_change_markers { using behavior_path_planner::LaneChangePath; +using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; using visualization_msgs::msg::MarkerArray; MarkerArray showAllValidLaneChangePath( const std::vector & lanes, 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); +MarkerArray showFilteredObjects( + const ExtendedPredictedObjects & current_lane_objects, + const ExtendedPredictedObjects & target_lane_objects, + const ExtendedPredictedObjects & other_lane_objects, const std::string & ns); } // namespace marker_utils::lane_change_markers #endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__LANE_CHANGE__DEBUG_HPP_ 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 ba8ddd9b19c46..ed83db0e778ae 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 @@ -29,6 +29,7 @@ #include +#include #include #include @@ -42,6 +43,7 @@ using behavior_path_planner::ShiftLineArray; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugPair; using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; +using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; using geometry_msgs::msg::Point; using geometry_msgs::msg::Polygon; using geometry_msgs::msg::Pose; @@ -109,6 +111,10 @@ MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::strin MarkerArray showPredictedPath(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); + +MarkerArray showFilteredObjects( + const ExtendedPredictedObjects & predicted_objects, const std::string & ns, + const ColorRGBA & color, int32_t id); } // namespace marker_utils #endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index 1a7350d82aa4e..a3469dee2909b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -154,6 +154,11 @@ class LaneChangeBase return object_debug_after_approval_; } + const LaneChangeTargetObjects & getDebugFilteredObjects() const + { + return debug_filtered_objects_; + } + const Pose & getEgoPose() const { return planner_data_->self_odometry->pose.pose; } const Point & getEgoPosition() const { return getEgoPose().position; } @@ -262,6 +267,7 @@ class LaneChangeBase mutable LaneChangePaths debug_valid_path_{}; mutable CollisionCheckDebugMap object_debug_{}; mutable CollisionCheckDebugMap object_debug_after_approval_{}; + mutable LaneChangeTargetObjects debug_filtered_objects_{}; mutable double object_debug_lifetime_{0.0}; mutable StopWatch stop_watch_; }; 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 93eb0457d7cf4..d4b43ed4c0613 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 @@ -77,6 +77,7 @@ struct ExtendedPredictedObject autoware_auto_perception_msgs::msg::Shape shape; std::vector predicted_paths; }; +using ExtendedPredictedObjects = std::vector; /** * @brief Specifies which object class should be checked. 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 7493982db78b1..a1b77b1802999 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 @@ -98,4 +98,28 @@ MarkerArray createLaneChangingVirtualWallMarker( return marker_array; } +MarkerArray showFilteredObjects( + const ExtendedPredictedObjects & current_lane_objects, + const ExtendedPredictedObjects & target_lane_objects, + const ExtendedPredictedObjects & other_lane_objects, const std::string & ns) +{ + int32_t update_id = 0; + auto current_marker = + marker_utils::showFilteredObjects(current_lane_objects, ns, colors::yellow(), update_id); + update_id += static_cast(current_marker.markers.size()); + auto target_marker = + marker_utils::showFilteredObjects(target_lane_objects, ns, colors::aqua(), update_id); + update_id += static_cast(target_marker.markers.size()); + auto other_marker = + marker_utils::showFilteredObjects(other_lane_objects, ns, colors::medium_orchid(), update_id); + + MarkerArray marker_array; + marker_array.markers.insert( + marker_array.markers.end(), current_marker.markers.begin(), current_marker.markers.end()); + marker_array.markers.insert( + marker_array.markers.end(), target_marker.markers.begin(), target_marker.markers.end()); + marker_array.markers.insert( + marker_array.markers.end(), other_marker.markers.begin(), other_marker.markers.end()); + return marker_array; +} } // namespace marker_utils::lane_change_markers diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp index d6a237079448f..7dc85de879a18 100644 --- a/planning/behavior_path_planner/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/src/marker_utils/utils.cpp @@ -21,6 +21,8 @@ #include +#include + namespace marker_utils { using behavior_path_planner::ShiftLine; @@ -645,4 +647,40 @@ MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, st return marker_array; } +MarkerArray showFilteredObjects( + const ExtendedPredictedObjects & predicted_objects, const std::string & ns, + const ColorRGBA & color, int32_t id) +{ + using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; + if (predicted_objects.empty()) { + return MarkerArray{}; + } + + const auto now = rclcpp::Clock{RCL_ROS_TIME}.now(); + + 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( + predicted_objects.size()); // poly ego, text ego, poly obj, text obj, cube obj + + std::for_each( + predicted_objects.begin(), predicted_objects.end(), [&](const ExtendedPredictedObject & obj) { + 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(obj.initial_pose.pose); + }); + + 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 5e47116117103..78e68386e7f65 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 @@ -15,6 +15,7 @@ #include "behavior_path_planner/scene_module/lane_change/interface.hpp" #include "behavior_path_planner/marker_utils/lane_change/debug.hpp" +#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/lane_change/utils.hpp" @@ -305,10 +306,12 @@ void LaneChangeInterface::setObjectDebugVisualization() const using marker_utils::showPredictedPath; using marker_utils::showSafetyCheckInfo; using marker_utils::lane_change_markers::showAllValidLaneChangePath; + using marker_utils::lane_change_markers::showFilteredObjects; const auto debug_data = module_type_->getDebugData(); const auto debug_after_approval = module_type_->getAfterApprovalDebugData(); const auto debug_valid_path = module_type_->getDebugValidPath(); + const auto debug_filtered_objects = module_type_->getDebugFilteredObjects(); debug_marker_.markers.clear(); const auto add = [this](const MarkerArray & added) { @@ -316,6 +319,9 @@ void LaneChangeInterface::setObjectDebugVisualization() const }; add(showAllValidLaneChangePath(debug_valid_path, "lane_change_valid_paths")); + add(showFilteredObjects( + debug_filtered_objects.current_lane, debug_filtered_objects.target_lane, + debug_filtered_objects.other_lane, "object_filtered")); if (!debug_data.empty()) { add(showSafetyCheckInfo(debug_data, "object_debug_info")); add(showPredictedPath(debug_data, "ego_predicted_path")); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index d5d7e77b13d5f..26c41006576f1 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -934,6 +934,7 @@ bool NormalLaneChange::getLaneChangePaths( utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_); const auto target_objects = getTargetObjects(current_lanes, target_lanes); + debug_filtered_objects_ = target_objects; const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); @@ -1150,6 +1151,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const const auto & target_lanes = status_.target_lanes; const auto target_objects = getTargetObjects(current_lanes, target_lanes); + debug_filtered_objects_ = target_objects; CollisionCheckDebugMap debug_data; const auto safety_status = isLaneChangePathSafe(