diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index 19550bac5c196..cf5bf683f7b73 100644 --- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -160,6 +160,7 @@ safety_check_params: # safety check configuration enable_safety_check: true + method: "integral_predicted_polygon" keep_unsafe_time: 3.0 # collision check parameters check_all_predicted_path: true @@ -170,10 +171,15 @@ lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.8 + integral_predicted_polygon_params: + forward_margin: 1.0 + backward_margin: 1.0 + lat_margin: 1.0 + time_horizon: 10.0 # hysteresis factor to expand/shrink polygon with the value hysteresis_factor_expand_rate: 1.0 # temporary - backward_path_length: 30.0 + backward_path_length: 100.0 forward_path_length: 100.0 # debug diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index bce41a46cc963..97a5fdd966b8f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -244,7 +244,6 @@ struct PreviousPullOverData stop_path = nullptr; stop_path_after_approval = nullptr; found_path = false; - is_safe = false; safety_status = SafetyStatus{}; has_decided_path = false; } @@ -252,7 +251,6 @@ struct PreviousPullOverData std::shared_ptr stop_path{nullptr}; std::shared_ptr stop_path_after_approval{nullptr}; bool found_path{false}; - bool is_safe{false}; SafetyStatus safety_status{}; bool has_decided_path{false}; }; 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 29fc44b439bbc..c4a60332fe4ff 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 @@ -134,6 +134,14 @@ struct RSSparams double rear_vehicle_deceleration{0.0}; ///< brake parameter }; +struct IntegralPredictedPolygonParams +{ + double forward_margin{0.0}; ///< Forward margin for extended ego polygon for collision check. + double backward_margin{0.0}; ///< Backward margin for extended ego polygon for collision check. + double lat_margin{0.0}; ///< Lateral margin for extended ego polygon for collision check. + double time_horizon{0.0}; ///< Time horizon for object's prediction. +}; + /** * @brief Parameters for generating the ego vehicle's predicted path. */ @@ -173,12 +181,15 @@ struct ObjectsFilteringParams struct SafetyCheckParams { bool enable_safety_check{false}; ///< Enable safety checks. - double keep_unsafe_time{0.0}; ///< Time to keep unsafe before changing to safe. + std::string method{"RSS"}; /// Method to use for safety checks. + /// possible values: ["RSS", "integral_predicted_polygon"] + double keep_unsafe_time{0.0}; ///< Time to keep unsafe before changing to safe. double hysteresis_factor_expand_rate{ 0.0}; ///< Hysteresis factor to expand/shrink polygon with the value. double backward_path_length{0.0}; ///< Length of the backward lane for path generation. double forward_path_length{0.0}; ///< Length of the forward path lane for path generation. RSSparams rss_params{}; ///< Parameters related to the RSS model. + IntegralPredictedPolygonParams integral_predicted_polygon_params{}; ///< Parameters for polygon. bool publish_debug_marker{false}; ///< Option to publish debug markers. }; 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 7a9bc3c9b3b17..a4851e97d5008 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 @@ -85,6 +85,20 @@ boost::optional calcInterpolatedPoseWithVelocity( boost::optional getInterpolatedPoseWithVelocityAndPolygonStamped( const std::vector & pred_path, const double current_time, const VehicleInfo & ego_info); +boost::optional getInterpolatedPoseWithVelocityAndPolygonStamped( + const std::vector & pred_path, const double current_time, + const Shape & shape); +template +std::vector filterPredictedPathByTimeHorizon( + const std::vector & path, const double time_horizon, const F & interpolateFunc); +std::vector filterPredictedPathByTimeHorizon( + const std::vector & path, const double time_horizon); +ExtendedPredictedObject filterObjectPredictedPathByTimeHorizon( + const ExtendedPredictedObject & object, const double time_horizon, + const bool check_all_predicted_path); +ExtendedPredictedObjects filterObjectPredictedPathByTimeHorizon( + const ExtendedPredictedObjects & objects, const double time_horizon, + const bool check_all_predicted_path); /** * @brief Iterate the points in the ego and target's predicted path and * perform safety check for each of the iterated points. @@ -131,12 +145,15 @@ std::vector getCollidedPolygons( bool checkPolygonsIntersects( const std::vector & polys_1, const std::vector & polys_2); +bool checkSafetyWithIntegralPredictedPolygon( + const std::vector & ego_predicted_path, const VehicleInfo & vehicle_info, + const ExtendedPredictedObjects & objects, const bool check_all_predicted_path, + const IntegralPredictedPolygonParams & params, CollisionCheckDebugMap & debug_map); // debug CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj); void updateCollisionCheckDebugMap( CollisionCheckDebugMap & debug_map, CollisionCheckDebugPair & object_debug, bool is_safe); - } // namespace behavior_path_planner::utils::path_safety_checker #endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 63ecb289a4db9..b2692200557ed 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -769,7 +769,7 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) const void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output) const { // safe or not safe(no feasible stop_path found) -> not_safe: try to find new feasible stop_path - if (prev_data_.is_safe || !prev_data_.stop_path_after_approval) { + if (prev_data_.safety_status.is_safe || !prev_data_.stop_path_after_approval) { auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); const auto stop_path = behavior_path_planner::utils::start_goal_planner_common::generateFeasibleStopPath( @@ -1817,8 +1817,23 @@ std::pair GoalPlannerModule::isSafePath() const const double hysteresis_factor = prev_data_.safety_status.is_safe ? 1.0 : parameters_->hysteresis_factor_expand_rate; - const bool current_is_safe = checkSafetyWithRSS( - pull_over_path, ego_predicted_path, target_objects_on_lane.on_current_lane, hysteresis_factor); + const bool current_is_safe = std::invoke([&]() { + if (parameters_->safety_check_params.method == "RSS") { + return checkSafetyWithRSS( + pull_over_path, ego_predicted_path, target_objects_on_lane.on_current_lane, + hysteresis_factor); + } else if (parameters_->safety_check_params.method == "integral_predicted_polygon") { + return utils::path_safety_checker::checkSafetyWithIntegralPredictedPolygon( + ego_predicted_path, vehicle_info_, target_objects_on_lane.on_current_lane, + objects_filtering_params_->check_all_predicted_path, + parameters_->safety_check_params.integral_predicted_polygon_params, + goal_planner_data_.collision_check); + } + RCLCPP_ERROR( + getLogger(), " [pull_over] invalid safety check method: %s", + parameters_->safety_check_params.method.c_str()); + throw std::domain_error("[pull_over] invalid safety check method"); + }); /*　　 *　　 ==== is_safe @@ -1940,11 +1955,34 @@ void GoalPlannerModule::setDebugData() goal_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); } - add(showSafetyCheckInfo(goal_planner_data_.collision_check, "object_debug_info")); + if (parameters_->safety_check_params.method == "RSS") { + add(showSafetyCheckInfo(goal_planner_data_.collision_check, "object_debug_info")); + } add(showPredictedPath(goal_planner_data_.collision_check, "ego_predicted_path")); add(showPolygon(goal_planner_data_.collision_check, "ego_and_target_polygon_relation")); utils::start_goal_planner_common::initializeCollisionCheckDebugMap( goal_planner_data_.collision_check); + + // visualize safety status maker + { + visualization_msgs::msg::MarkerArray marker_array{}; + const auto color = prev_data_.safety_status.is_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); + auto marker = createDefaultMarker( + header.frame_id, header.stamp, "safety_status", 0, + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); + + marker.pose = planner_data_->self_odometry->pose.pose; + marker.text += "is_safe: " + std::to_string(prev_data_.safety_status.is_safe) + "\n"; + if (prev_data_.safety_status.safe_start_time) { + const double elapsed_time_from_safe_start = + (clock_->now() - prev_data_.safety_status.safe_start_time.value()).seconds(); + marker.text += + "elapsed_time_from_safe_start: " + std::to_string(elapsed_time_from_safe_start) + "\n"; + } + marker_array.markers.push_back(marker); + add(marker_array); + } } // Visualize planner type text diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index 8baeb10ed1b85..83c0d7002936c 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -336,6 +336,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( node->declare_parameter(safety_check_ns + "enable_safety_check"); p.safety_check_params.keep_unsafe_time = node->declare_parameter(safety_check_ns + "keep_unsafe_time"); + p.safety_check_params.method = node->declare_parameter(safety_check_ns + "method"); p.safety_check_params.hysteresis_factor_expand_rate = node->declare_parameter(safety_check_ns + "hysteresis_factor_expand_rate"); p.safety_check_params.backward_path_length = @@ -361,6 +362,19 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( node->declare_parameter(rss_ns + "longitudinal_velocity_delta_time"); } + // IntegralPredictedPolygonParams + std::string integral_ns = safety_check_ns + "integral_predicted_polygon_params."; + { + p.safety_check_params.integral_predicted_polygon_params.forward_margin = + node->declare_parameter(integral_ns + "forward_margin"); + p.safety_check_params.integral_predicted_polygon_params.backward_margin = + node->declare_parameter(integral_ns + "backward_margin"); + p.safety_check_params.integral_predicted_polygon_params.lat_margin = + node->declare_parameter(integral_ns + "lat_margin"); + p.safety_check_params.integral_predicted_polygon_params.time_horizon = + node->declare_parameter(integral_ns + "time_horizon"); + } + // debug { const std::string ns = base_ns + "debug."; 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 a29eeb070ce88..473a0489a14a9 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 @@ -14,13 +14,16 @@ #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "interpolation/linear_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include "tier4_autoware_utils/ros/uuid_helper.hpp" +#include #include #include +#include #include namespace behavior_path_planner::utils::path_safety_checker @@ -181,6 +184,27 @@ Polygon2d createExtendedPolygon( : tier4_autoware_utils::inverseClockwise(polygon); } +std::vector createExtendedPolygonsFromPoseWithVelocityStamped( + const std::vector & predicted_path, const VehicleInfo & vehicle_info, + const double forward_margin, const double backward_margin, const double lat_margin) +{ + std::vector polygons{}; + polygons.reserve(predicted_path.size()); + + for (const auto & elem : predicted_path) { + const auto & pose = elem.pose; + const double base_to_front = vehicle_info.max_longitudinal_offset_m + forward_margin; + const double base_to_rear = vehicle_info.rear_overhang_m + backward_margin; + const double width = vehicle_info.vehicle_width_m + lat_margin * 2; + + const auto polygon = + tier4_autoware_utils::toFootprint(pose, base_to_front, base_to_rear, width); + polygons.push_back(polygon); + } + + return polygons; +} + PredictedPath convertToPredictedPath( const std::vector & path, const double time_resolution) { @@ -274,6 +298,147 @@ boost::optional getInterpolatedPoseWithVeloci return PoseWithVelocityAndPolygonStamped{current_time, pose, velocity, ego_polygon}; } +boost::optional getInterpolatedPoseWithVelocityAndPolygonStamped( + const std::vector & pred_path, const double current_time, + const Shape & shape) +{ + auto toPoseWithVelocityStampedVector = [](const auto & pred_path) { + std::vector path; + path.reserve(pred_path.size()); + for (const auto & elem : pred_path) { + path.push_back(PoseWithVelocityStamped{elem.time, elem.pose, elem.velocity}); + } + return path; + }; + + const auto interpolation_result = + calcInterpolatedPoseWithVelocity(toPoseWithVelocityStampedVector(pred_path), current_time); + + if (!interpolation_result) { + return {}; + } + + const auto & pose = interpolation_result->pose; + const auto & velocity = interpolation_result->velocity; + + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(pose, shape); + + return PoseWithVelocityAndPolygonStamped{current_time, pose, velocity, obj_polygon}; +} + +template +std::vector filterPredictedPathByTimeHorizon( + const std::vector & path, const double time_horizon, const F & interpolateFunc) +{ + std::vector filtered_path; + + for (const auto & elem : path) { + if (elem.time < time_horizon) { + filtered_path.push_back(elem); + } else { + break; + } + } + + const auto interpolated_opt = interpolateFunc(path, time_horizon); + if (interpolated_opt) { + filtered_path.push_back(*interpolated_opt); + } + + return filtered_path; +}; + +std::vector filterPredictedPathByTimeHorizon( + const std::vector & path, const double time_horizon) +{ + return filterPredictedPathByTimeHorizon( + path, time_horizon, [](const auto & path, const auto & time) { + return calcInterpolatedPoseWithVelocity(path, time); + }); +} + +ExtendedPredictedObject filterObjectPredictedPathByTimeHorizon( + const ExtendedPredictedObject & object, const double time_horizon, + const bool check_all_predicted_path) +{ + auto filtered_object = object; + auto filtered_predicted_paths = getPredictedPathFromObj(object, check_all_predicted_path); + + for (auto & predicted_path : filtered_predicted_paths) { + // path is vector of polygon + const auto filtered_path = filterPredictedPathByTimeHorizon( + predicted_path.path, time_horizon, [&object](const auto & poses, double t) { + return getInterpolatedPoseWithVelocityAndPolygonStamped(poses, t, object.shape); + }); + predicted_path.path = filtered_path; + } + + filtered_object.predicted_paths = filtered_predicted_paths; + return filtered_object; +} + +ExtendedPredictedObjects filterObjectPredictedPathByTimeHorizon( + const ExtendedPredictedObjects & objects, const double time_horizon, + const bool check_all_predicted_path) +{ + ExtendedPredictedObjects filtered_objects; + filtered_objects.reserve(objects.size()); + + for (const auto & object : objects) { + filtered_objects.push_back( + filterObjectPredictedPathByTimeHorizon(object, time_horizon, check_all_predicted_path)); + } + + return filtered_objects; +} + +bool checkSafetyWithIntegralPredictedPolygon( + const std::vector & ego_predicted_path, const VehicleInfo & vehicle_info, + const ExtendedPredictedObjects & objects, const bool check_all_predicted_path, + const IntegralPredictedPolygonParams & params, CollisionCheckDebugMap & debug_map) +{ + const std::vector filtered_ego_path = filterPredictedPathByTimeHorizon( + ego_predicted_path, params.time_horizon); // path is vector of pose + const std::vector extended_ego_polygons = + createExtendedPolygonsFromPoseWithVelocityStamped( + filtered_ego_path, vehicle_info, params.forward_margin, params.backward_margin, + params.lat_margin); + + const ExtendedPredictedObjects filtered_path_objects = filterObjectPredictedPathByTimeHorizon( + objects, params.time_horizon, check_all_predicted_path); // path is vector of polygon + + Polygon2d ego_integral_polygon{}; + for (const auto & ego_polygon : extended_ego_polygons) { + std::vector unions{}; + boost::geometry::union_(ego_integral_polygon, ego_polygon, unions); + if (!unions.empty()) { + ego_integral_polygon = unions.front(); + boost::geometry::correct(ego_integral_polygon); + } + } + + // check collision + for (const auto & object : filtered_path_objects) { + CollisionCheckDebugPair debug_pair = createObjectDebug(object); + for (const auto & path : object.predicted_paths) { + for (const auto & pose_with_poly : path.path) { + if (boost::geometry::overlaps(ego_integral_polygon, pose_with_poly.poly)) { + { + debug_pair.second.ego_predicted_path = ego_predicted_path; // raw path + debug_pair.second.obj_predicted_path = path.path; // raw path + debug_pair.second.extended_obj_polygon = pose_with_poly.poly; + debug_pair.second.extended_ego_polygon = + ego_integral_polygon; // time filtered extended polygon + updateCollisionCheckDebugMap(debug_map, debug_pair, false); + } + return false; + } + } + } + } + return true; +} + bool checkCollision( const PathWithLaneId & planned_path, const std::vector & predicted_ego_path,