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 <horibe.takamasa@gmail.com> * fix precommit Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * fix for merge Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * update readme Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * fix doc Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> --------- Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
autowarefoundation · Jun 27, 2023 · 3fea24f · 3fea24f
1 parent 6f5af96
commit 3fea24f
Show file tree

Hide file tree

Showing 7 changed files with 26 additions and 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
@@ -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
@@ -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        |
+| `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/..._path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/..._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
@@ -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
@@ -833,6 +833,7 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam()
   p.cancel.delta_time = declare_parameter<double>(parameter("cancel.delta_time"));
   p.cancel.duration = declare_parameter<double>(parameter("cancel.duration"));
   p.cancel.max_lateral_jerk = declare_parameter<double>(parameter("cancel.max_lateral_jerk"));
+  p.cancel.overhang_tolerance = declare_parameter<double>(parameter("cancel.overhang_tolerance"));
 
   p.finish_judge_lateral_threshold =
     declare_parameter<double>("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
@@ -396,7 +396,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
@@ -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)