feat(behavior_velocity_planner): add out of lane module (#269)

* Add initial param file for new out_of_lane module Signed-off-by: Maxime CLEMENT <[email protected]> * Add params for extending the ego footprint Signed-off-by: Maxime CLEMENT <[email protected]> * Add more parameters Signed-off-by: Maxime CLEMENT <[email protected]> * add a few more params Signed-off-by: Maxime CLEMENT <[email protected]> * Add/rename parameters for new version with 3 methods (thr, inter, ttc) Signed-off-by: Maxime CLEMENT <[email protected]> * Add parameters for "skip_if_*", "strict", and "use_predicted_path" Signed-off-by: Maxime CLEMENT <[email protected]> * Update default parameters Signed-off-by: Maxime CLEMENT <[email protected]> * style(pre-commit): autofix * Fix typo Signed-off-by: Maxime CLEMENT <[email protected]> * Change param ego.extra_front_offset 1.0 -> 0.0 Signed-off-by: Maxime CLEMENT <[email protected]> * Update rviz config with "out_of_lane" virtual wall and debug markers Signed-off-by: Maxime CLEMENT <[email protected]> --------- Signed-off-by: Maxime CLEMENT <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
tier4 · Apr 10, 2023 · 2a9ef1c · 2a9ef1c
1 parent ea7cb4e
commit 2a9ef1c
Show file tree

Hide file tree

Showing 4 changed files with 65 additions and 0 deletions.
diff --git a/..._driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml b/..._driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml
@@ -11,6 +11,7 @@
     launch_no_stopping_area: true
     launch_run_out: false
     launch_speed_bump: false
+    launch_out_of_lane: true
     forward_path_length: 1000.0
     backward_path_length: 5.0
     stop_line_extend_length: 5.0

diff --git a/..._planning/lane_driving/behavior_planning/behavior_velocity_planner/out_of_lane.param.yaml b/..._planning/lane_driving/behavior_planning/behavior_velocity_planner/out_of_lane.param.yaml
@@ -0,0 +1,39 @@
+/**:
+  ros__parameters:
+    out_of_lane:  # module to stop or slowdown before overlapping another lane with other objects
+      mode: ttc # mode used to consider a conflict with an object. "threshold", "intervals", or "ttc"
+      skip_if_already_overlapping: true # do not run this module when ego already overlaps another lane
+
+      threshold:
+        time_threshold: 5.0  # [s] consider objects that will reach an overlap within this time
+      intervals:  # consider objects if their estimated time interval spent on the overlap intersect the estimated time interval of ego
+        ego_time_buffer: 0.5 # [s] extend the ego time interval by this buffer
+        objects_time_buffer: 0.5 # [s] extend the time intervals of objects by this buffer
+      ttc:
+        threshold: 3.0 # [s] consider objects with an estimated time to collision bellow this value while on the overlap
+
+      objects:
+        minimum_velocity: 0.5  # [m/s] objects lower than this velocity will be ignored
+        use_predicted_paths: true  # if true, use the predicted paths to estimate future positions.
+                                   # if false, assume the object moves at constant velocity along *all* lanelets it currently is located in.
+
+      overlap:
+        minimum_distance: 0.0  # [m] minimum distance inside a lanelet for an overlap to be considered
+        extra_length: 0.0  # [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times)
+
+      action:  # action to insert in the path if an object causes a conflict at an overlap
+        skip_if_over_max_decel: true  # if true, do not take an action that would cause more deceleration than the maximum allowed
+        strict: true # if true, when a decision is taken to avoid entering a lane, the stop point will make sure no lane at all is entered by ego
+                     # if false, ego stops just before entering a lane but may then be overlapping another lane.
+        distance_buffer: 1.5  # [m] buffer distance to try to keep between the ego footprint and lane
+        slowdown:
+          distance_threshold: 30.0 # [m] insert a slowdown when closer than this distance from an overlap
+          velocity: 2.0  # [m/s] slowdown velocity
+        stop:
+          distance_threshold: 15.0 # [m] insert a stop when closer than this distance from an overlap
+
+      ego:
+        extra_front_offset: 0.0 # [m] extra front distance
+        extra_rear_offset: 0.0 # [m] extra rear distance
+        extra_right_offset: 0.0 # [m] extra right distance
+        extra_left_offset: 0.0 # [m] extra left distance
diff --git a/autoware_launch/launch/components/tier4_planning_component.launch.xml b/autoware_launch/launch/components/tier4_planning_component.launch.xml
@@ -89,6 +89,7 @@
     />
     <arg name="run_out_param_path" value="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml"/>
     <arg name="speed_bump_param_path" value="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/speed_bump.param.yaml"/>
+    <arg name="out_of_lane_param_path" value="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/out_of_lane.param.yaml"/>
     <arg
       name="behavior_velocity_planner_param_path"
       value="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml"

diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz
@@ -1506,6 +1506,18 @@ Visualization Manager:
                             Reliability Policy: Reliable
                             Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/speed_bump
                           Value: true
+                        - Class: rviz_default_plugins/MarkerArray
+                          Enabled: true
+                          Name: VirtualWall (OutOfLane)
+                          Namespaces:
+                            {}
+                          Topic:
+                            Depth: 5
+                            Durability Policy: Volatile
+                            History Policy: Keep Last
+                            Reliability Policy: Reliable
+                            Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/out_of_lane
+                          Value: true
                       Enabled: true
                       Name: VirtualWall
                     - Class: rviz_common/Group
@@ -1726,6 +1738,18 @@ Visualization Manager:
                             Reliability Policy: Reliable
                             Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/speed_bump
                           Value: false
+                        - Class: rviz_default_plugins/MarkerArray
+                          Enabled: false
+                          Name: OutOfLane
+                          Namespaces:
+                            {}
+                          Topic:
+                            Depth: 5
+                            Durability Policy: Volatile
+                            History Policy: Keep Last
+                            Reliability Policy: Reliable
+                            Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/out_of_lane
+                          Value: false
                       Enabled: false
                       Name: DebugMarker
                   Enabled: true