Merge pull request #287 from tier4/sync-upstream

chore: sync upstream

autoware_launch/rviz/autoware.rviz

@@ -1302,6 +1302,43 @@ Visualization Manager:
                             Reliability Policy: Reliable
                             Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/run_out
                           Value: false
+                        - Class: rviz_common/Group
+                          Displays:
+                            - Class: rviz_default_plugins/MarkerArray
+                              Enabled: true
+                              Name: GoalSearchArea
+                              Namespaces:
+                                collision_polygons: true
+                              Topic:
+                                Depth: 5
+                                Durability Policy: Volatile
+                                History Policy: Keep Last
+                                Reliability Policy: Reliable
+                                Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/debug/parking_area
+                              Value: false
+                            - Alpha: 0.9990000128746033
+                              Arrow Length: 0.30000001192092896
+                              Axes Length: 0.30000001192092896
+                              Axes Radius: 0.009999999776482582
+                              Class: rviz_default_plugins/PoseArray
+                              Color: 255; 25; 0
+                              Enabled: false
+                              Head Length: 0.10000000149011612
+                              Head Radius: 0.20000000298023224
+                              Name: PullOverPathPoseArray
+                              Shaft Length: 0.20000000298023224
+                              Shaft Radius: 0.05000000074505806
+                              Shape: Arrow (3D)
+                              Topic:
+                                Depth: 5
+                                Durability Policy: Volatile
+                                Filter size: 10
+                                History Policy: Keep Last
+                                Reliability Policy: Reliable
+                                Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/debug/path_pose_array
+                              Value: false
+                          Enabled: false
+                          Name: PullOver
                       Enabled: false
                       Name: DebugMarker
                   Enabled: true
@@ -1519,6 +1556,26 @@ Visualization Manager:
                   Value: true
               Enabled: true
               Name: Parking
+            - Alpha: 1
+              Axes Length: 1
+              Axes Radius: 0.30000001192092896
+              Class: rviz_default_plugins/Pose
+              Color: 255; 25; 0
+              Enabled: true
+              Head Length: 0.30000001192092896
+              Head Radius: 0.10000000149011612
+              Name: ModifiedGoal
+              Shaft Length: 1
+              Shaft Radius: 0.05000000074505806
+              Shape: Axes
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                Filter size: 10
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /planning/scenario_planning/modified_goal
+              Value: true
           Enabled: true
           Name: ScenarioPlanning
       Enabled: true

perception_launch/launch/perception.launch.xml

@@ -4,6 +4,7 @@
   <!-- common parameters -->
   <arg name="input/pointcloud" default="/sensing/lidar/concatenated/pointcloud" description="The topic will be used in the detection module"/>
   <arg name="mode" default="camera_lidar_fusion" description="options: `camera_lidar_radar_fusion`, `camera_lidar_fusion`, `lidar_radar_fusion`, `lidar` or `radar`"/>
+  <arg name="vehicle_param_file" default="$(find-pkg-share vehicle_info_util)/config/vehicle_info.param.yaml" description="path to the file of vehicle info yaml"/>
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`"/>
   <arg name="image_raw0" default="/sensing/camera/camera0/image_rect_color" description="image raw topic name"/>
   <arg name="camera_info0" default="/sensing/camera/camera0/camera_info" description="camera info topic name"/>
@@ -42,6 +43,7 @@
         <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)" />
         <arg name="container_name" value="$(var pointcloud_container_name)"/>
         <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
+        <arg name="vehicle_param_file" value="$(var vehicle_param_file)"/>
       </include>
     </group>
 

planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml

@@ -1,25 +1,54 @@
 /**:
   ros__parameters:
     pull_over:
+      request_length: 100.0
+      th_arrived_distance_m: 1.0
+      th_stopped_velocity_mps: 0.01
+      th_stopped_time_sec: 2.0  # It must be greater than the state_machine's.
+      pull_over_velocity: 2.0
+      pull_over_minimum_velocity: 0.3
+      margin_from_boundary: 0.5
+      decide_path_distance: 10.0
+      min_acc: -0.5
+      enable_shift_parking: true
+      enable_arc_forward_parking: true
+      enable_arc_backward_parking: false
+      # goal research
+      search_priority: "efficient_path" # "efficient_path" or "close_goal"
+      enable_goal_research: true
+      forward_goal_search_length: 20.0
+      backward_goal_search_length: 20.0
+      goal_search_interval: 1.0
+      goal_to_obj_margin: 2.0
+      # occupancy grid map
+      collision_check_margin: 0.5
+      theta_size: 360
+      obstacle_threshold: 60
+      # shift path
+      pull_over_sampling_num: 4
+      maximum_lateral_jerk: 2.0
+      minimum_lateral_jerk: 0.5
+      deceleration_interval: 15.0
+      maximum_deceleration: 1.0
+      after_pull_over_straight_distance: 5.0
+      before_pull_over_straight_distance: 5.0
+      # parallel parking path
+      after_forward_parking_straight_distance: 2.0
+      after_backward_parking_straight_distance: 2.0
+      forward_parking_velocity: 0.3
+      backward_parking_velocity: -0.3
+      arc_path_interval: 1.0
+      # hazard. Not used now.
+      hazard_on_threshold_dis: 1.0
+      hazard_on_threshold_vel: 0.5
+      # check safety with dynamic objects. Not used now.
+      pull_over_duration: 2.0
+      pull_over_prepare_duration: 4.0
       min_stop_distance: 5.0
       stop_time: 2.0
       hysteresis_buffer_distance: 2.0
-      pull_over_prepare_duration: 4.0
-      pull_over_duration: 2.0
-      pull_over_finish_judge_buffer: 3.0
-      minimum_pull_over_velocity: 3.0
-      prediction_duration: 30.0
-      prediction_time_resolution: 0.5
-      static_obstacle_velocity_thresh: 1.5
-      maximum_deceleration: 1.0
       enable_collision_check_at_prepare_phase: false
       use_predicted_path_outside_lanelet: false
       use_all_predicted_path: false
-      enable_blocked_by_obstacle: false
-      pull_over_search_distance: 30.0
-      after_pull_over_straight_distance: 5.0
-      before_pull_over_straight_distance: 5.0
-      margin_from_boundary: 0.5
-      maximum_lateral_jerk: 2.0
-      minimum_lateral_jerk: 0.5
-      deceleration_interval: 15.0
+      # debug
+      print_debug_info: false

planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml

@@ -10,7 +10,7 @@
       stop_position_threshold: 1.0 # [m] threshold for check whether the vehicle stop in front of crosswalk
 
       # param for ego velocity
-      slow_velocity: 2.78 # [m/s] target vehicle velocity when module receive slow down command from FOA (2.78 m/s = 10.0 kmph)
+      min_slow_down_velocity: 2.78 # [m/s] target vehicle velocity when module receive slow down command from FOA (2.78 m/s = 10.0 kmph)
       max_slow_down_jerk: -1.5 # [m/sss] minimum jerk deceleration for safe brake
       max_slow_down_accel: -2.5 # [m/ss] minimum accel deceleration for safe brake
       no_relax_velocity: 2.78 # [m/s] if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints 2.78 m/s = 10 kmph)