feat: Adds documentation and debug marker visualization.

The visualization will show the remaining distance between the overhang_pose and
referenced linestring.

this is a part of .iv PR (tier4/autoware.iv#2383) port

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

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/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp

@@ -265,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/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/scene_module/avoidance/avoidance_module.cpp

@@ -187,6 +187,8 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects(
       ? calcSignedArcLength(path_points, ego_pos, rh->getGoalPose().position)
       : std::numeric_limits<double>::max();
 
+  lanelet::ConstLineStrings3d debug_linestring;
+  debug_linestring.clear();
   // for filtered objects
   ObjectDataArray target_objects;
   for (const auto & i : lane_filtered_objects_index) {
@@ -254,6 +256,7 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects(
         }();
         object_data.to_road_shoulder_distance =
           distance2d(to2D(overhang_basic_pose), to2D(target_left_line.basicLineString()));
+        debug_linestring.push_back(target_left_line);
       } else {
         const auto & target_right_line = [this, &rh, &overhang_lanelet]() {
           if (
@@ -269,6 +272,7 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects(
         }();
         object_data.to_road_shoulder_distance =
           distance2d(to2D(overhang_basic_pose), to2D(target_right_line.basicLineString()));
+        debug_linestring.push_back(target_right_line);
       }
     }
 
@@ -290,6 +294,7 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects(
 
   // debug
   {
+    debug.linestring_wrt_overhang = std::make_shared<lanelet::ConstLineStrings3d>(debug_linestring);
     debug.current_lanelets = std::make_shared<lanelet::ConstLanelets>(current_lanes);
     debug.expanded_lanelets = std::make_shared<lanelet::ConstLanelets>(expanded_lanelets);
   }
@@ -2587,10 +2592,12 @@ void AvoidanceModule::setDebugData(const PathShifter & shifter, const DebugData
   using marker_utils::createAvoidPointMarkerArray;
   using marker_utils::createLaneletsAreaMarkerArray;
   using marker_utils::createObjectsMarkerArray;
+  using marker_utils::createOvehangFurthestLineStringMarkerArray;
   using marker_utils::createPathMarkerArray;
   using marker_utils::createPoseMarkerArray;
   using marker_utils::createShiftLengthMarkerArray;
   using marker_utils::createShiftPointMarkerArray;
+  using marker_utils::makeOverhangToRoadShoulderMarkerArray;
 
   debug_marker_.markers.clear();
 
@@ -2617,6 +2624,9 @@ void AvoidanceModule::setDebugData(const PathShifter & shifter, const DebugData
   add(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0));
   add(createLaneletsAreaMarkerArray(*debug.expanded_lanelets, "expanded_lanelet", 0.8, 0.8, 0.0));
   add(createAvoidanceObjectsMarkerArray(avoidance_data_.objects, "avoidance_object"));
+  add(makeOverhangToRoadShoulderMarkerArray(avoidance_data_.objects));
+  add(createOvehangFurthestLineStringMarkerArray(
+    *debug.linestring_wrt_overhang, "linestring_to_overhang", 1.0, 0.0, 1.0));
 
   // parent object info
   addAvoidPoint(debug.registered_raw_shift, "p_registered_shift", 0.8, 0.8, 0.0);

planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp

@@ -481,7 +481,85 @@ MarkerArray createPoseMarkerArray(
 
   return msg;
 }
+MarkerArray makeOverhangToRoadShoulderMarkerArray(
+  const behavior_path_planner::ObjectDataArray & objects)
+{
+  const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now();
+  MarkerArray msg;
+
+  Marker marker{};
+  marker.header.frame_id = "map";
+  marker.header.stamp = current_time;
+  marker.ns = "overhang";
 
+  const auto normal_color = tier4_autoware_utils::createMarkerColor(1.0, 1.0, 0.0, 1.0);
+
+  int32_t i = 0;
+  for (const auto & object : objects) {
+    marker.id = i++;
+    marker.lifetime = rclcpp::Duration::from_seconds(0.2);
+    marker.type = Marker::TEXT_VIEW_FACING;
+    // marker.action = Marker::ADD;
+    marker.pose = object.overhang_pose;
+    marker.scale = tier4_autoware_utils::createMarkerScale(1.0, 1.0, 1.0);
+    marker.color = normal_color;
+    std::ostringstream string_stream;
+    string_stream << "(to_road_shoulder_distance = " << object.to_road_shoulder_distance << " [m])";
+    marker.text = string_stream.str();
+    msg.markers.push_back(marker);
+  }
+
+  return msg;
+}
+
+MarkerArray createOvehangFurthestLineStringMarkerArray(
+  const lanelet::ConstLineStrings3d & linestrings, const std::string & ns, const double r,
+  const double g, const double b)
+{
+  const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now();
+  MarkerArray msg;
+
+  for (const auto & linestring : linestrings) {
+    Marker marker{};
+    marker.header.frame_id = "map";
+    marker.header.stamp = current_time;
+
+    marker.ns = ns;
+    marker.id = linestring.id();
+    marker.lifetime = rclcpp::Duration::from_seconds(0.2);
+    marker.type = Marker::LINE_STRIP;
+    marker.action = Marker::ADD;
+    marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0);
+    marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.0, 0.0);
+    marker.color = tier4_autoware_utils::createMarkerColor(r, g, b, 0.999);
+    for (const auto & p : linestring.basicLineString()) {
+      Point point;
+      point.x = p.x();
+      point.y = p.y();
+      point.z = p.z();
+      marker.points.push_back(point);
+    }
+    if (!marker.points.empty()) {
+      marker.points.push_back(marker.points.front());
+    }
+    msg.markers.push_back(marker);
+    marker.ns = "linestring id";
+    marker.type = Marker::TEXT_VIEW_FACING;
+    Pose text_id_pose;
+    marker.scale = tier4_autoware_utils::createMarkerScale(1.0, 1.0, 1.0);
+    marker.color = tier4_autoware_utils::createMarkerColor(1.0, 1.0, 1.0, 0.8);
+    text_id_pose.position.x = linestring.front().x();
+    text_id_pose.position.y = linestring.front().y();
+    text_id_pose.position.z = linestring.front().z();
+    marker.pose = text_id_pose;
+    std::ostringstream ss;
+    ss << "(ID : " << linestring.id() << ") ";
+    marker.text = ss.str();
+    msg.markers.push_back(marker);
+  }
+
+  return msg;
+}
 }  // namespace marker_utils
 
 std::string toStrInfo(const behavior_path_planner::ShiftPointArray & sp_arr)