From 5b79adff04f0d6d2f15098632914cce5a5108a1b Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Tue, 27 Jun 2023 19:14:32 +0900 Subject: [PATCH] feat(lane_change): add param for a lateral distance margin where the abort can be performed (#4083) * feat(lane_change): add param for abort execution margin Signed-off-by: Takamasa Horibe * fix precommit Signed-off-by: Takamasa Horibe * fix for merge Signed-off-by: Takamasa Horibe * update readme Signed-off-by: Takamasa Horibe * fix doc Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe --- .../config/lane_change/lane_change.param.yaml | 1 + .../behavior_path_planner_lane_change_design.md | 15 ++++++++------- .../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 ++++++++++++---- 7 files changed, 26 insertions(+), 13 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 8357fa77910c0..75d9cf5109376 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 @@ -57,6 +57,7 @@ delta_time: 1.0 # [s] duration: 5.0 # [s] max_lateral_jerk: 1000.0 # [m/s3] + overhang_tolerance: 0.0 # [m] finish_judge_lateral_threshold: 0.2 # [m] 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 fe3be99c4cdc1..6a30a12a60493 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 @@ -444,13 +444,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 | +| `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 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 2a77419092189..67a23f23d5827 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 @@ -34,6 +34,7 @@ struct LaneChangeCancelParameters double delta_time{1.0}; double duration{5.0}; double max_lateral_jerk{10.0}; + double overhang_tolerance{0.0}; }; struct LaneChangeParameters { 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 32e5316f2f0a4..da22ddddece9d 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -811,6 +811,7 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() p.cancel.delta_time = declare_parameter(parameter("cancel.delta_time")); p.cancel.duration = declare_parameter(parameter("cancel.duration")); p.cancel.max_lateral_jerk = declare_parameter(parameter("cancel.max_lateral_jerk")); + p.cancel.overhang_tolerance = declare_parameter(parameter("cancel.overhang_tolerance")); 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 8bb8923f88c0c..be863a4f7e2d6 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 @@ -333,7 +333,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_->cancel.overhang_tolerance); } } diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 144f9a1711074..683767909acae 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1101,7 +1101,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); @@ -1111,9 +1111,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)