diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index 583bdeaaf8dc5..e7f3b51bd2d26 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -28,15 +28,28 @@ # safety check safety_check: - expected_front_deceleration: -1.0 - expected_rear_deceleration: -1.0 - expected_front_deceleration_for_abort: -1.0 - expected_rear_deceleration_for_abort: -2.0 - rear_vehicle_reaction_time: 2.0 - rear_vehicle_safety_time_margin: 1.0 - lateral_distance_max_threshold: 2.0 - longitudinal_distance_min_threshold: 3.0 - longitudinal_velocity_delta_time: 0.8 + allow_loose_check_for_cancel: true + execution: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -1.0 + rear_vehicle_reaction_time: 2.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 + cancel: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -2.0 + rear_vehicle_reaction_time: 1.5 + rear_vehicle_safety_time_margin: 0.8 + lateral_distance_max_threshold: 1.0 + longitudinal_distance_min_threshold: 2.5 + longitudinal_velocity_delta_time: 0.6 + + # lane expansion for object filtering + lane_expansion: + left_offset: 0.0 # [m] + right_offset: 0.0 # [m] # lateral acceleration map lateral_acceleration: 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/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index d3555eae83539..e74a6d7b72b66 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -65,6 +65,8 @@ struct LaneChangeParameters bool check_objects_on_current_lanes{true}; bool check_objects_on_other_lanes{true}; bool use_all_predicted_path{false}; + double lane_expansion_left_offset{0.0}; + double lane_expansion_right_offset{0.0}; // regulatory elements bool regulate_on_crosswalk{false}; @@ -78,8 +80,9 @@ struct LaneChangeParameters utils::path_safety_checker::ObjectTypesToCheck object_types_to_check; // safety check - utils::path_safety_checker::RSSparams rss_params; - utils::path_safety_checker::RSSparams rss_params_for_abort; + bool allow_loose_check_for_cancel{true}; + utils::path_safety_checker::RSSparams rss_params{}; + utils::path_safety_checker::RSSparams rss_params_for_abort{}; // abort LaneChangeCancelParameters cancel; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index 18f53b49d21d2..70ea047f2a19f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -188,5 +188,27 @@ std::optional createPolygon( ExtendedPredictedObject transform( const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, const LaneChangeParameters & lane_change_parameters); + +bool isCollidedPolygonsInLanelet( + const std::vector & collided_polygons, const lanelet::ConstLanelets & lanes); + +/** + * @brief Generates expanded lanelets based on the given direction and offsets. + * + * Expands the provided lanelets in either the left or right direction based on + * the specified direction. If the direction is 'LEFT', the lanelets are expanded + * using the left_offset; if 'RIGHT', they are expanded using the right_offset. + * Otherwise, no expansion occurs. + * + * @param lanes The lanelets to be expanded. + * @param direction The direction of expansion: either LEFT or RIGHT. + * @param left_offset The offset value for left expansion. + * @param right_offset The offset value for right expansion. + * @return lanelet::ConstLanelets A collection of expanded lanelets. + */ +lanelet::ConstLanelets generateExpandedLanelets( + const lanelet::ConstLanelets & lanes, const Direction direction, const double left_offset, + const double right_offset); + } // namespace behavior_path_planner::utils::lane_change #endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__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 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/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index a9404038d831a..c9cedca39aca0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -83,7 +83,6 @@ boost::optional calcInterpolatedPoseWithVelocity( boost::optional getInterpolatedPoseWithVelocityAndPolygonStamped( const std::vector & pred_path, const double current_time, const VehicleInfo & ego_info); - /** * @brief Iterate the points in the ego and target's predicted path and * perform safety check for each of the iterated points. @@ -106,6 +105,28 @@ bool checkCollision( const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, const double hysteresis_factor, CollisionCheckDebug & debug); +/** + * @brief Iterate the points in the ego and target's predicted path and + * perform safety check for each of the iterated points. + * @param planned_path The predicted path of the ego vehicle. + * @param predicted_ego_path Ego vehicle's predicted path + * @param ego_current_velocity Current velocity of the ego vehicle. + * @param target_object The predicted object to check collision with. + * @param target_object_path The predicted path of the target object. + * @param common_parameters The common parameters used in behavior path planner. + * @param front_object_deceleration The deceleration of the object in the front.(used in RSS) + * @param rear_object_deceleration The deceleration of the object in the rear.(used in RSS) + * @param debug The debug information for collision checking. + * @return a list of polygon when collision is expected. + */ +std::vector getCollidedPolygons( + const PathWithLaneId & planned_path, + const std::vector & predicted_ego_path, + const ExtendedPredictedObject & target_object, + const PredictedPathWithPolygon & target_object_path, + const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, + const double hysteresis_factor, CollisionCheckDebug & debug); + /** * @brief Check collision between ego path footprints with extra longitudinal stopping margin and * objects. 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/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 8d90a0a13179c..8b865b37b6969 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -74,7 +74,10 @@ LaneChangeModuleManager::LaneChangeModuleManager( getOrDeclareParameter(*node, parameter("check_objects_on_other_lanes")); p.use_all_predicted_path = getOrDeclareParameter(*node, parameter("use_all_predicted_path")); - + p.lane_expansion_left_offset = + getOrDeclareParameter(*node, parameter("safety_check.lane_expansion.left_offset")); + p.lane_expansion_right_offset = + getOrDeclareParameter(*node, parameter("safety_check.lane_expansion.right_offset")); // lane change regulations p.regulate_on_crosswalk = getOrDeclareParameter(*node, parameter("regulation.crosswalk")); p.regulate_on_intersection = @@ -86,35 +89,41 @@ LaneChangeModuleManager::LaneChangeModuleManager( p.stop_time_threshold = getOrDeclareParameter(*node, parameter("stuck_detection.stop_time")); + // safety check + p.allow_loose_check_for_cancel = + getOrDeclareParameter(*node, parameter("safety_check.allow_loose_check_for_cancel")); + p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.longitudinal_distance_min_threshold")); + *node, parameter("safety_check.execution.longitudinal_distance_min_threshold")); + p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter("safety_check.execution.longitudinal_distance_min_threshold")); p.rss_params.longitudinal_velocity_delta_time = getOrDeclareParameter( - *node, parameter("safety_check.longitudinal_velocity_delta_time")); - p.rss_params.front_vehicle_deceleration = - getOrDeclareParameter(*node, parameter("safety_check.expected_front_deceleration")); - p.rss_params.rear_vehicle_deceleration = - getOrDeclareParameter(*node, parameter("safety_check.expected_rear_deceleration")); - p.rss_params.rear_vehicle_reaction_time = - getOrDeclareParameter(*node, parameter("safety_check.rear_vehicle_reaction_time")); - p.rss_params.rear_vehicle_safety_time_margin = - getOrDeclareParameter(*node, parameter("safety_check.rear_vehicle_safety_time_margin")); - p.rss_params.lateral_distance_max_threshold = - getOrDeclareParameter(*node, parameter("safety_check.lateral_distance_max_threshold")); + *node, parameter("safety_check.execution.longitudinal_velocity_delta_time")); + p.rss_params.front_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.execution.expected_front_deceleration")); + p.rss_params.rear_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.execution.expected_rear_deceleration")); + p.rss_params.rear_vehicle_reaction_time = getOrDeclareParameter( + *node, parameter("safety_check.execution.rear_vehicle_reaction_time")); + p.rss_params.rear_vehicle_safety_time_margin = getOrDeclareParameter( + *node, parameter("safety_check.execution.rear_vehicle_safety_time_margin")); + p.rss_params.lateral_distance_max_threshold = getOrDeclareParameter( + *node, parameter("safety_check.execution.lateral_distance_max_threshold")); p.rss_params_for_abort.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.longitudinal_distance_min_threshold")); + *node, parameter("safety_check.cancel.longitudinal_distance_min_threshold")); p.rss_params_for_abort.longitudinal_velocity_delta_time = getOrDeclareParameter( - *node, parameter("safety_check.longitudinal_velocity_delta_time")); + *node, parameter("safety_check.cancel.longitudinal_velocity_delta_time")); p.rss_params_for_abort.front_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.expected_front_deceleration_for_abort")); + *node, parameter("safety_check.cancel.expected_front_deceleration")); p.rss_params_for_abort.rear_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.expected_rear_deceleration_for_abort")); - p.rss_params_for_abort.rear_vehicle_reaction_time = - getOrDeclareParameter(*node, parameter("safety_check.rear_vehicle_reaction_time")); - p.rss_params_for_abort.rear_vehicle_safety_time_margin = - getOrDeclareParameter(*node, parameter("safety_check.rear_vehicle_safety_time_margin")); - p.rss_params_for_abort.lateral_distance_max_threshold = - getOrDeclareParameter(*node, parameter("safety_check.lateral_distance_max_threshold")); + *node, parameter("safety_check.cancel.expected_rear_deceleration")); + p.rss_params_for_abort.rear_vehicle_reaction_time = getOrDeclareParameter( + *node, parameter("safety_check.cancel.rear_vehicle_reaction_time")); + p.rss_params_for_abort.rear_vehicle_safety_time_margin = getOrDeclareParameter( + *node, parameter("safety_check.cancel.rear_vehicle_safety_time_margin")); + p.rss_params_for_abort.lateral_distance_max_threshold = getOrDeclareParameter( + *node, parameter("safety_check.cancel.lateral_distance_max_threshold")); // target object { @@ -159,6 +168,26 @@ LaneChangeModuleManager::LaneChangeModuleManager( exit(EXIT_FAILURE); } + // validation of safety check parameters + // if loosely check is not allowed, lane change module will keep on chattering and canceling, and + // false positive situation might occur + if (!p.allow_loose_check_for_cancel) { + if ( + p.rss_params.front_vehicle_deceleration > p.rss_params_for_abort.front_vehicle_deceleration || + p.rss_params.rear_vehicle_deceleration > p.rss_params_for_abort.rear_vehicle_deceleration || + p.rss_params.rear_vehicle_reaction_time > p.rss_params_for_abort.rear_vehicle_reaction_time || + p.rss_params.rear_vehicle_safety_time_margin > + p.rss_params_for_abort.rear_vehicle_safety_time_margin || + p.rss_params.lateral_distance_max_threshold > + p.rss_params_for_abort.lateral_distance_max_threshold || + p.rss_params.longitudinal_distance_min_threshold > + p.rss_params_for_abort.longitudinal_distance_min_threshold || + p.rss_params.longitudinal_velocity_delta_time > + p.rss_params_for_abort.longitudinal_velocity_delta_time) { + RCLCPP_FATAL_STREAM(logger_, "abort parameter might be loose... Terminating the program..."); + exit(EXIT_FAILURE); + } + } if (p.cancel.delta_time < 1.0) { RCLCPP_WARN_STREAM( logger_, "cancel.delta_time: " << p.cancel.delta_time 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 e392e67428b08..cf6f2fc01773a 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 @@ -670,10 +670,30 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( const auto current_polygon = utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits::max()); - const auto target_polygon = - utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits::max()); - const auto target_backward_polygon = utils::lane_change::createPolygon( - target_backward_lanes, 0.0, std::numeric_limits::max()); + const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( + target_lanes, direction_, lane_change_parameters_->lane_expansion_left_offset, + lane_change_parameters_->lane_expansion_right_offset); + const auto target_polygon = utils::lane_change::createPolygon( + expanded_target_lanes, 0.0, std::numeric_limits::max()); + const auto dist_ego_to_current_lanes_center = + lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); + std::vector> target_backward_polygons; + for (const auto & target_backward_lane : target_backward_lanes) { + // Check to see is target_backward_lane is in current_lanes + // Without this check, current lane object might be treated as target lane object + const auto is_current_lane = [&](const lanelet::ConstLanelet & current_lane) { + return current_lane.id() == target_backward_lane.id(); + }; + + if (std::any_of(current_lanes.begin(), current_lanes.end(), is_current_lane)) { + continue; + } + + lanelet::ConstLanelets lanelet{target_backward_lane}; + auto lane_polygon = + utils::lane_change::createPolygon(lanelet, 0.0, std::numeric_limits::max()); + target_backward_polygons.push_back(lane_polygon); + } auto filtered_objects = objects; @@ -702,6 +722,14 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( min_dist_ego_to_obj = std::min(dist_ego_to_obj, min_dist_ego_to_obj); } + const auto is_lateral_far = [&]() { + const auto dist_object_to_current_lanes_center = + lanelet::utils::getLateralDistanceToClosestLanelet( + current_lanes, object.kinematics.initial_pose_with_covariance.pose); + const auto lateral = dist_object_to_current_lanes_center - dist_ego_to_current_lanes_center; + return std::abs(lateral) > (common_parameters.vehicle_width / 2); + }; + // ignore static object that are behind the ego vehicle if (obj_velocity_norm < 1.0 && max_dist_ego_to_obj < 0.0) { continue; @@ -712,14 +740,22 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( // TODO(watanabe): ignore static parked object that are in front of the ego vehicle in target // lanes - filtered_obj_indices.target_lane.push_back(i); - continue; + if (max_dist_ego_to_obj >= 0 || is_lateral_far()) { + filtered_obj_indices.target_lane.push_back(i); + continue; + } } + const auto check_backward_polygon = [&obj_polygon](const auto & target_backward_polygon) { + return target_backward_polygon && + boost::geometry::intersects(target_backward_polygon.value(), obj_polygon); + }; + // check if the object intersects with target backward lanes if ( - target_backward_polygon && - boost::geometry::intersects(target_backward_polygon.value(), obj_polygon)) { + !target_backward_polygons.empty() && + std::any_of( + target_backward_polygons.begin(), target_backward_polygons.end(), check_backward_polygon)) { filtered_obj_indices.target_lane.push_back(i); continue; } @@ -908,6 +944,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); @@ -1124,6 +1161,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( @@ -1420,23 +1458,41 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( target_objects.other_lane.end()); } + const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( + lane_change_path.info.target_lanes, direction_, + lane_change_parameters_->lane_expansion_left_offset, + lane_change_parameters_->lane_expansion_right_offset); + for (const auto & obj : collision_check_objects) { 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); 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)) { - path_safety_status.is_safe = false; - marker_utils::updateCollisionCheckDebugMap( - debug_data, current_debug_data, path_safety_status.is_safe); - const auto & obj_pose = obj.initial_pose.pose; - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); - path_safety_status.is_object_coming_from_rear |= - !utils::path_safety_checker::isTargetObjectFront( - path, current_pose, common_parameters.vehicle_info, obj_polygon); + const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons( + path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, + current_debug_data.second); + + if (collided_polygons.empty()) { + continue; } + + const auto collision_in_current_lanes = utils::lane_change::isCollidedPolygonsInLanelet( + collided_polygons, lane_change_path.info.current_lanes); + const auto collision_in_target_lanes = + utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_lanes); + + if (!collision_in_current_lanes && !collision_in_target_lanes) { + continue; + } + + path_safety_status.is_safe = false; + marker_utils::updateCollisionCheckDebugMap( + debug_data, current_debug_data, path_safety_status.is_safe); + const auto & obj_pose = obj.initial_pose.pose; + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); + path_safety_status.is_object_coming_from_rear |= + !utils::path_safety_checker::isTargetObjectFront( + path, current_pose, common_parameters.vehicle_info, obj_polygon); } marker_utils::updateCollisionCheckDebugMap( debug_data, current_debug_data, path_safety_status.is_safe); diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 1395f07968c2d..269eb194988e1 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -1111,4 +1111,25 @@ ExtendedPredictedObject transform( return extended_object; } + +bool isCollidedPolygonsInLanelet( + const std::vector & collided_polygons, const lanelet::ConstLanelets & lanes) +{ + const auto lanes_polygon = createPolygon(lanes, 0.0, std::numeric_limits::max()); + + const auto is_in_lanes = [&](const auto & collided_polygon) { + return lanes_polygon && boost::geometry::intersects(lanes_polygon.value(), collided_polygon); + }; + + return std::any_of(collided_polygons.begin(), collided_polygons.end(), is_in_lanes); +} + +lanelet::ConstLanelets generateExpandedLanelets( + const lanelet::ConstLanelets & lanes, const Direction direction, const double left_offset, + const double right_offset) +{ + const auto left_extend_offset = (direction == Direction::LEFT) ? left_offset : 0.0; + const auto right_extend_offset = (direction == Direction::RIGHT) ? -right_offset : 0.0; + return lanelet::utils::getExpandedLanelets(lanes, left_extend_offset, right_extend_offset); +} } // namespace behavior_path_planner::utils::lane_change 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 1838dc6bdc07b..6ea21faef2e2f 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 @@ -246,6 +246,20 @@ boost::optional getInterpolatedPoseWithVeloci } bool checkCollision( + const PathWithLaneId & planned_path, + const std::vector & predicted_ego_path, + const ExtendedPredictedObject & target_object, + const PredictedPathWithPolygon & target_object_path, + const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, + const double hysteresis_factor, CollisionCheckDebug & debug) +{ + const auto collided_polygons = getCollidedPolygons( + planned_path, predicted_ego_path, target_object, target_object_path, common_parameters, + rss_parameters, hysteresis_factor, debug); + return collided_polygons.empty(); +} + +std::vector getCollidedPolygons( [[maybe_unused]] const PathWithLaneId & planned_path, const std::vector & predicted_ego_path, const ExtendedPredictedObject & target_object, @@ -259,6 +273,8 @@ bool checkCollision( debug.current_obj_pose = target_object.initial_pose.pose; } + std::vector collided_polygons{}; + collided_polygons.reserve(target_object_path.path.size()); for (const auto & obj_pose_with_poly : target_object_path.path) { const auto & current_time = obj_pose_with_poly.time; @@ -290,7 +306,8 @@ bool checkCollision( // check overlap if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { debug.unsafe_reason = "overlap_polygon"; - return false; + collided_polygons.push_back(obj_polygon); + continue; } // compute which one is at the front of the other @@ -329,13 +346,12 @@ bool checkCollision( // check overlap with extended polygon if (boost::geometry::overlaps(extended_ego_polygon, extended_obj_polygon)) { debug.unsafe_reason = "overlap_extended_polygon"; - return false; + collided_polygons.push_back(obj_polygon); } } - return true; + return collided_polygons; } - bool checkCollisionWithExtraStoppingMargin( const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, const double base_to_front, const double base_to_rear, const double width,