diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 3a4a44ef33edb..78113d333ec52 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -1954,10 +1954,13 @@ 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); + const auto extenal_velocity_limit_ptr = planner_data_->external_limit_max_velocity; + const auto max_velocity_limit = (extenal_velocity_limit_ptr) ? std::min(static_cast(extenal_velocity_limit_ptr->max_velocity), getCommonParam().max_vel) : getCommonParam().max_vel; + + 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 = utils::path_safety_checker::createObjectDebug(obj); @@ -1966,7 +1969,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( auto is_safe = true; for (const auto & obj_path : obj_predicted_paths) { const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons( - path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, + path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, max_velocity_limit, current_debug_data.second); if (collided_polygons.empty()) { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 99e312f98f100..dbab0d900a016 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -21,6 +21,7 @@ #include "behavior_path_planner_common/interface/steering_factor_interface.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include #include #include @@ -36,6 +37,7 @@ #include #include #include +#include #include #include #include @@ -103,6 +105,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr modified_goal_publisher_; rclcpp::Publisher::SharedPtr stop_reason_publisher_; rclcpp::Publisher::SharedPtr reroute_availability_publisher_; + rclcpp::Subscription::SharedPtr + external_limit_max_velocity_subscriber_; rclcpp::TimerBase::SharedPtr timer_; std::map::SharedPtr> path_candidate_publishers_; @@ -142,6 +146,9 @@ class BehaviorPathPlannerNode : public rclcpp::Node void onRoute(const LaneletRoute::ConstSharedPtr route_msg); void onOperationMode(const OperationModeState::ConstSharedPtr msg); void onLateralOffset(const LateralOffset::ConstSharedPtr msg); + void on_external_velocity_limiter( + const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); + SetParametersResult onSetParam(const std::vector & parameters); OnSetParametersCallbackHandle::SharedPtr m_set_param_res; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index e41061a2ffbd2..db93b67648985 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -22,8 +22,10 @@ #include #include +#include #include +#include #include #include #include @@ -112,6 +114,11 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod current_scenario_ = std::make_shared(*msg); }, createSubscriptionOptions(this)); + external_limit_max_velocity_subscriber_ = + create_subscription( + "/planning/scenario_planning/max_velocity", 1, + std::bind(&BehaviorPathPlannerNode::on_external_velocity_limiter, this, _1), + createSubscriptionOptions(this)); // route_handler vector_map_subscriber_ = create_subscription( @@ -817,6 +824,17 @@ void BehaviorPathPlannerNode::onOperationMode(const OperationModeState::ConstSha const std::lock_guard lock(mutex_pd_); planner_data_->operation_mode = msg; } + +void BehaviorPathPlannerNode::on_external_velocity_limiter( + const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) +{ + if(!msg){ + return; + } + + const std::lock_guard lock(mutex_pd_); + planner_data_->external_limit_max_velocity = msg; +} void BehaviorPathPlannerNode::onLateralOffset(const LateralOffset::ConstSharedPtr msg) { std::lock_guard lock(mutex_pd_); 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..2d19c6cfac824 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 @@ -25,6 +25,7 @@ #include #include #include +#include namespace behavior_path_planner::utils::path_safety_checker { @@ -560,7 +561,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 +571,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; @@ -599,7 +600,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)) {