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 244e1a8a71616..912daea201978 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 @@ -39,6 +39,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::TargetObjectsOnLane; using geometry_msgs::msg::Point; using geometry_msgs::msg::Polygon; using geometry_msgs::msg::Pose; @@ -103,6 +104,8 @@ MarkerArray createPredictedPathMarkerArray( MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); +MarkerArray showPolygon(const TargetObjectsOnLane & filtered_objects, std::string && ns); + MarkerArray showPredictedPath(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); 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 aa470eb2dd80c..01e42639ac9bd 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 @@ -115,6 +115,18 @@ struct TargetObjectsOnLane std::vector on_left_lane{}; ///< Objects on the left lane. std::vector on_shoulder_lane{}; ///< Objects on the shoulder lane. std::vector on_other_lane{}; ///< Objects on other lanes. + + [[nodiscard]] bool empty() const + { + return on_current_lane.empty() && on_right_lane.empty() && on_left_lane.empty() && + on_shoulder_lane.empty() && on_other_lane.empty(); + } + + [[nodiscard]] bool size() const + { + return on_current_lane.empty() + on_right_lane.empty() + on_left_lane.empty() + + on_shoulder_lane.empty() + on_other_lane.empty(); + } }; /** diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp index 4cec9b958f904..12334e7a010f3 100644 --- a/planning/behavior_path_planner/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/src/marker_utils/utils.cpp @@ -31,6 +31,7 @@ namespace marker_utils using behavior_path_planner::ShiftLine; using behavior_path_planner::utils::calcPathArcLengthArray; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; +using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; using std_msgs::msg::ColorRGBA; using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::createDefaultMarker; @@ -492,6 +493,57 @@ MarkerArray createPredictedPathMarkerArray( return msg; } +MarkerArray showPolygon(const TargetObjectsOnLane & filtered_objects, std::string && ns) +{ + using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; + if (filtered_objects.empty()) { + return MarkerArray{}; + } + + int32_t id{0}; + 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( + filtered_objects.size()); // poly ego, text ego, poly obj, text obj, cube obj + + const auto & current_lane_obj = filtered_objects.on_current_lane; + std::for_each( + current_lane_obj.begin(), current_lane_obj.end(), [&](const ExtendedPredictedObject & obj) { + const auto color = colors::yellow(); + + 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); + }); + + const auto & right_lane_obj = filtered_objects.on_right_lane; + std::for_each( + right_lane_obj.begin(), right_lane_obj.end(), [&](const ExtendedPredictedObject & obj) { + const auto color = colors::green(); + + 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; +} + MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) { if (obj_debug_vec.empty()) { @@ -504,8 +556,10 @@ MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::strin 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); + auto default_line_marker = [&](bool is_safe, const auto & color = colors::green()) { + return createDefaultMarker( + "map", now, ns + (is_safe ? "_safe" : "_unsafe"), ++id, Marker::LINE_STRIP, line_marker_scale, + color); }; constexpr float text_scale_val{1.5}; @@ -536,7 +590,7 @@ MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::strin 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 = default_line_marker(info.is_safe, 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)); 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 74af83c2db2a1..3c78f1d95f52d 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 @@ -297,6 +297,15 @@ void LaneChangeInterface::setObjectDebugVisualization() const 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 before = module_type_->getDebugFilteredObjects(); + const auto debug_object_filtered = std::invoke([&]() { + utils::path_safety_checker::TargetObjectsOnLane objs; + objs.on_current_lane.insert( + objs.on_current_lane.end(), before.current_lane.begin(), before.current_lane.end()); + objs.on_right_lane.insert( + objs.on_right_lane.end(), before.target_lane.begin(), before.target_lane.end()); + return objs; + }); debug_marker_.markers.clear(); const auto add = [this](const MarkerArray & added) { @@ -308,12 +317,14 @@ void LaneChangeInterface::setObjectDebugVisualization() const add(showSafetyCheckInfo(debug_data, "object_debug_info")); add(showPredictedPath(debug_data, "ego_predicted_path")); add(showPolygon(debug_data, "ego_and_target_polygon_relation")); + add(showPolygon(debug_object_filtered, "object_filtered_before_approval")); } 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")); + add(showPolygon(debug_object_filtered, "object_filtered_after_approval")); } } 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 376bac3ed65fb..5110061cbdedf 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 @@ -727,8 +727,12 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( if ( target_backward_polygon && boost::geometry::intersects(target_backward_polygon.value(), obj_polygon)) { - filtered_obj_indices.target_lane.push_back(i); - continue; + const auto lateral = tier4_autoware_utils::calcLateralDeviation( + current_pose, object.kinematics.initial_pose_with_covariance.pose.position); + if (std::abs(lateral) > common_parameters.vehicle_width) { + filtered_obj_indices.target_lane.push_back(i); + continue; + } } // check if the object intersects with current lanes @@ -875,6 +879,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; candidate_paths->reserve(longitudinal_acc_sampling_values.size() * lateral_acc_sampling_num); @@ -1066,6 +1071,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( @@ -1366,10 +1372,12 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( auto current_debug_data = marker_utils::createObjectDebug(obj); const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( obj, lane_change_parameters_->use_all_predicted_path); + auto is_safe{true}; for (const auto & obj_path : obj_predicted_paths) { - if (!utils::path_safety_checker::checkCollision( - path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, - current_debug_data.second)) { + is_safe = utils::path_safety_checker::checkCollision( + path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, + current_debug_data.second); + if (!is_safe) { path_safety_status.is_safe = false; marker_utils::updateCollisionCheckDebugMap( debug_data, current_debug_data, path_safety_status.is_safe); @@ -1380,8 +1388,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( path, current_pose, common_parameters.vehicle_info, obj_polygon); } } - marker_utils::updateCollisionCheckDebugMap( - debug_data, current_debug_data, path_safety_status.is_safe); + marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); } return path_safety_status; 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 9fb7d0acdf432..e8077edf10ae8 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 @@ -117,8 +117,8 @@ Polygon2d createExtendedPolygon( } Polygon2d createExtendedPolygon( - const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin, - CollisionCheckDebug & debug) + const Pose & obj_pose, const Shape & shape, const double lon_length, + [[maybe_unused]] const double lat_margin, CollisionCheckDebug & debug) { const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, shape); if (obj_polygon.outer().empty()) { @@ -140,8 +140,9 @@ Polygon2d createExtendedPolygon( } const double lon_offset = max_x + lon_length; - const double left_lat_offset = max_y + lat_margin; - const double right_lat_offset = min_y - lat_margin; + constexpr double percentage = 0.0; + const double left_lat_offset = max_y + lat_margin * percentage; + const double right_lat_offset = min_y - lat_margin * percentage; { debug.extended_polygon_lon_offset = lon_offset;