From 4012af0f6ffb3d04a522ed1aa7a7e8ed68418a3e Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Sat, 1 Jun 2024 08:06:08 +0900 Subject: [PATCH] feat(start_planner,lane_departure_checker): add margin param for lane departure check (#7193) * add param for lane departure margin Signed-off-by: Daniel Sanchez * json thing Signed-off-by: Daniel Sanchez * docs Signed-off-by: Daniel Sanchez * make separate param for lane departure margin expansion Signed-off-by: Daniel Sanchez * update docs Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- control/lane_departure_checker/README.md | 1 + .../config/lane_departure_checker.param.yaml | 1 + .../lane_departure_checker.hpp | 1 + .../schema/lane_departure_checker.json | 6 ++++++ .../lane_departure_checker.cpp | 3 ++- .../lane_departure_checker_node.cpp | 2 ++ .../README.md | 17 +++++++++-------- .../config/start_planner.param.yaml | 1 + .../data_structs.hpp | 1 + .../src/manager.cpp | 5 +++++ .../src/start_planner_module.cpp | 5 +++++ 11 files changed, 34 insertions(+), 9 deletions(-) diff --git a/control/lane_departure_checker/README.md b/control/lane_departure_checker/README.md index 67b770aefb3d2..4a6592a103f2f 100644 --- a/control/lane_departure_checker/README.md +++ b/control/lane_departure_checker/README.md @@ -93,6 +93,7 @@ This package includes the following features: | Name | Type | Description | Default value | | :------------------------- | :----- | :--------------------------------------------------------------------------------- | :------------ | | footprint_margin_scale | double | Coefficient for expanding footprint margin. Multiplied by 1 standard deviation. | 1.0 | +| footprint_extra_margin | double | Coefficient for expanding footprint margin. When checking for lane departure | 0.0 | | resample_interval | double | Minimum Euclidean distance between points when resample trajectory.[m] | 0.3 | | max_deceleration | double | Maximum deceleration when calculating braking distance. | 2.8 | | delay_time | double | Delay time which took to actuate brake when calculating braking distance. [second] | 1.3 | diff --git a/control/lane_departure_checker/config/lane_departure_checker.param.yaml b/control/lane_departure_checker/config/lane_departure_checker.param.yaml index f0a8df21c425b..2724c28e2536a 100644 --- a/control/lane_departure_checker/config/lane_departure_checker.param.yaml +++ b/control/lane_departure_checker/config/lane_departure_checker.param.yaml @@ -17,6 +17,7 @@ # Core footprint_margin_scale: 1.0 + footprint_extra_margin: 0.0 resample_interval: 0.3 max_deceleration: 2.8 delay_time: 1.3 diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index c658cf4497973..b95c0a4b26e5c 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -60,6 +60,7 @@ typedef boost::geometry::index::rtree LaneDepartureChecker::createVehicleFootprints( const PathWithLaneId & path) const { // Create vehicle footprint in base_link coordinate - const auto local_vehicle_footprint = vehicle_info_ptr_->createFootprint(); + const auto local_vehicle_footprint = + vehicle_info_ptr_->createFootprint(param_.footprint_extra_margin); // Create vehicle footprint on each Path point std::vector vehicle_footprints; diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index 06d11133920f4..4d6c86990c399 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -150,6 +150,7 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o // Core Parameter param_.footprint_margin_scale = declare_parameter("footprint_margin_scale"); + param_.footprint_extra_margin = declare_parameter("footprint_extra_margin"); param_.resample_interval = declare_parameter("resample_interval"); param_.max_deceleration = declare_parameter("max_deceleration"); param_.delay_time = declare_parameter("delay_time"); @@ -403,6 +404,7 @@ rcl_interfaces::msg::SetParametersResult LaneDepartureCheckerNode::onParameter( // Core update_param(parameters, "footprint_margin_scale", param_.footprint_margin_scale); + update_param(parameters, "footprint_extra_margin", param_.footprint_extra_margin); update_param(parameters, "resample_interval", param_.resample_interval); update_param(parameters, "max_deceleration", param_.max_deceleration); update_param(parameters, "delay_time", param_.delay_time); diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index dd3dddef9d863..ab2f0460b3fcb 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -494,14 +494,15 @@ See also [[1]](https://www.sciencedirect.com/science/article/pii/S14746670153474 #### parameters for geometric pull out -| Name | Unit | Type | Description | Default value | -| :-------------------------- | :---- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| enable_geometric_pull_out | [-] | bool | flag whether to enable geometric pull out | true | -| divide_pull_out_path | [-] | bool | flag whether to divide arc paths. The path is assumed to be divided because the curvature is not continuous. But it requires a stop during the departure. | false | -| geometric_pull_out_velocity | [m/s] | double | velocity of geometric pull out | 1.0 | -| arc_path_interval | [m] | double | path points interval of arc paths of geometric pull out | 1.0 | -| lane_departure_margin | [m] | double | margin of deviation to lane right | 0.2 | -| pull_out_max_steer_angle | [rad] | double | maximum steer angle for path generation | 0.26 | +| Name | Unit | Type | Description | Default value | +| :------------------------------------ | :---- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_geometric_pull_out | [-] | bool | flag whether to enable geometric pull out | true | +| divide_pull_out_path | [-] | bool | flag whether to divide arc paths. The path is assumed to be divided because the curvature is not continuous. But it requires a stop during the departure. | false | +| geometric_pull_out_velocity | [m/s] | double | velocity of geometric pull out | 1.0 | +| arc_path_interval | [m] | double | path points interval of arc paths of geometric pull out | 1.0 | +| lane_departure_margin | [m] | double | margin of deviation to lane right | 0.2 | +| lane_departure_check_expansion_margin | [m] | double | margin to expand the ego vehicle footprint when doing lane departure checks | 0.0 | +| pull_out_max_steer_angle | [rad] | double | maximum steer angle for path generation | 0.26 | ## **backward pull out start point search** diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml index 46469de68149e..7de76c28cef5c 100644 --- a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml @@ -40,6 +40,7 @@ geometric_pull_out_velocity: 1.0 arc_path_interval: 1.0 lane_departure_margin: 0.2 + lane_departure_check_expansion_margin: 0.0 backward_velocity: -1.0 pull_out_max_steer_angle: 0.26 # 15deg # search start pose backward diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp index 0e85c1d0e76b3..04248ee7bd5fb 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp @@ -71,6 +71,7 @@ struct StartPlannerParameters behavior_path_planner::utils::path_safety_checker::ObjectTypesToCheck object_types_to_check_for_path_generation{}; double center_line_path_interval{0.0}; + double lane_departure_check_expansion_margin{0.0}; // shift pull out bool enable_shift_pull_out{false}; diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index 794577d7cc7aa..d5731f54224b6 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -101,6 +101,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) node->declare_parameter(ns + "arc_path_interval"); p.parallel_parking_parameters.pull_out_lane_departure_margin = node->declare_parameter(ns + "lane_departure_margin"); + p.lane_departure_check_expansion_margin = + node->declare_parameter(ns + "lane_departure_check_expansion_margin"); p.parallel_parking_parameters.pull_out_max_steer_angle = node->declare_parameter(ns + "pull_out_max_steer_angle"); // 15deg p.parallel_parking_parameters.center_line_path_interval = @@ -435,6 +437,9 @@ void StartPlannerModuleManager::updateModuleParams( updateParam( parameters, ns + "lane_departure_margin", p->parallel_parking_parameters.pull_out_lane_departure_margin); + updateParam( + parameters, ns + "lane_departure_check_expansion_margin", + p->lane_departure_check_expansion_margin); updateParam( parameters, ns + "pull_out_max_steer_angle", p->parallel_parking_parameters.pull_out_max_steer_angle); diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index d3afdb2aefae5..7e1f38f30c378 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -65,6 +65,11 @@ StartPlannerModule::StartPlannerModule( { lane_departure_checker_ = std::make_shared(); lane_departure_checker_->setVehicleInfo(vehicle_info_); + lane_departure_checker::Param lane_departure_checker_params; + lane_departure_checker_params.footprint_extra_margin = + parameters->lane_departure_check_expansion_margin; + + lane_departure_checker_->setParam(lane_departure_checker_params); // set enabled planner if (parameters_->enable_shift_pull_out) {