feat(behavior_path_planner): better avoidance drivable areas extension in behavior path planning

planning/behavior_path_planner/behavior_path_planner_avoidance-design.md

@@ -238,6 +238,148 @@ The shift points are modified by a filtering process in order to get the expecte
 - Similar gradient removal: Connect two shift points with a straight line, and remove the shift points in between if their shift amount is in the vicinity of the straight line.
 - Remove momentary returns: For shift points that reduce the avoidance width (for going back to the center line), if there is enough long distance in the longitudinal direction, remove them.
 
+#### Computing shift length (available as of develop/v0.25.0)
+
+**Note**: This feature is available as of develop/`v0.25.0`.
+
+The shift length is set as a constant value before the feature is implemented (develop/`v0.24.0` and below). Setting the shift length like this will cause the module to generate an avoidance path regardless of actual environmental properties. For example, the path might exceed the actual road boundary or go towards a wall. Therefore, to address this limitation, in addition to [how to decide the target obstacle](#how-to-decide-the-target-obstacles), the upgraded module also takes into account the following additional element
+
+- The obstacles' current lane and position.
+- The road shoulder with reference to the direction to avoid.
+  - Note: Lane direction is disregarded.
+
+These elements are used to compute the distance from the object to the road's shoulder (`(class ObjectData.to_road_shoulder_distance)`).
+
+![fig1](./image/obstacle_to_road_shoulder_distance.png)
+
+##### Computing shift length
+
+To compute the shift length, in addition to the vehicle's width and the parameterized `lateral_collision_margin`, the upgraded feature also adds two new parameters; `lateral_collision_safety_buffer` and `road_shoulder_safety_margin`.
+
+- The `lateral_collision_safety_buffer` parameter is used to set a safety gap that will act as the final line of defense when computing avoidance path.
+  - The rationale behind having this parameter is that the parameter `lateral_collision_margin` might be changed according to the situation for various reasons. Therefore, `lateral_collision_safety_buffer` will act as the final line of defense in case of the usage of `lateral_collision_margin` fails.
+  - It is recommended to set the value to more than half of the ego vehicle's width.
+- The `road_shoulder_safety_margin` will prevent the module from generating a path that might cause the vehicle to go too near the road shoulder.
+
+![fig1](./image/shift_length_parameters.png)
+
+The shift length is subjected to the following constraints.
+
+<!-- spell-checker:disable -->
+
+![shift_length_equation](./image/shift_length_equation.png)
+
+<!--<img src="https://latex.codecogs.com/gif.latex?\textrm{shift&space;length}=\left\{\begin{matrix}&space;d_{lcsb}&plus;d_{lcm}&plus;\frac{1}{2}(W_{ego})&\textrm{if}&\left(d_{lcsb}&plus;d_{lcm}&plus;W_{ego}&plus;d_{rssm}\right)<d_{trsd}\\&space;0&space;&&space;\textrm{if}&\left(d_{lcsb}&plus;d_{lcm}&plus;W_{ego}&plus;d_{rssm}\right)\geq&space;d_{trsd}&space;\end{matrix}\right." title="shift_length_computation" /> -->
+
+where
+
+- <img src="https://latex.codecogs.com/svg.latex?\inline&space;\large&space;d_{lcsb}" title="\large d_{lcsb}" /> = `lateral_collision_safety_buffer`
+- <img src="https://latex.codecogs.com/svg.latex?\inline&space;\large&space;d_{lcm}" title="\large d_{lcm}" /> = `lateral_collision_margin`
+- <img src="https://latex.codecogs.com/svg.latex?\inline&space;\large&space;W_{ego}" title="\large d_{W_{ego}}" /> = ego vehicle's width
+- <img src="https://latex.codecogs.com/svg.latex?\inline&space;\large&space;d_{rssm}" title="\large d_{rssm}" /> = `road_shoulder_safety_margin`
+- <img src="https://latex.codecogs.com/svg.latex?\inline&space;\large&space;d_{trsd}" title="\large d_{trsm}" /> = `(class ObjectData).to_road_shoulder_distance`
+
+```plantuml
+@startuml
+skinparam monochrome true
+skinparam defaultTextAlignment center
+skinparam noteTextAlignment left
+
+title avoidance path planning
+start
+partition calcAvoidanceTargetObjects() {
+:calcOverhangDistance;
+note right
+ - get overhang_pose
+end note
+
+:getClosestLaneletWithinRoute;
+note right
+ - obtain overhang_lanelet
+end note
+
+if(Is overhang_lanelet.id() exist?) then (no)
+stop
+else (\n yes)
+
+if(isOnRight(obstacle)?) then (yes)
+partition getLeftMostLineString() {
+repeat
+repeat
+:getLeftLanelet;
+note left
+ Check both
+ - lane-changeable lane
+ - non lane-changeable lane
+end note
+repeat while (Same direction Lanelet exist?) is (yes) not (no)
+:getLeftOppositeLanelet;
+repeat while (Opposite direction Lanelet exist?) is (yes) not (no)
+}
+:compute\n(class ObjectData).to_road_shoulder_distance;
+note left
+distance from overhang_pose
+to left most linestring
+end note
+else(\n no)
+partition getrightMostLineString() {
+repeat
+repeat
+:getRightLanelet;
+note right
+ Check both
+ - lane-changeable lane
+ - non lane-changeable lane
+end note
+repeat while (Same direction Lanelet exist?) is (yes) not (no)
+:getRightOppositeLanelet;
+repeat while (Opposite direction Lanelet exist?) is (yes) not (no)
+}
+:compute\n(class ObjectData).to_road_shoulder_distance;
+note right
+distance from overhang_pose
+to right most linestring
+end note
+endif
+endif
+}
+
+partition calcRawShiftPointsFromObjects() {
+:compute max_allowable_lateral_distance;
+note right
+The sum of
+- lat_collision_safety_buffer
+- lat_collision_margin
+- vehicle_width
+end note
+:compute max_shift_length;
+note right
+subtract
+- road_shoulder_safety_margin
+- 0.5 x vehicle_width
+from (class ObjectData).to_road_shoulder_margin
+end note
+if(isOnRight(object)?) then (yes)
+if((class ObjectData).to_road_shoulder_distance \n\l>\l\n max_allowable_lateral_distance ?) then (yes)
+:max_left_shift_limit = max_shift_length;
+else (\n No)
+:max_left_shift_limit = 0.0;
+endif
+else (\n No)
+if((class ObjectData).to_road_shoulder_distance \n\l>\l\n max_allowable_lateral_distance ?) then (yes)
+:max_right_shift_limit = -max_shift_length;
+else (\n No)
+:max_right_shift_limit = 0.0;
+endif
+endif
+:compute shift length;
+}
+stop
+@enduml
+```
+
+<!-- spell-checker:enable -->
+
 #### How to keep the consistency of the planning
 
 TODO
@@ -250,7 +392,8 @@ TODO
 | :----------------------------------------- | :----- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ |
 | resample_interval_for_planning             | [m]    | double | Path resample interval for avoidance planning path.                                                         | 0.3           |
 | resample_interval_for_output               | [m]    | double | Path resample interval for output path. Too short interval increases computational cost for latter modules. | 3.0           |
-| lateral_collision_margin                   | [m]    | double | The lateral distance between ego and avoidance targets.                                                     | 2.0           |
+| lateral_collision_margin                   | [m]    | double | The lateral distance between ego and avoidance targets.                                                     | 1.5           |
+| lateral_collision_safety_buffer            | [m]    | double | Creates an additional gap that will prevent the vehicle from getting to near to the obstacle                | 0.5           |
 | longitudinal_collision_margin_min_distance | [m]    | double | when complete avoidance motion, there is a distance margin with the object for longitudinal direction.      | 0.0           |
 | longitudinal_collision_margin_time         | [s]    | double | when complete avoidance motion, there is a time margin with the object for longitudinal direction.          | 0.0           |
 | prepare_time                               | [s]    | double | Avoidance shift starts from point ahead of this time x ego_speed to avoid sudden path change.               | 1.0           |
@@ -262,6 +405,7 @@ TODO
 | min_sharp_avoidance_speed                  | [m/s]  | double | Minimum speed for jerk calculation in a sharp situation (\*1).                                              | 1.0           |
 | max_right_shift_length                     | [m]    | double | Maximum shift length for right direction                                                                    | 5.0           |
 | max_left_shift_length                      | [m]    | double | Maximum shift length for left direction                                                                     | 5.0           |
+| road_shoulder_safety_margin                | [m]    | double | Prevents the generated path to come too close to the road shoulders.                                        | 0.5           |
 
 ### Speed limit modification
 

planning/behavior_path_planner/behavior_path_planner_design_limitations.md

@@ -0,0 +1,52 @@
+# Design limitation
+
+The document describes the limitations that are currently present in the `behavior_path_planner` module.
+
+The following items (but not limited to) fall in the scope of limitation:
+
+- limitations due to the third-party API design and requirement
+- limitations due to any shortcoming out of the developer's control.
+
+## Limitation: Multiple connected opposite lanes require Linestring with shared ID
+
+To fully utilize the `Lanelet2`'s [API](https://github.com/fzi-forschungszentrum-informatik/Lanelet2/blob/master/lanelet2_core/doc/LaneletPrimitives.md#lanelet), the design of the vector map (`.osm`) needs to follow all the criteria described in `Lanelet2` documentation. Specifically, in the case of 2 or more lanes, the Linestrings that divide the current lane with the opposite/adjacent lane need to have a matching `Linestring ID`. Assume the following **ideal case**.
+
+![limitation01-01-ideal-case1](./image/limitations/limitation01-01.png)
+
+In the image, `Linestring ID51` is shared by `Lanelet A` and `Lanelet B`. Hence we can directly use the available `left`, `adjacentLeft`, `right`, `adjacentRight` and `findUsages` method within `Lanelet2`'s API to directly query the direction and opposite lane availability.
+
+```cpp
+const auto right_lane = routing_graph_ptr_->right(lanelet);
+const auto adjacent_right_lane = routing_graph_ptr_->adjacentRight(lanelet);
+const auto opposite_right_lane = lanelet_map_ptr_->laneletLayer.findUsages(lanelet.rightBound().invert());
+```
+
+The following images show the situation where **these API does not work directly**. This means that we cannot use them straight away, and several assumptions and logical instruction are needed to make these APIs work.
+
+![limitation01-02-non-work](./image/limitations/limitation01-02-not-work.png)
+
+In this example (multiple linestring issues), `Lanelet C` contains `Linestring ID61` and `ID62`, while `Lanelet D` contains `Linestring ID63` and `ID 64`. Although the `Linestring ID62` and `ID64` have identical point IDs and seem visually connected, the API will treat these Linestring as though they are separated. When it searches for any `Lanelet` that is connected via `Linestring ID62`, it will return `NULL`, since `ID62` only connects to `Lanelet C` and not other `Lanelet`.
+
+Although, in this case, it is possible to forcefully search the lanelet availability by checking the lanelet that contains the points, using`getLaneletFromPoint` method. But, the implementation requires complex rules for it to work. Take the following images as an example.
+
+![limitation01-03-not-equal-length](./image/limitations/limitation01-03-not-equal-length.png)
+
+Assume `Object X` is in `Lanelet F`. We can forcefully search `Lanelet E` via `Point 7`, and it will work if `Point 7` is utilized by **only 2 lanelet**. However, the complexity increases when we want to start searching for the **direction** of the opposite lane. We can infer the direction of the lanelet by using mathematical operations (dot product of vector `V_ID72` (`Point 6` minus `Point 9`), and `V_ID74` (`Point 7` minus `Point 8`). But, notice that we did not use Point 7 in V_ID72. This is because searching it requires an iteration, adding additional non-beneficial computation.
+
+Suppose the points are used by **more than 2 lanelets**. In that case, we have to find the differences for all lanelet, and the result might be undefined. The reason is that the differences between the coordinates do not reflect the actual **shape** of the lanelet. The following image demonstrates this point.
+
+![equal-length-but-non-identical-shape](./image/limitations/limitation01-04-equal-length.png)
+
+![points-shared-more-than-one](./image/limitations/limitation01-04-not-equal.png)
+
+There are many other available solutions to try. However, further attempt to solve this might cause issues in the future, especially for maintaining or scaling up the software.
+
+In conclusion, the multiple Linestring issues will not be supported. Covering these scenarios might give the user an "everything is possible" impression. This is dangerous since any attempt to create a non-standardized vector map is not compliant with safety regulations.
+
+## Limitation: Avoidance at Corners and Intersections
+
+Currently, the implementation doesn't cover avoidance at corners and intersections. The reason is similar to here. However, this case can still be supported in the future (assuming the vector map is defined correctly).
+
+![limitation-at-the-intersections](./image/limitations/limitation-intersection.png)
+
+![limitation-at-the-corner](./image/limitations/limitation-corner.png)

planning/behavior_path_planner/config/avoidance/avoidance.param.yaml

@@ -7,11 +7,15 @@
       detection_area_right_expand_dist: 0.0
       detection_area_left_expand_dist: 1.0
 
+      enable_avoidance_over_same_direction: true
+      enable_avoidance_over_opposite_direction: true
+
       threshold_distance_object_is_on_center: 1.0 # [m]
       threshold_speed_object_is_stopped: 1.0      # [m/s]
       object_check_forward_distance: 150.0        # [m]
       object_check_backward_distance: 2.0         # [m]
-      lateral_collision_margin: 2.0               # [m]
+      lateral_collision_margin: 1.5               # [m]
+      lateral_collision_safety_buffer: 0.5        # [m]
 
       prepare_time: 2.0                           # [s]
       min_prepare_distance: 1.0                   # [m]
@@ -20,6 +24,8 @@
       min_nominal_avoidance_speed: 7.0  # [m/s]
       min_sharp_avoidance_speed: 1.0    # [m/s]
 
+      road_shoulder_safety_margin: 0.5 # [m]
+
       max_right_shift_length: 5.0
       max_left_shift_length: 5.0
 
@@ -30,7 +36,7 @@
 
       # For prevention of large acceleration while avoidance
       min_avoidance_speed_for_acc_prevention: 3.0  # [m/s]
-      max_avoidance_acceleration: 0.5  # [m/ss]
+      max_avoidance_acceleration: 0.5  # [m/s2]
 
       # for debug
       publish_debug_marker: false

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp

@@ -135,7 +135,7 @@ class AvoidanceModule : public SceneModuleInterface
 
   // -- path generation --
   ShiftedPath generateAvoidancePath(PathShifter & shifter) const;
-  void generateExtendedDrivableArea(ShiftedPath * shifted_path, double margin) const;
+  void generateExtendedDrivableArea(ShiftedPath * shifted_path) const;
 
   // -- velocity planning --
   void modifyPathVelocityToPreventAccelerationOnAvoidance(ShiftedPath & shifted_path) const;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp

@@ -52,6 +52,13 @@ struct AvoidanceParameters
   // lanelet expand length for left side to find avoidance target vehicles
   double detection_area_left_expand_dist = 1.0;
 
+  // enable avoidance to be perform only in lane with same direction
+  bool enable_avoidance_over_same_direction{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 enable_avoidance_over_opposite_direction{true};
+
   // Vehicles whose distance to the center of the path is
   // less than this will not be considered for avoidance.
   double threshold_distance_object_is_on_center;
@@ -67,6 +74,9 @@ struct AvoidanceParameters
 
   // we want to keep this lateral margin when avoiding
   double lateral_collision_margin;
+  // a buffer in case lateral_collision_margin is set to 0. Will throw error
+  // don't ever set this value to 0
+  double lateral_collision_safety_buffer{0.5};
 
   // when complete avoidance motion, there is a distance margin with the object
   // for longitudinal direction
@@ -96,6 +106,10 @@ struct AvoidanceParameters
   // distance for avoidance. Need a sharp avoidance path to avoid the object.
   double min_sharp_avoidance_speed;
 
+  // The margin is configured so that the generated avoidance trajectory does not come near to the
+  // road shoulder.
+  double road_shoulder_safety_margin{1.0};
+
   // Even if the obstacle is very large, it will not avoid more than this length for right direction
   double max_right_shift_length;
 
@@ -152,6 +166,15 @@ struct ObjectData  // avoidance target
 
   // count up when object disappeared. Removed when it exceeds threshold.
   int lost_count = 0;
+
+  // store the information of the lanelet which the object's overhang is currently occupying
+  lanelet::ConstLanelet overhang_lanelet;
+
+  // the position of the overhang
+  Pose overhang_pose;
+
+  // lateral distance from overhang to the road shoulder
+  double to_road_shoulder_distance{0.0};
 };
 using ObjectDataArray = std::vector<ObjectData>;
 
@@ -242,6 +265,7 @@ struct DebugData
 {
   std::shared_ptr<lanelet::ConstLanelets> expanded_lanelets;
   std::shared_ptr<lanelet::ConstLanelets> current_lanelets;
+  std::shared_ptr<lanelet::ConstLineStrings3d> linestring_wrt_overhang;
 
   AvoidPointArray current_shift_points;  // in path shifter
   AvoidPointArray new_shift_points;      // in path shifter

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp

@@ -46,7 +46,8 @@ void clipByMinStartIdx(const AvoidPointArray & shift_points, PathWithLaneId & pa
 double calcDistanceToClosestFootprintPoint(
   const PathWithLaneId & path, const PredictedObject & object, const Point & ego_pos);
 
-double calcOverhangDistance(const ObjectData & object_data, const Pose & base_pose);
+double calcOverhangDistance(
+  const ObjectData & object_data, const Pose & base_pose, Point & overhang_pose);
 
 void setEndData(
   AvoidPoint & ap, const double length, const geometry_msgs::msg::Pose & end, const size_t end_idx,

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp

@@ -90,6 +90,12 @@ MarkerArray createPoseMarkerArray(
   const Pose & pose, const std::string & ns, const int64_t id, const double r, const double g,
   const double b);
 
+MarkerArray makeOverhangToRoadShoulderMarkerArray(
+  const behavior_path_planner::ObjectDataArray & objects);
+
+MarkerArray createOvehangFurthestLineStringMarkerArray(
+  const lanelet::ConstLineStrings3d & linestrings, const std::string & ns, const double r,
+  const double g, const double b);
 }  // namespace marker_utils
 
 std::string toStrInfo(const behavior_path_planner::ShiftPointArray & sp_arr);

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -207,12 +207,15 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
   p.resample_interval_for_output = dp("resample_interval_for_output", 3.0);
   p.detection_area_right_expand_dist = dp("detection_area_right_expand_dist", 0.0);
   p.detection_area_left_expand_dist = dp("detection_area_left_expand_dist", 1.0);
+  p.enable_avoidance_over_same_direction = dp("enable_avoidance_over_same_direction", true);
+  p.enable_avoidance_over_opposite_direction = dp("enable_avoidance_over_opposite_direction", true);
 
   p.threshold_distance_object_is_on_center = dp("threshold_distance_object_is_on_center", 1.0);
   p.threshold_speed_object_is_stopped = dp("threshold_speed_object_is_stopped", 1.0);
   p.object_check_forward_distance = dp("object_check_forward_distance", 150.0);
   p.object_check_backward_distance = dp("object_check_backward_distance", 2.0);
   p.lateral_collision_margin = dp("lateral_collision_margin", 2.0);
+  p.lateral_collision_safety_buffer = dp("lateral_collision_safety_buffer", 0.5);
 
   p.prepare_time = dp("prepare_time", 3.0);
   p.min_prepare_distance = dp("min_prepare_distance", 10.0);
@@ -221,6 +224,8 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
   p.min_nominal_avoidance_speed = dp("min_nominal_avoidance_speed", 5.0);
   p.min_sharp_avoidance_speed = dp("min_sharp_avoidance_speed", 1.0);
 
+  p.road_shoulder_safety_margin = dp("road_shoulder_safety_margin", 0.5);
+
   p.max_right_shift_length = dp("max_right_shift_length", 1.5);
   p.max_left_shift_length = dp("max_left_shift_length", 1.5);