diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp index d196f2bc551bb..09f3c2c66bcd4 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include @@ -65,6 +66,7 @@ using route_handler::RouteHandler; using tier4_planning_msgs::msg::LateralOffset; using PlanResult = PathWithLaneId::SharedPtr; using lanelet::TrafficLight; +using tier4_planning_msgs::msg::VelocityLimit; using unique_identifier_msgs::msg::UUID; struct TrafficSignalStamped @@ -161,6 +163,7 @@ struct PlannerData std::map traffic_light_id_map; BehaviorPathPlannerParameters parameters{}; drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{}; + VelocityLimit::ConstSharedPtr external_limit_max_velocity{}; mutable std::vector drivable_area_expansion_prev_path_poses{}; mutable std::vector drivable_area_expansion_prev_curvatures{}; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index 13009d5114439..53c4706e49c10 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -141,7 +141,7 @@ std::vector getCollidedPolygons( const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, - const double hysteresis_factor, CollisionCheckDebug & debug); + const double hysteresis_factor, const double max_velocity_limit, CollisionCheckDebug & debug); bool checkPolygonsIntersects( const std::vector & polys_1, const std::vector & polys_2); diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index e15a3c164bef1..6390c45376b37 100644 --- a/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -26,6 +26,8 @@ #include #include +#include + namespace behavior_path_planner::utils::path_safety_checker { @@ -560,7 +562,7 @@ bool checkCollision( { const auto collided_polygons = getCollidedPolygons( planned_path, predicted_ego_path, target_object, target_object_path, common_parameters, - rss_parameters, hysteresis_factor, debug); + rss_parameters, hysteresis_factor, std::numeric_limits::max(), debug); return collided_polygons.empty(); } @@ -570,7 +572,7 @@ std::vector getCollidedPolygons( const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, - double hysteresis_factor, CollisionCheckDebug & debug) + double hysteresis_factor, const double max_velocity_limit, CollisionCheckDebug & debug) { { debug.ego_predicted_path = predicted_ego_path; @@ -586,7 +588,7 @@ std::vector getCollidedPolygons( // get object information at current time const auto & obj_pose = obj_pose_with_poly.pose; const auto & obj_polygon = obj_pose_with_poly.poly; - const auto & object_velocity = obj_pose_with_poly.velocity; + const auto object_velocity = obj_pose_with_poly.velocity; // get ego information at current time // Note: we can create these polygons in advance. However, it can decrease the readability and @@ -599,7 +601,7 @@ std::vector getCollidedPolygons( } const auto & ego_pose = interpolated_data->pose; const auto & ego_polygon = interpolated_data->poly; - const auto & ego_velocity = interpolated_data->velocity; + const auto ego_velocity = std::min(interpolated_data->velocity, max_velocity_limit); // check overlap if (boost::geometry::overlaps(ego_polygon, obj_polygon)) {