From 071333efab83b646ff0ef650cc22d107db8a6fc6 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Tue, 9 Apr 2024 13:01:35 +0900 Subject: [PATCH] feat(lane_change): use external velocity limit in safety check (#6760) * feat(lane_change): use external velocity limit in safety check Signed-off-by: Muhammad Zulfaqar * style(pre-commit): autofix * Minor refactoring Signed-off-by: Muhammad Zulfaqar * Fix spell check and remove headers Signed-off-by: Muhammad Zulfaqar Azmi * Add warning Signed-off-by: Muhammad Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Signed-off-by: Muhammad Zulfaqar Azmi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../behavior_path_lane_change_module/scene.hpp | 2 ++ .../src/scene.cpp | 13 ++++++++++++- .../behavior_path_planner_node.hpp | 6 ++++++ .../src/behavior_path_planner_node.cpp | 18 ++++++++++++++++++ .../data_manager.hpp | 3 +++ .../utils/path_safety_checker/safety_check.hpp | 2 +- .../utils/path_safety_checker/safety_check.cpp | 10 ++++++---- 7 files changed, 48 insertions(+), 6 deletions(-) diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index a8746b0e7de12..bbf6cbbbaa728 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp @@ -170,6 +170,8 @@ class NormalLaneChange : public LaneChangeBase bool isVehicleStuck( const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const; + double get_max_velocity_for_safety_check() const; + bool isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const; bool check_prepare_phase() const; diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index d80ffb9474d13..ee32add05dd51 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -1976,7 +1976,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( 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, - current_debug_data.second); + get_max_velocity_for_safety_check(), current_debug_data.second); if (collided_polygons.empty()) { utils::path_safety_checker::updateCollisionCheckDebugMap( @@ -2071,6 +2071,17 @@ bool NormalLaneChange::isVehicleStuck( return false; } +double NormalLaneChange::get_max_velocity_for_safety_check() const +{ + const auto external_velocity_limit_ptr = planner_data_->external_limit_max_velocity; + if (external_velocity_limit_ptr) { + return std::min( + static_cast(external_velocity_limit_ptr->max_velocity), getCommonParam().max_vel); + } + + return getCommonParam().max_vel; +} + bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const { if (current_lanes.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 5d40f2a8614ed..8ab76c6028a5c 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 @@ -38,6 +38,7 @@ #include #include #include +#include #include #include @@ -101,6 +102,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_; @@ -140,6 +143,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 342638c4a1a5b..7531c1bd8d0dc 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -112,6 +112,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( @@ -815,6 +820,19 @@ 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) +{ + // Note: Using this parameter during path planning might cause unexpected deceleration or jerk. + // Therefore, do not use it for anything other than safety checks. + 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 28943b5724fe8..7dd973e05520a 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 @@ -37,6 +37,7 @@ #include #include #include +#include #include #include @@ -64,6 +65,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 @@ -160,6 +162,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 e209e8dba36be..275018288512d 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 { @@ -478,7 +480,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(); } @@ -488,7 +490,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; @@ -504,7 +506,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 @@ -517,7 +519,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)) {