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 eb6ea5996cc6f..a82cff16756f8 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 @@ -106,6 +106,7 @@ struct FreespacePlannerDebugData struct GoalPlannerDebugData { FreespacePlannerDebugData freespace_planner{}; + std::vector ego_polygons_expanded{}; }; class GoalPlannerModule : public SceneModuleInterface 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 9cc4afe22bdce..4983060aa1c0a 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 @@ -110,6 +110,10 @@ bool checkCollision( const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, const double hysteresis_factor, CollisionCheckDebug & debug); +std::vector generatePolygonsWithStoppingAndInertialMargin( + const PathWithLaneId & ego_path, const double base_to_front, const double base_to_rear, + const double width, const double maximum_deceleration, const double max_extra_stopping_margin); + /** * @brief Iterate the points in the ego and target's predicted path and * perform safety check for each of the iterated points. @@ -133,14 +137,12 @@ std::vector getCollidedPolygons( const double hysteresis_factor, CollisionCheckDebug & debug); /** - * @brief Check collision between ego path footprints with extra longitudinal stopping margin and - * objects. + * @brief Check collision between ego polygons with margin and objects. * @return Has collision or not */ -bool checkCollisionWithExtraStoppingMargin( - const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, - const double base_to_front, const double base_to_rear, const double width, - const double maximum_deceleration, const double margin, const double max_stopping_margin); +bool checkCollisionWithMargin( + const std::vector & ego_polygons, const PredictedObjects & dynamic_objects, + const double collision_check_margin); } // 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 574a1a38fc66a..ce88760212d90 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 @@ -1296,28 +1296,32 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const } } - if (parameters_->use_object_recognition) { - const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( - *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length); - const auto [pull_over_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_over_lanes); - const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( - pull_over_lane_objects, parameters_->th_moving_object_velocity); - const auto common_parameters = planner_data_->parameters; - const double base_link2front = common_parameters.base_link2front; - const double base_link2rear = common_parameters.base_link2rear; - const double vehicle_width = common_parameters.vehicle_width; - if (utils::path_safety_checker::checkCollisionWithExtraStoppingMargin( - path, pull_over_lane_stop_objects, base_link2front, base_link2rear, vehicle_width, - parameters_->maximum_deceleration, parameters_->object_recognition_collision_check_margin, - parameters_->object_recognition_collision_check_max_extra_stopping_margin)) { - return true; - } + if (!parameters_->use_object_recognition) { + return false; } - return false; + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); + const auto [pull_over_lane_objects, others] = + utils::path_safety_checker::separateObjectsByLanelets( + *(planner_data_->dynamic_object), pull_over_lanes); + const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + pull_over_lane_objects, parameters_->th_moving_object_velocity); + const auto common_parameters = planner_data_->parameters; + const double base_link2front = common_parameters.base_link2front; + const double base_link2rear = common_parameters.base_link2rear; + const double vehicle_width = common_parameters.vehicle_width; + + const auto ego_polygons_expanded = + utils::path_safety_checker::generatePolygonsWithStoppingAndInertialMargin( + path, base_link2front, base_link2rear, vehicle_width, parameters_->maximum_deceleration, + parameters_->object_recognition_collision_check_max_extra_stopping_margin); + debug_data_.ego_polygons_expanded = ego_polygons_expanded; + + return utils::path_safety_checker::checkCollisionWithMargin( + ego_polygons_expanded, pull_over_lane_stop_objects, + parameters_->object_recognition_collision_check_margin); } bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) const @@ -1686,6 +1690,24 @@ void GoalPlannerModule::setDebugData() add( createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9)); } + + auto marker = tier4_autoware_utils::createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "detection_polygons", 0, Marker::LINE_LIST, + tier4_autoware_utils::createMarkerScale(0.01, 0.0, 0.0), + tier4_autoware_utils::createMarkerColor(0.0, 0.0, 1.0, 0.999)); + + for (const auto & ego_polygon : debug_data_.ego_polygons_expanded) { + for (size_t ep_idx = 0; ep_idx < ego_polygon.outer().size(); ++ep_idx) { + const auto & current_point = ego_polygon.outer().at(ep_idx); + const auto & next_point = ego_polygon.outer().at((ep_idx + 1) % ego_polygon.outer().size()); + + marker.points.push_back( + tier4_autoware_utils::createPoint(current_point.x(), current_point.y(), 0.0)); + marker.points.push_back( + tier4_autoware_utils::createPoint(next_point.x(), next_point.y(), 0.0)); + } + } + debug_marker_.markers.push_back(marker); } // safety check if (parameters_->safety_check_params.enable_safety_check) { @@ -1695,7 +1717,6 @@ void GoalPlannerModule::setDebugData() add(createPredictedPathMarkerArray( ego_predicted_path, vehicle_info_, "ego_predicted_path_goal_planner", 0, 0.0, 0.5, 0.9)); } - if (goal_planner_data_.filtered_objects.objects.size() > 0) { add(createObjectsMarkerArray( goal_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); 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 4e671f9f4b599..b44a3f841035d 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 @@ -364,27 +364,49 @@ std::vector getCollidedPolygons( 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, - const double maximum_deceleration, const double collision_check_margin, - const double max_extra_stopping_margin) + +std::vector generatePolygonsWithStoppingAndInertialMargin( + const PathWithLaneId & ego_path, const double base_to_front, const double base_to_rear, + const double width, const double maximum_deceleration, const double max_extra_stopping_margin) { - for (const auto & p : ego_path.points) { + std::vector polygons; + const auto curvatures = motion_utils::calcCurvature(ego_path.points); + + for (size_t i = 0; i < ego_path.points.size(); ++i) { + const auto p = ego_path.points.at(i); + const double extra_stopping_margin = std::min( std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / maximum_deceleration, max_extra_stopping_margin); + double extra_lateral_margin = (-1) * curvatures[i] * p.point.longitudinal_velocity_mps * + std::abs(p.point.longitudinal_velocity_mps); + extra_lateral_margin = + std::clamp(extra_lateral_margin, -extra_stopping_margin, extra_stopping_margin); + + const auto lateral_offset_pose = + tier4_autoware_utils::calcOffsetPose(p.point.pose, 0.0, extra_lateral_margin / 2.0, 0.0); const auto ego_polygon = tier4_autoware_utils::toFootprint( - p.point.pose, base_to_front + extra_stopping_margin, base_to_rear, width); + lateral_offset_pose, base_to_front + extra_stopping_margin, base_to_rear, + width + std::abs(extra_lateral_margin)); + polygons.push_back(ego_polygon); + } + return polygons; +} +bool checkCollisionWithMargin( + const std::vector & ego_polygons, const PredictedObjects & dynamic_objects, + const double collision_check_margin) +{ + for (const auto & ego_polygon : ego_polygons) { for (const auto & object : dynamic_objects.objects) { const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); const double distance = boost::geometry::distance(obj_polygon, ego_polygon); - if (distance < collision_check_margin) return true; + if (distance < collision_check_margin) { + return true; + } } } - return false; } } // namespace behavior_path_planner::utils::path_safety_checker