feat(rviz): update autoware.rviz for motion_velocity_obstacle_<stop/s…

…low_down/cruise>_module (autowarefoundation#1314) * feat: add motion_velocity_obstacle_stop/slow_down/cruise_module Signed-off-by: Takayuki Murooka <[email protected]> * update autoware.rviz Signed-off-by: Takayuki Murooka <[email protected]> * update rviz Signed-off-by: Takayuki Murooka <[email protected]> * disable obstacle_cruise_planner Signed-off-by: Takayuki Murooka <[email protected]> * update motion velocity planner params Signed-off-by: Takayuki Murooka <[email protected]> * add module.param.yaml Signed-off-by: Takayuki Murooka <[email protected]> * enable obstacle_cruise_planner Signed-off-by: Takayuki Murooka <[email protected]> --------- Signed-off-by: Takayuki Murooka <[email protected]>
tier4 · Jan 30, 2025 · 0a0a4ab · 0a0a4ab
1 parent 0f5aa93
commit 0a0a4ab
Show file tree

Hide file tree

Showing 7 changed files with 339 additions and 0 deletions.
diff --git a/autoware_launch/config/planning/preset/default_preset.yaml b/autoware_launch/config/planning/preset/default_preset.yaml
@@ -93,6 +93,15 @@ launch:
       #         none
 
   # motion velocity planner modules
+  - arg:
+      name: launch_obstacle_stop_module
+      default: "false"
+  - arg:
+      name: launch_obstacle_slow_down_module
+      default: "false"
+  - arg:
+      name: launch_obstacle_cruise_module
+      default: "false"
   - arg:
       name: launch_dynamic_obstacle_stop_module
       default: "true"

diff --git a/...g/lane_driving/motion_planning/motion_velocity_planner/motion_velocity_planner.param.yaml b/...g/lane_driving/motion_planning/motion_velocity_planner/motion_velocity_planner.param.yaml
@@ -1,3 +1,14 @@
 /**:
   ros__parameters:
     smooth_velocity_before_planning: true  # [-] if true, smooth the velocity profile of the input trajectory before planning
+
+    trajectory_polygon_collision_check:
+      decimate_trajectory_step_length : 2.0 # longitudinal step length to calculate trajectory polygon for collision checking
+      goal_extended_trajectory_length: 6.0
+
+      # consider the current ego pose (it is not the nearest pose on the reference trajectory)
+      # Both the lateral error and the yaw error are assumed to decrease to zero by the time duration "time_to_convergence"
+      # The both errors decrease with constant rates against the time.
+      consider_current_pose:
+        enable_to_consider_current_pose: true
+        time_to_convergence: 1.5 #[s]
diff --git a/..._planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_cruise.param.yaml b/..._planning/lane_driving/motion_planning/motion_velocity_planner/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/...anning/lane_driving/motion_planning/motion_velocity_planner/obstacle_slow_down.param.yaml b/...anning/lane_driving/motion_planning/motion_velocity_planner/obstacle_slow_down.param.yaml
@@ -0,0 +1,50 @@
+/**:
+  ros__parameters:
+    obstacle_slow_down:
+      slow_down_planning:
+        slow_down_min_acc: -1.0         # slow down min deceleration [m/ss]
+        slow_down_min_jerk: -1.0        # slow down min jerk [m/sss]
+
+        lpf_gain_slow_down_vel: 0.99 # low-pass filter gain for slow down velocity
+        lpf_gain_lat_dist: 0.999 # low-pass filter gain for lateral distance from obstacle to ego's path
+        lpf_gain_dist_to_slow_down: 0.7 # low-pass filter gain for distance to slow down start
+
+        time_margin_on_target_velocity: 1.5 # [s]
+
+        # parameters to calculate slow down velocity by linear interpolation
+        object_type_specified_params:
+          types:
+            - "default"
+          default:
+            moving:
+              min_lat_margin: 0.2
+              max_lat_margin: 1.0
+              min_ego_velocity: 2.0
+              max_ego_velocity: 8.0
+            static:
+              min_lat_margin: 0.2
+              max_lat_margin: 1.0
+              min_ego_velocity: 4.0
+              max_ego_velocity: 8.0
+
+        moving_object_speed_threshold: 0.5 # [m/s] how fast the object needs to move to be considered as "moving"
+        moving_object_hysteresis_range: 0.1 # [m/s] hysteresis range used to prevent chattering when obstacle moves close to moving_object_speed_threshold
+
+      obstacle_filtering:
+        object_type:
+          unknown: false
+          car: true
+          truck: true
+          bus: true
+          trailer: true
+          motorcycle: true
+          bicycle: true
+          pedestrian: true
+          pointcloud: false
+
+        min_lat_margin: 0.0  # lateral margin between obstacle and trajectory band with ego's width to avoid the conflict with the obstacle_stop
+        max_lat_margin: 1.1  # lateral margin between obstacle and trajectory band with ego's width
+        lat_hysteresis_margin: 0.2
+
+        successive_num_to_entry_slow_down_condition: 5
+        successive_num_to_exit_slow_down_condition: 5
diff --git a/...io_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_stop.param.yaml b/...io_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_stop.param.yaml
@@ -0,0 +1,74 @@
+/**:
+  ros__parameters:
+    obstacle_stop:
+      option:
+        ignore_crossing_obstacle: true
+        suppress_sudden_stop: true
+
+      stop_planning:
+        stop_margin : 5.0 # longitudinal margin to obstacle [m]
+        terminal_stop_margin : 3.0 # Stop margin at the goal. This value cannot exceed stop margin. [m]
+        min_behavior_stop_margin: 3.0 # [m]
+
+        hold_stop_velocity_threshold: 0.01 # The maximum ego velocity to hold stopping [m/s]
+        hold_stop_distance_threshold: 0.3 # The ego keeps stopping if the distance to stop changes within the threshold [m]
+
+        stop_on_curve:
+          enable_approaching: false
+          additional_stop_margin: 3.0 # [m]
+          min_stop_margin: 3.0 # [m]
+
+        object_type_specified_params:
+          types: # For the listed types, the node try to read the following type specified values
+            - "default"
+            - "unknown"
+          # default: For the default type, which denotes the other types listed above, the following param is defined implicitly, and the other type-specified parameters are not defined.
+            # limit_min_acc: common_param.yaml/limit.min_acc
+          unknown:
+            limit_min_acc: -1.2 # overwrite the deceleration limit, in usually, common_param.yaml/limit.min_acc is referred.
+            sudden_object_acc_threshold: -1.1 # If a stop can be achieved by a deceleration smaller than this value, it is not considered as "sudden stop".
+            sudden_object_dist_threshold: 1000.0 # If a stop distance is longer than this value, it is not considered as "sudden stop".
+            abandon_to_stop: false # If true, the planner gives up to stop when it cannot avoid to run over while maintaining the deceleration limit.
+
+      obstacle_filtering:
+        object_type:
+          pointcloud: false
+
+          inside:
+            unknown: true
+            car: true
+            truck: true
+            bus: true
+            trailer: true
+            motorcycle: true
+            bicycle: true
+            pedestrian: true
+
+          outside:
+            unknown: false
+            car: false
+            truck: false
+            bus: false
+            trailer: false
+            motorcycle: false
+            bicycle: false
+            pedestrian: false
+
+        # hysteresis for velocity
+        obstacle_velocity_threshold_to_stop : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
+        obstacle_velocity_threshold_from_stop : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
+
+        max_lat_margin: 0.3 # lateral margin between the obstacles except for unknown and ego's footprint
+        max_lat_margin_against_unknown: 0.3 # lateral margin between the unknown obstacles and ego's footprint
+
+        min_velocity_to_reach_collision_point: 2.0 # minimum velocity margin to calculate time to reach collision [m/s]
+        stop_obstacle_hold_time_threshold : 1.0 # maximum time for holding closest stop obstacle
+
+        outside_obstacle:
+          max_lateral_time_margin: 4.5 # time threshold of lateral margin between the obstacles and ego's footprint [s]
+          num_of_predicted_paths: 1 # number of the highest confidence predicted path to check collision between obstacle and ego
+          pedestrian_deceleration_rate: 0.5 # planner perceives pedestrians will stop with this rate to avoid unnecessary stops [m/ss]
+          bicycle_deceleration_rate: 0.5 # planner perceives bicycles will stop with this rate to avoid unnecessary stops [m/ss]
+
+        crossing_obstacle:
+          collision_time_margin : 1.0 # time threshold of collision between obstacle adn ego for cruise or stop [s]
diff --git a/autoware_launch/launch/components/tier4_planning_component.launch.xml b/autoware_launch/launch/components/tier4_planning_component.launch.xml
@@ -74,6 +74,9 @@
     <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_obstacle_stop_module_param_path" value="$(var motion_config_path)/motion_velocity_planner/obstacle_stop.param.yaml"/>
+    <arg name="motion_velocity_planner_obstacle_slow_down_module_param_path" value="$(var motion_config_path)/motion_velocity_planner/obstacle_slow_down.param.yaml"/>
+    <arg name="motion_velocity_planner_obstacle_cruise_module_param_path" value="$(var motion_config_path)/motion_velocity_planner/obstacle_cruise.param.yaml"/>
     <arg name="motion_velocity_planner_dynamic_obstacle_stop_module_param_path" value="$(var motion_config_path)/motion_velocity_planner/dynamic_obstacle_stop.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_planner_obstacle_velocity_limiter_param_path" value="$(var motion_config_path)/motion_velocity_planner/obstacle_velocity_limiter.param.yaml"/>

diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz
@@ -2114,6 +2114,42 @@ Visualization Manager:
                             Reliability Policy: Reliable
                             Value: /planning/scenario_planning/velocity_smoother/virtual_wall
                           Value: true
+                        - Class: rviz_default_plugins/MarkerArray
+                          Enabled: true
+                          Name: VirtualWall (ObstacleStop)
+                          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/obstacle_stop/virtual_walls
+                          Value: true
+                        - Class: rviz_default_plugins/MarkerArray
+                          Enabled: true
+                          Name: VirtualWall (ObstacleSlowDown)
+                          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/obstacle_slow_down/virtual_walls
+                          Value: true
+                        - Class: rviz_default_plugins/MarkerArray
+                          Enabled: true
+                          Name: VirtualWall (ObstacleCruise)
+                          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/obstacle_cruise/virtual_walls
+                          Value: true
                         - Class: rviz_default_plugins/MarkerArray
                           Enabled: true
                           Name: VirtualWall (OutOfLane)
@@ -2251,6 +2287,42 @@ Visualization Manager:
                           Value: false
                         - Class: rviz_common/Group
                           Displays:
+                            - Class: rviz_default_plugins/MarkerArray
+                              Enabled: true
+                              Name: ObstacleStop
+                              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/obstacle_stop/debug_markers
+                              Value: true
+                            - Class: rviz_default_plugins/MarkerArray
+                              Enabled: true
+                              Name: ObstacleSlowDown
+                              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/obstacle_slow_down/debug_markers
+                              Value: true
+                            - Class: rviz_default_plugins/MarkerArray
+                              Enabled: true
+                              Name: ObstacleCruise
+                              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/obstacle_cruise/debug_markers
+                              Value: true
                             - Class: rviz_default_plugins/MarkerArray
                               Enabled: true
                               Name: OutOfLane