From 8651a21862749b727a3c6948fc487eaa8d3c91f6 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Thu, 27 Jun 2024 10:35:19 +0900 Subject: [PATCH 1/2] refactor(static_obstacle_avoidance): organize params for drivable lane Signed-off-by: satoshi-ota --- .../README.md | 38 +++++++++++-------- .../static_obstacle_avoidance.param.yaml | 8 +++- .../data_structs.hpp | 8 +--- .../parameter_helper.hpp | 3 +- .../static_obstacle_avoidance.schema.json | 15 +++----- .../src/utils.cpp | 12 +++--- 6 files changed, 44 insertions(+), 40 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md index b27723ab2066f..ec139b9a8b099 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md @@ -760,7 +760,7 @@ Basically, this module tries to generate avoidance path in order to keep lateral But if there isn't enough space to keep `soft_margin` distance, this module shortens soft constraint lateral margin. The parameter `soft_margin` is a maximum value of soft constraint, and actual soft margin can be a value between 0.0 and `soft_margin`. On the other hand, this module definitely keeps `hard_margin` or `hard_margin_for_parked_vehicle` depending on the situation. Thus, the minimum value of total lateral margin is `hard_margin`/`hard_margin_for_parked_vehicle`, and the maximum value is the sum of `hard_margin`/`hard_margin_for_parked_vehicle` and `soft_margin`. -Following figure shows the situation where this module shortens lateral soft constraint in order not to drive opposite direction lane when user set a parameter `use_opposite_lane` to `false`. +Following figure shows the situation where this module shortens lateral soft constraint in order not to drive opposite direction lane when user set a parameter `use_lane_type` to `same_direction_lane`. ![fig](./images/path_generation/adjust_margin.png) @@ -784,7 +784,7 @@ On the other hand, if the lateral distance is larger than `hard_margin`/`hard_ma ### When there is not enough space -This module inserts stop point only when the ego can potentially avoid the object. So, if it is not able to keep distance more than `hard_margin`/`hard_margin_for_parked_vehicle`, this module does nothing. Following figure shows the situation where this module is not able to keep enough lateral distance when user set a parameter `use_opposite_lane` to `false`. +This module inserts stop point only when the ego can potentially avoid the object. So, if it is not able to keep distance more than `hard_margin`/`hard_margin_for_parked_vehicle`, this module does nothing. Following figure shows the situation where this module is not able to keep enough lateral distance when user set a parameter `use_lane_type` to `same_direction_lane`. ![fig](./images/path_generation/do_nothing.png) @@ -804,15 +804,19 @@ Usable lane for avoidance module can be selected by config file. ```yaml ... - use_adjacent_lane: true - use_opposite_lane: true + # drivable lane setting. this module is able to use not only current lane but also right/left lane + # if the current lane(=lanelt::Lanelet) and the rignt/left lane share the boundary(=lanelet::Linestring) in HDMap. + # "current_lane" : use only current lane. this module doesn't use adjacent lane to avoid object. + # "same_direction_lane" : this module uses same direction lane to avoid object if need. + # "opposite_direction_lane": this module uses both same direction and opposite direction lane. + use_lane_type: "opposite_direction_lane" ``` -When user set parameter both `use_adjacent_lane` and `use_opposite_lane` to `true`, it is possible to use opposite lane. +When user set parameter `use_lane_type` to `opposite_direction_lane`, it is possible to use opposite lane. ![fig](./images/path_generation/opposite_direction.png) -When user only set parameter `use_adjacent_lane` to `true`, the module doesn't create path that overlaps opposite lane. +When user set parameter `use_lane_type` to `same_direction_lane`, the module doesn't create path that overlaps opposite lane. ![fig](./images/path_generation/same_direction.png) @@ -972,21 +976,25 @@ This module supports drivable area expansion for following polygons defined in H Please set the flags to `true` when user wants to make it possible to use those areas in avoidance maneuver. ```yaml +# drivable lane setting. this module is able to use not only current lane but also right/left lane +# if the current lane(=lanelt::Lanelet) and the rignt/left lane share the boundary(=lanelet::Linestring) in HDMap. +# "current_lane" : use only current lane. this module doesn't use adjacent lane to avoid object. +# "same_direction_lane" : this module uses same direction lane to avoid object if need. +# "opposite_direction_lane": this module uses both same direction and opposite direction lane. +use_lane_type: "opposite_direction_lane" # drivable area setting -use_adjacent_lane: true -use_opposite_lane: true use_intersection_areas: true use_hatched_road_markings: true use_freespace_areas: true ``` -| | | | -| --------------------- | ---------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| adjacent lane | ![fig](./images/advanced/avoidance_same_direction.png) | | -| opposite lane | ![fig](./images/advanced/avoidance_opposite_direction.png) | | -| intersection area | ![fig](./images/advanced/avoidance_intersection.png) | The intersection area is defined on Lanelet map. See [here](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md) | -| hatched road markings | ![fig](./images/advanced/avoidance_zebra.png) | The hatched road marking is defined on Lanelet map. See [here](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md#hatched-road-markings-area) | -| freespace area | ![fig](./images/advanced/avoidance_freespace.png) | The freespace area is defined on Lanelet map. (unstable) | +| | | | +| -------------------------------------- | ---------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| use_lane_type: same_direction_lane | ![fig](./images/advanced/avoidance_same_direction.png) | | +| use_lane_type: opposite_direction_lane | ![fig](./images/advanced/avoidance_opposite_direction.png) | | +| intersection area | ![fig](./images/advanced/avoidance_intersection.png) | The intersection area is defined on Lanelet map. See [here](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md) | +| hatched road markings | ![fig](./images/advanced/avoidance_zebra.png) | The hatched road marking is defined on Lanelet map. See [here](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md#hatched-road-markings-area) | +| freespace area | ![fig](./images/advanced/avoidance_freespace.png) | The freespace area is defined on Lanelet map. (unstable) | ## Future extensions / Unimplemented parts diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml index 80d2128bf70e0..3087ccc93934b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml @@ -5,9 +5,13 @@ resample_interval_for_planning: 0.3 # [m] FOR DEVELOPER resample_interval_for_output: 4.0 # [m] FOR DEVELOPER + # drivable lane setting. this module is able to use not only current lane but also right/left lane + # if the current lane(=lanelet::Lanelet) and the right/left lane share the boundary(=lanelet::Linestring) in HDMap. + # "current_lane" : use only current lane. this module doesn't use adjacent lane to avoid object. + # "same_direction_lane" : this module uses same direction lane to avoid object if need. + # "opposite_direction_lane": this module uses both same direction and opposite direction lane. + use_lane_type: "opposite_direction_lane" # drivable area setting - use_adjacent_lane: true - use_opposite_lane: true use_intersection_areas: true use_hatched_road_markings: true use_freespace_areas: true diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp index dbedbfe1f70cc..d02b39047e71c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp @@ -101,12 +101,8 @@ struct AvoidanceParameters // computational cost for latter modules. double resample_interval_for_output = 3.0; - // enable avoidance to be perform only in lane with same direction - bool use_adjacent_lane{true}; - - // enable avoidance to be perform in opposite lane direction - // to use this, enable_avoidance_over_same_direction need to be set to true. - bool use_opposite_lane{true}; + // drivable lane config + std::string use_lane_type{"current_lane"}; // if this param is true, it reverts avoidance path when the path is no longer needed. bool enable_cancel_maneuver{false}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp index a79a67d5fdd4a..84cf7c4e33d26 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp @@ -46,8 +46,7 @@ AvoidanceParameters getParameter(rclcpp::Node * node) // drivable area { const std::string ns = "avoidance."; - p.use_adjacent_lane = getOrDeclareParameter(*node, ns + "use_adjacent_lane"); - p.use_opposite_lane = getOrDeclareParameter(*node, ns + "use_opposite_lane"); + p.use_lane_type = getOrDeclareParameter(*node, ns + "use_lane_type"); p.use_intersection_areas = getOrDeclareParameter(*node, ns + "use_intersection_areas"); p.use_hatched_road_markings = getOrDeclareParameter(*node, ns + "use_hatched_road_markings"); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json index 1e0753a288a28..6effe1f7002af 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json @@ -22,14 +22,10 @@ "description": "Path generation method.", "default": "shift_line_base" }, - "use_adjacent_lane": { - "type": "boolean", - "description": "Extend avoidance trajectory to adjacent lanes that has same direction. If false, avoidance only happen in current lane.", - "default": "true" - }, - "use_opposite_lane": { - "type": "boolean", - "description": "Extend avoidance trajectory to opposite direction lane. `use_adjacent_lane` must be `true` to take effects.", + "use_lane_type": { + "type": "string", + "enum": ["current_lane", "same_direction_lane", "opposite_direction_lane"], + "description": "Drivable lane configuration.", "default": "true" }, "use_hatched_road_markings": { @@ -1447,8 +1443,7 @@ "resample_interval_for_planning", "resample_interval_for_output", "path_generation_method", - "use_adjacent_lane", - "use_opposite_lane", + "use_lane_type", "use_hatched_road_markings", "use_intersection_areas", "use_freespace_areas", diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 70de9e9499917..8d0f9904b5bad 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -2238,14 +2238,16 @@ DrivableLanes generateExpandedDrivableLanes( current_drivable_lanes.left_lane = lanelet; current_drivable_lanes.right_lane = lanelet; - if (!parameters->use_adjacent_lane) { + if (parameters->use_lane_type == "current_lane") { return current_drivable_lanes; } + const auto use_opposite_lane = parameters->use_lane_type == "opposite_direction_lane"; + // 1. get left/right side lanes const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) { - const auto all_left_lanelets = route_handler->getAllLeftSharedLinestringLanelets( - target_lane, parameters->use_opposite_lane, true); + const auto all_left_lanelets = + route_handler->getAllLeftSharedLinestringLanelets(target_lane, use_opposite_lane, true); if (!all_left_lanelets.empty()) { current_drivable_lanes.left_lane = all_left_lanelets.back(); // leftmost lanelet pushUniqueVector( @@ -2254,8 +2256,8 @@ DrivableLanes generateExpandedDrivableLanes( } }; const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) { - const auto all_right_lanelets = route_handler->getAllRightSharedLinestringLanelets( - target_lane, parameters->use_opposite_lane, true); + const auto all_right_lanelets = + route_handler->getAllRightSharedLinestringLanelets(target_lane, use_opposite_lane, true); if (!all_right_lanelets.empty()) { current_drivable_lanes.right_lane = all_right_lanelets.back(); // rightmost lanelet pushUniqueVector( From 65ace5a8ca733bf6a020abf85fcf90c098520b7f Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 27 Jun 2024 10:52:10 +0900 Subject: [PATCH 2/2] Update planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json --- .../schema/static_obstacle_avoidance.schema.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json index 6effe1f7002af..db3215fa8d238 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json @@ -26,7 +26,7 @@ "type": "string", "enum": ["current_lane", "same_direction_lane", "opposite_direction_lane"], "description": "Drivable lane configuration.", - "default": "true" + "default": "opposite_direction_lane" }, "use_hatched_road_markings": { "type": "boolean",