feat(motion_velocity_planner): add new motion velocity planning (autowarefoundation#992)

Signed-off-by: Maxime CLEMENT <[email protected]>

Author: maxime-clem

autoware_launch/config/planning/preset/default_preset.yaml

@@ -76,7 +76,7 @@ launch:
       default: "false"
   - arg:
       name: launch_out_of_lane_module
-      default: "true"
+      default: "false"
   - arg:
       name: launch_no_drivable_lane_module
       default: "false"
@@ -98,6 +98,11 @@ launch:
       #         path_sampler
       #         none
 
+  # motion velocity planner modules
+  - arg:
+      name: launch_motion_out_of_lane_module
+      default: "true"
+
   - arg:
       name: motion_stop_planner_type
       default: obstacle_cruise_planner

autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/motion_velocity_planner.param.yaml

@@ -0,0 +1,3 @@
+/**:
+  ros__parameters:
+    smooth_velocity_before_planning: true  # [-] if true, smooth the velocity profile of the input trajectory before planning

autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/out_of_lane.param.yaml

@@ -0,0 +1,44 @@
+/**:
+  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: false # do not run this module when ego already overlaps another lane
+      ignore_overlaps_over_lane_changeable_lanelets: true  # if true, overlaps on lane changeable lanelets are ignored
+
+      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.
+        predicted_path_min_confidence : 0.1  # when using predicted paths, ignore the ones whose confidence is lower than this value.
+        distance_buffer: 1.0 # [m] distance buffer used to determine if a collision will occur in the other lane
+        cut_predicted_paths_beyond_red_lights: true # if true, predicted paths are cut beyond the stop line of red traffic lights
+
+      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
+        precision: 0.1  # [m] precision when inserting a stop pose in the path
+        distance_buffer: 1.5  # [m] buffer distance to try to keep between the ego footprint and lane
+        min_duration: 1.0  # [s] minimum duration needed before a decision can be canceled
+        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:
+        min_assumed_velocity: 2.0 # [m/s] minimum velocity used to calculate the enter and exit times of 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

autoware_launch/launch/components/tier4_planning_component.launch.xml

@@ -69,6 +69,9 @@
     <arg name="obstacle_stop_planner_param_path" value="$(var motion_config_path)/obstacle_stop_planner/obstacle_stop_planner.param.yaml"/>
     <arg name="obstacle_stop_planner_acc_param_path" value="$(var motion_config_path)/obstacle_stop_planner/adaptive_cruise_control.param.yaml"/>
     <arg name="obstacle_cruise_planner_param_path" value="$(var motion_config_path)/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml"/>
+    <arg name="motion_velocity_planner_param_path" value="$(var motion_config_path)/motion_velocity_planner/motion_velocity_planner.param.yaml"/>
+    <arg name="motion_velocity_planner_out_of_lane_module_param_path" value="$(var motion_config_path)/motion_velocity_planner/out_of_lane.param.yaml"/>
+    <arg name="motion_velocity_smoother_type_param_path" value="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/common/motion_velocity_smoother/Analytical.param.yaml"/>
 
     <!-- motion velocity smoother -->
     <arg name="motion_velocity_smoother_param_path" value="$(var common_config_path)/motion_velocity_smoother/motion_velocity_smoother.param.yaml"/>

autoware_launch/rviz/autoware.rviz

@@ -2128,6 +2128,18 @@ Visualization Manager:
                             Reliability Policy: Reliable
                             Value: /planning/scenario_planning/motion_velocity_smoother/virtual_wall
                           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/motion_planning/motion_velocity_planner/out_of_lane/virtual_walls
+                          Value: true
                       Enabled: true
                       Name: VirtualWall
                     - Class: rviz_common/Group
@@ -2251,6 +2263,22 @@ Visualization Manager:
                             Reliability Policy: Reliable
                             Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker
                           Value: false
+                        - Class: rviz_common/Group
+                          Displays:
+                            - Class: rviz_default_plugins/MarkerArray
+                              Enabled: true
+                              Name: OutOfLane
+                              Namespaces:
+                                {}
+                              Topic:
+                                Depth: 5
+                                Durability Policy: Volatile
+                                History Policy: Keep Last
+                                Reliability Policy: Reliable
+                                Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/out_of_lane/debug_markers
+                              Value: true
+                          Enabled: false
+                          Name: MotionVelocityPlanner
                       Enabled: false
                       Name: DebugMarker
                   Enabled: true