feat: introduce motion_velocity_obstacle_<stop/slow_down/cruise>_modu…

…le (#9807) * implement obstacle stop, slow_down, and cruise Signed-off-by: Takayuki Murooka <[email protected]> * fix clang-tidy Signed-off-by: Takayuki Murooka <[email protected]> * revert obstacle_cruise_planner Signed-off-by: Takayuki Murooka <[email protected]> --------- Signed-off-by: Takayuki Murooka <[email protected]>
autowarefoundation · Jan 30, 2025 · 04ed652 · 04ed652
1 parent 17927bf
commit 04ed652
Show file tree

Hide file tree

Showing 52 changed files with 8,541 additions and 0 deletions.
diff --git a/planning/.pages b/planning/.pages
@@ -63,6 +63,9 @@ nav:
     - 'Motion Velocity Planner':
       - 'About Motion Velocity Planner': planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/
       - 'Available Modules':
+        - 'Obstacle Stop': planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/
+        - 'Obstacle Slow Down': planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/
+        - 'Obstacle Cruise': planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/
         - 'Dynamic Obstacle Stop': planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/
         - 'Out of Lane': planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/
         - 'Obstacle Velocity Limiter': planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/

diff --git a/...ng/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/CMakeLists.txt b/...ng/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/CMakeLists.txt
@@ -0,0 +1,12 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_motion_velocity_obstacle_cruise_module)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+pluginlib_export_plugin_description_file(autoware_motion_velocity_planner_node_universe plugins.xml)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  DIRECTORY src
+)
+
+ament_auto_package(INSTALL_TO_SHARE config)
diff --git a/...tion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/README.md b/...tion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/README.md
@@ -0,0 +1,118 @@
+# Obstacle Cruise
+
+## Role
+
+The `obstacle_cruise` module does the cruise planning against a dynamic obstacle in front of the ego.
+
+## Activation
+
+This module is activated if the launch parameter `launch_obstacle_cruise_module` is set to true.
+
+## Inner-workings / Algorithms
+
+### Obstacle Filtering
+
+The obstacles meeting the following condition are determined as obstacles for cruising.
+
+- The lateral distance from the object to the ego's trajectory is smaller than `obstacle_filtering.max_lat_margin`.
+
+- The object type is for cruising according to `obstacle_filtering.object_type.*`.
+- The object is not crossing the ego's trajectory (\*1).
+- If the object is inside the trajectory.
+  - The object type is for inside cruising according to `obstacle_filtering.object_type.inside.*`.
+  - The object velocity is larger than `obstacle_filtering.obstacle_velocity_threshold_from_cruise`.
+- If the object is outside the trajectory.
+  - The object type is for outside cruising according to `obstacle_filtering.object_type.outside.*`.
+  - The object velocity is larger than `obstacle_filtering.outside_obstacle.obstacle_velocity_threshold`.
+  - The highest confident predicted path collides with the ego's trajectory.
+  - Its collision's period is larger than `obstacle_filtering.outside_obstacle.ego_obstacle_overlap_time_threshold`.
+
+#### NOTE
+
+##### \*1: Crossing obstacles
+
+Crossing obstacle is the object whose orientation's yaw angle against the ego's trajectory is smaller than `obstacle_filtering.crossing_obstacle.obstacle_traj_angle_threshold`.
+
+##### Yield for vehicles that might cut in into the ego's lane
+
+It is also possible to yield (cruise) behind vehicles in neighbor lanes if said vehicles might cut in the ego vehicle's current lane.
+
+The obstacles meeting the following condition are determined as obstacles for yielding (cruising).
+
+- The object type is for cruising according to `obstacle_filtering.object_type.*` and it is moving with a speed greater than `obstacle_filtering.yield.stopped_obstacle_velocity_threshold`.
+- The object is not crossing the ego's trajectory (\*1).
+- There is another object of type `obstacle_filtering.object_type.*` stopped in front of the moving obstacle.
+- The lateral distance (using the ego's trajectory as reference) between both obstacles is less than `obstacle_filtering.yield.max_lat_dist_between_obstacles`
+- Both obstacles, moving and stopped, are within `obstacle_filtering.yield.lat_distance_threshold` and `obstacle_filtering.yield.lat_distance_threshold` + `obstacle_filtering.yield.max_lat_dist_between_obstacles` lateral distance from the ego's trajectory respectively.
+
+If the above conditions are met, the ego vehicle will cruise behind the moving obstacle, yielding to it so it can cut in into the ego's lane to avoid the stopped obstacle.
+
+### Cruise Planning
+
+The role of the cruise planning is keeping a safe distance with dynamic vehicle objects with smoothed velocity transition.
+This includes not only cruising a front vehicle, but also reacting a cut-in and cut-out vehicle.
+
+The safe distance is calculated dynamically based on the Responsibility-Sensitive Safety (RSS) by the following equation.
+
+$$
+d_{rss} = v_{ego} t_{idling} + \frac{1}{2} a_{ego} t_{idling}^2 + \frac{v_{ego}^2}{2 a_{ego}} - \frac{v_{obstacle}^2}{2 a_{obstacle}},
+$$
+
+assuming that $d_{rss}$ is the calculated safe distance, $t_{idling}$ is the idling time for the ego to detect the front vehicle's deceleration, $v_{ego}$ is the ego's current velocity, $v_{obstacle}$ is the front obstacle's current velocity, $a_{ego}$ is the ego's acceleration, and $a_{obstacle}$ is the obstacle's acceleration.
+These values are parameterized as follows. Other common values such as ego's minimum acceleration is defined in `common.param.yaml`.
+
+| Parameter                                  | Type   | Description                                                                   |
+| ------------------------------------------ | ------ | ----------------------------------------------------------------------------- |
+| `cruise_planning.idling_time`              | double | idling time for the ego to detect the front vehicle starting deceleration [s] |
+| `cruise_planning.min_ego_accel_for_rss`    | double | ego's acceleration for RSS [m/ss]                                             |
+| `cruise_planning.min_object_accel_for_rss` | double | front obstacle's acceleration for RSS [m/ss]                                  |
+
+The detailed formulation is as follows.
+
+$$
+\begin{align}
+d_{error} & = d - d_{rss} \\
+d_{normalized} & = lpf(d_{error} / d_{obstacle}) \\
+d_{quad, normalized} & = sign(d_{normalized}) *d_{normalized}*d_{normalized} \\
+v_{pid} & = pid(d_{quad, normalized}) \\
+v_{add} & = v_{pid} > 0 ? v_{pid}* w_{acc} : v_{pid} \\
+v_{target} & = max(v_{ego} + v_{add}, v_{min, cruise})
+\end{align}
+$$
+
+| Variable          | Description                             |
+| ----------------- | --------------------------------------- |
+| `d`               | actual distance to obstacle             |
+| `d_{rss}`         | ideal distance to obstacle based on RSS |
+| `v_{min, cruise}` | `min_cruise_target_vel`                 |
+| `w_{acc}`         | `output_ratio_during_accel`             |
+| `lpf(val)`        | apply low-pass filter to `val`          |
+| `pid(val)`        | apply pid to `val`                      |
+
+#### Algorithm selection for cruise planner
+
+Currently, only a PID-based planner is supported.
+Each planner will be explained in the following.
+
+| Parameter                   | Type   | Description                                                  |
+| --------------------------- | ------ | ------------------------------------------------------------ |
+| `option.planning_algorithm` | string | cruise and stop planning algorithm, selected from "pid_base" |
+
+#### PID-based planner
+
+In order to keep the safe distance, the target velocity and acceleration is calculated and sent as an external velocity limit to the velocity smoothing package (`velocity_smoother` by default).
+The target velocity and acceleration is respectively calculated with the PID controller according to the error between the reference safe distance and the actual distance.
+
+#### Optimization-based planner
+
+under construction
+
+## Debugging
+
+### Obstacle for cruise
+
+Orange sphere which is an obstacle for cruise is visualized by `obstacles_to_cruise` in the `~/debug/marker` topic.
+
+Orange wall which means a safe distance to cruise if the ego's front meets the wall is visualized in the `~/debug/cruise/virtual_wall` topic.
+
+![cruise_visualization](./docs/cruise_visualization.png)
diff --git a/...planner/autoware_motion_velocity_obstacle_cruise_module/config/obstacle_cruise.param.yaml b/...planner/autoware_motion_velocity_obstacle_cruise_module/config/obstacle_cruise.param.yaml
@@ -0,0 +1,120 @@
+/**:
+  ros__parameters:
+    obstacle_cruise:
+      option:
+        planning_algorithm: "pid_base" # currently supported algorithm is "pid_base"
+
+      cruise_planning:
+        idling_time: 2.0 # idling time to detect front vehicle starting deceleration [s]
+        min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss]
+        min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss]
+        safe_distance_margin : 5.0 # This is also used as a stop margin [m]
+
+        pid_based_planner:
+          use_velocity_limit_based_planner: true
+          error_function_type: quadratic # choose from linear, quadratic
+
+          velocity_limit_based_planner:
+              # PID gains to keep safe distance with the front vehicle
+              kp: 10.0
+              ki: 0.0
+              kd: 2.0
+
+              output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]
+              vel_to_acc_weight: 12.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-]
+
+              enable_jerk_limit_to_output_acc: false
+
+              disable_target_acceleration: true
+
+          velocity_insertion_based_planner:
+              kp_acc: 6.0
+              ki_acc: 0.0
+              kd_acc: 2.0
+
+              kp_jerk: 20.0
+              ki_jerk: 0.0
+              kd_jerk: 0.0
+
+              output_acc_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]
+              output_jerk_ratio_during_accel: 1.0 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]
+
+              enable_jerk_limit_to_output_acc: true
+
+          min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss]
+          min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s]
+          time_to_evaluate_rss: 0.0
+
+          lpf_normalized_error_cruise_dist_gain: 0.2
+
+        optimization_based_planner:
+          dense_resampling_time_interval: 0.2
+          sparse_resampling_time_interval: 2.0
+          dense_time_horizon: 5.0
+          max_time_horizon: 25.0
+          velocity_margin: 0.2 #[m/s]
+
+          # Parameters for safe distance
+          t_dangerous: 0.5
+
+          # For initial Motion
+          replan_vel_deviation: 5.0         # velocity deviation to replan initial velocity [m/s]
+          engage_velocity: 0.25              # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed)
+          engage_acceleration: 0.1           # engage acceleration [m/ss] (use this acceleration when engagement)
+          engage_exit_ratio: 0.5             # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity.
+          stop_dist_to_prohibit_engage: 0.5  # if the stop point is in this distance, the speed is set to 0 not to move the vehicle [m]
+
+          # Weights for optimization
+          max_s_weight: 100.0
+          max_v_weight: 1.0
+          over_s_safety_weight:  1000000.0
+          over_s_ideal_weight:  50.0
+          over_v_weight:  500000.0
+          over_a_weight:  5000.0
+          over_j_weight:  10000.0
+
+      obstacle_filtering:
+        object_type:
+          inside:
+            unknown: true
+            car: true
+            truck: true
+            bus: true
+            trailer: true
+            motorcycle: true
+            bicycle: true
+            pedestrian: false
+
+          outside:
+            unknown: false
+            car: true
+            truck: true
+            bus: true
+            trailer: true
+            motorcycle: true
+            bicycle: false
+            pedestrian: false
+
+        max_lat_margin: 1.0 # lateral margin between obstacle and trajectory band with ego's width
+
+        # if crossing vehicle is determined as target obstacles or not
+        crossing_obstacle:
+          obstacle_velocity_threshold : 1.0 # velocity threshold for crossing obstacle for cruise or stop [m/s]
+          obstacle_traj_angle_threshold : 0.523599 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop
+
+        outside_obstacle:
+          obstacle_velocity_threshold : 1.5 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s]
+          ego_obstacle_overlap_time_threshold : 0.2 #  time threshold to decide cut-in obstacle for cruise or stop [s]
+          max_prediction_time_for_collision_check : 10.0 # prediction time to check collision between obstacle and ego
+          max_lateral_time_margin: 5.0 # time threshold of lateral margin between obstacle and trajectory band with ego's width [s]
+          num_of_predicted_paths: 1 # number of the highest confidence predicted path to check collision between obstacle and ego
+        yield:
+          enable_yield: true
+          lat_distance_threshold: 5.0 # lateral margin between obstacle in neighbor lanes and trajectory band with ego's width for yielding
+          max_lat_dist_between_obstacles: 2.5 # lateral margin between moving obstacle in neighbor lanes and stopped obstacle in front of it
+          max_obstacles_collision_time: 10.0 # how far the blocking obstacle
+          stopped_obstacle_velocity_threshold: 0.5
+
+        # hysteresis for cruise and stop
+        obstacle_velocity_threshold_from_cruise : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
+        obstacle_velocity_threshold_to_cruise : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
diff --git a/...r/autoware_motion_velocity_obstacle_cruise_module/docs/cruise_visualization.png b/...r/autoware_motion_velocity_obstacle_cruise_module/docs/cruise_visualization.png
diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/package.xml
@@ -0,0 +1,41 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_motion_velocity_obstacle_cruise_module</name>
+  <version>0.40.0</version>
+  <description>obstacle cruise feature in motion_velocity_planner</description>
+
+  <maintainer email="[email protected]">Takayuki Murooka</maintainer>
+  <maintainer email="[email protected]">Yuki Takagi</maintainer>
+  <maintainer email="[email protected]">Maxime Clement</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+  <depend>autoware_motion_utils</depend>
+  <depend>autoware_motion_velocity_planner_common_universe</depend>
+  <depend>autoware_osqp_interface</depend>
+  <depend>autoware_perception_msgs</depend>
+  <depend>autoware_planning_msgs</depend>
+  <depend>autoware_route_handler</depend>
+  <depend>autoware_signal_processing</depend>
+  <depend>autoware_universe_utils</depend>
+  <depend>autoware_vehicle_info_utils</depend>
+  <depend>geometry_msgs</depend>
+  <depend>libboost-dev</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>tf2</depend>
+  <depend>tier4_metric_msgs</depend>
+  <depend>tier4_planning_msgs</depend>
+  <depend>visualization_msgs</depend>
+
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/plugins.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/plugins.xml
@@ -0,0 +1,3 @@
+<library path="autoware_motion_velocity_obstacle_cruise_module">
+  <class type="autoware::motion_velocity_planner::ObstacleCruiseModule" base_class_type="autoware::motion_velocity_planner::PluginModuleInterface"/>
+</library>