From 6afd6930881578feb445c5adac2f84c94d43ed2a Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 26 Jun 2023 19:39:47 +0900 Subject: [PATCH 1/5] feat(lane_change): add param for abort execution margin Signed-off-by: Takamasa Horibe --- .../config/lane_change/lane_change.param.yaml | 1 + .../lane_change/lane_change_module_data.hpp | 1 + .../behavior_path_planner/utils/utils.hpp | 2 +- .../src/behavior_path_planner_node.cpp | 1 + .../src/scene_module/lane_change/normal.cpp | 3 ++- .../behavior_path_planner/src/utils/utils.cpp | 16 ++++++++++++---- 6 files changed, 18 insertions(+), 6 deletions(-) diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index de3802b070326..94c60eef82172 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -53,6 +53,7 @@ abort_delta_time: 1.0 # [s] aborting_time: 5.0 # [s] abort_max_lateral_jerk: 1000.0 # [m/s3] + abort_exec_lateral_threshold: 0.0 finish_judge_lateral_threshold: 0.2 # [m] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 85a77df4c1e0d..6bfd9a236ad43 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -65,6 +65,7 @@ struct LaneChangeParameters double abort_delta_time{1.0}; double aborting_time{5.0}; double abort_max_lateral_jerk{10.0}; + double abort_exec_lateral_threshold{0.0}; double finish_judge_lateral_threshold{0.2}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index b5da46148a9f3..cc2d109f0cf1c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -306,7 +306,7 @@ bool isEgoOutOfRoute( bool isEgoWithinOriginalLane( const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, - const BehaviorPathPlannerParameters & common_param); + const BehaviorPathPlannerParameters & common_param, const double outer_margin = 0.0); // path management 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 229ae0eb679ea..cdf45aadd791d 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -826,6 +826,7 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() p.abort_delta_time = declare_parameter(parameter("abort_delta_time")); p.aborting_time = declare_parameter(parameter("aborting_time")); p.abort_max_lateral_jerk = declare_parameter(parameter("abort_max_lateral_jerk")); + p.abort_exec_lateral_threshold = declare_parameter(parameter("abort_exec_lateral_threshold")); p.finish_judge_lateral_threshold = declare_parameter("lane_change.finish_judge_lateral_threshold"); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 548d23642581e..ea050f2fda4fc 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -373,7 +373,8 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const if (dist > estimated_travel_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; return utils::isEgoWithinOriginalLane( - status_.current_lanes, estimated_pose, planner_data_->parameters); + status_.current_lanes, estimated_pose, planner_data_->parameters, + lane_change_parameters_->abort_exec_lateral_threshold); } } diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index e1c2560b1c0d1..5b89b352d9574 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1114,7 +1114,7 @@ bool isEgoOutOfRoute( bool isEgoWithinOriginalLane( const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, - const BehaviorPathPlannerParameters & common_param) + const BehaviorPathPlannerParameters & common_param, const double outer_margin) { const auto lane_length = lanelet::utils::getLaneletLength2d(current_lanes); const auto lane_poly = lanelet::utils::getPolygonFromArcLength(current_lanes, 0, lane_length); @@ -1124,9 +1124,17 @@ bool isEgoWithinOriginalLane( const auto vehicle_poly = tier4_autoware_utils::toFootprint(current_pose, base_link2front, base_link2rear, vehicle_width); - // Check if the ego vehicle is entirely within the lane by checking if the vehicle's polygon - // is within the lane's polygon - return boost::geometry::within(vehicle_poly, lanelet::utils::to2D(lane_poly).basicPolygon()); + // Check if the ego vehicle is entirely within the lane with a given outer margin. + for (const auto & p : vehicle_poly.outer()) { + // When the point is in the polygon, the distance is 0. When it is out of the polygon, return a + // positive value. + const auto dist = boost::geometry::distance(p, lanelet::utils::to2D(lane_poly).basicPolygon()); + if (dist > std::max(outer_margin, 0.0)) { + return false; // out of polygon + } + } + + return true; // inside polygon } lanelet::ConstLanelets transformToLanelets(const DrivableLanes & drivable_lanes) From 5bff51f26729ca756781018cfbe18dde962a96b2 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 26 Jun 2023 19:46:11 +0900 Subject: [PATCH 2/5] fix precommit Signed-off-by: Takamasa Horibe --- .../behavior_path_planner/src/behavior_path_planner_node.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 cdf45aadd791d..f203b5735891f 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -826,7 +826,8 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() p.abort_delta_time = declare_parameter(parameter("abort_delta_time")); p.aborting_time = declare_parameter(parameter("aborting_time")); p.abort_max_lateral_jerk = declare_parameter(parameter("abort_max_lateral_jerk")); - p.abort_exec_lateral_threshold = declare_parameter(parameter("abort_exec_lateral_threshold")); + p.abort_exec_lateral_threshold = + declare_parameter(parameter("abort_exec_lateral_threshold")); p.finish_judge_lateral_threshold = declare_parameter("lane_change.finish_judge_lateral_threshold"); From 5e655cc79f986f2dac61d8cc2a5d5037672302c6 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Tue, 27 Jun 2023 16:36:56 +0900 Subject: [PATCH 3/5] fix for merge Signed-off-by: Takamasa Horibe --- .../src/scene_module/lane_change/normal.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 04e722d3ffcef..59ea5533b4356 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -396,7 +396,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; return utils::isEgoWithinOriginalLane( status_.current_lanes, estimated_pose, planner_data_->parameters, - lane_change_parameters_->abort_exec_lateral_threshold); + lane_change_parameters_->cancel.overhang_tolerance); } } From 6964a2250d18e0a71bd24f5538bd7bbc2fcd2ee4 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Tue, 27 Jun 2023 17:33:02 +0900 Subject: [PATCH 4/5] update readme Signed-off-by: Takamasa Horibe --- .../behavior_path_planner_lane_change_design.md | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md index 4b9d04794e43f..b42f9e8bede6c 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md @@ -301,13 +301,14 @@ The following parameters are configurable in `behavior_path_planner.param.yaml`. The following parameters are configurable in `lane_change.param.yaml`. -| Name | Unit | Type | Description | Default value | -| :------------------------------------- | ------- | ------- | -------------------------------------------------------------- | ------------- | -| `cancel.enable_on_prepare_phase` | [-] | boolean | Enable cancel lane change | true | -| `cancel.enable_on_lane_changing_phase` | [-] | boolean | Enable abort lane change. | false | -| `cancel.delta_time` | [s] | double | The time taken to start steering to return to the center line. | 3.0 | -| `cancel.duration` | [s] | double | The time taken to complete returning to the center line. | 3.0 | -| `cancel.max_lateral_jerk` | [m/sss] | double | The maximum lateral jerk for abort path | 1000.0 | +| Name | Unit | Type | Description | Default value | +| :------------------------------------- | ------- | ------- | ---------------------------------------------------------------------------------------------------------------- | ------------- | +| `cancel.enable_on_prepare_phase` | [-] | boolean | Enable cancel lane change | true | +| `cancel.enable_on_lane_changing_phase` | [-] | boolean | Enable abort lane change. | false | +| `cancel.delta_time` | [s] | double | The time taken to start steering to return to the center line. | 3.0 | +| `cancel.duration` | [s] | double | The time taken to complete returning to the center line. | 3.0 | +| `cancel.max_lateral_jerk` | [m/sss] | double | The maximum lateral jerk for abort path | 1000.0 | +| `overhang_tolerance` | [m] | double | Lane change cancel is prohibited if the vehicle head exceeds the lane boundary more than this tolerance distance | 0.0 | ### Debug From ff70591cf8afaecfd6ac20a2b4bf4ab3f5d28fcb Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Tue, 27 Jun 2023 17:45:57 +0900 Subject: [PATCH 5/5] fix doc Signed-off-by: Takamasa Horibe --- .../docs/behavior_path_planner_lane_change_design.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md index b42f9e8bede6c..f8c771a27cf56 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md @@ -308,7 +308,7 @@ The following parameters are configurable in `lane_change.param.yaml`. | `cancel.delta_time` | [s] | double | The time taken to start steering to return to the center line. | 3.0 | | `cancel.duration` | [s] | double | The time taken to complete returning to the center line. | 3.0 | | `cancel.max_lateral_jerk` | [m/sss] | double | The maximum lateral jerk for abort path | 1000.0 | -| `overhang_tolerance` | [m] | double | Lane change cancel is prohibited if the vehicle head exceeds the lane boundary more than this tolerance distance | 0.0 | +| `cancel.overhang_tolerance` | [m] | double | Lane change cancel is prohibited if the vehicle head exceeds the lane boundary more than this tolerance distance | 0.0 | ### Debug