Merge pull request #198 from tier4/sync-upstream

sync upstream
tier4 · Nov 15, 2021 · e715151 · e715151
2 parents 0b83ea8 + 06123fe
commit e715151
Show file tree

Hide file tree

Showing 35 changed files with 692 additions and 282 deletions.
diff --git a/autoware_api_launch/launch/include/external_api_adaptor.launch.py b/autoware_api_launch/launch/include/external_api_adaptor.launch.py
@@ -33,13 +33,15 @@ def generate_launch_description():
         _create_api_node('door', 'Door'),
         _create_api_node('emergency', 'Emergency'),
         # _create_api_node('engage', 'Engage'),
+        _create_api_node('fail_safe_state', 'FailSafeState'),
         _create_api_node('initial_pose', 'InitialPose'),
         _create_api_node('map', 'Map'),
         _create_api_node('operator', 'Operator'),
         _create_api_node('metadata_packages', 'MetadataPackages'),
         _create_api_node('route', 'Route'),
         _create_api_node('service', 'Service'),
         # _create_api_node('start', 'Start'),
+        _create_api_node('vehicle_status', 'VehicleStatus'),
         _create_api_node('velocity', 'Velocity'),
         _create_api_node('version', 'Version'),
     ]

diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz
@@ -383,7 +383,12 @@ Visualization Manager:
             right_lane_bound: true
             road_lanelets: false
             stop_lines: true
+            shoulder_center_lane_line: false
+            shoulder_left_lane_bound: true
+            shoulder_right_lane_bound: true
+            shoulder_road_lanelets: false
             traffic_light: true
+            traffic_light_id: false
             traffic_light_triangle: true
             walkway_lanelets: true
           Topic:
@@ -1040,32 +1045,32 @@ Visualization Manager:
                       Value: true
                     - Class: rviz_default_plugins/MarkerArray
                       Enabled: true
-                      Name: LaneChange
+                      Name: OcclusionSpot
                       Namespaces:
-                        candidate_lane_change_path: false
-                        ego_lane_change_path: false
-                        ego_lane_follow_path: false
-                        factor_text: true
-                        object_predicted_path: false
-                        selected_path: false
-                        stop_virtual_wall: true
+                        arrow: false
+                        occlusion spot slow down: true
+                        collision: false
+                        info_obstacle: false
+                        obstacle: false
+                        sidewalk: false
+                        slow factor_text: true
                       Topic:
                         Depth: 5
                         Durability Policy: Volatile
                         History Policy: Keep Last
                         Reliability Policy: Reliable
-                        Value: /planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/debug/predicted_path_markers
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/occlusion_spot
                       Value: true
                     - Class: rviz_plugins/Path
                       Color Border Vel Max: 3
                       Enabled: true
-                      Name: LaneChangeCandidate
+                      Name: PathChangeCandidate
                       Topic:
                         Depth: 5
                         Durability Policy: Volatile
                         History Policy: Keep Last
                         Reliability Policy: Reliable
-                        Value: /planning/scenario_planning/lane_driving/lane_change_candidate_path
+                        Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/output/path_candidate
                       Value: true
                       View Path:
                         Alpha: 0.30000001192092896
@@ -1108,14 +1113,25 @@ Visualization Manager:
                         Value: true
                     - Class: rviz_default_plugins/MarkerArray
                       Enabled: true
-                      Name: ObstaclePointCLoudStop
+                      Name: ObstacleStop
                       Namespaces:
                         collision_polygons: false
                         detection_polygons: false
-                        factor_text: true
-                        stop_virtual_wall: true
+                        slow_down_polygons: false
+                        slow_down_detection_polygons: false
                         stop_obstacle_point: false
                         stop_obstacle_text: false
+                        slow_down_obstacle_point: false
+                        slow_down_obstacle_text: false
+                        slow_down_start_virtual_wall: false
+                        slow_down_start_factor_text: false
+                        slow_down_end_virtual_wall: false
+                        slow_down_end_factor_text: false
+                        slow_down_end: false
+                        stop_virtual_wall: true
+                        stop_factor_text: true
+                        slow_down_virtual_wall: true
+                        slow_down_factor_text: true
                       Topic:
                         Depth: 5
                         Durability Policy: Volatile
@@ -1150,11 +1166,11 @@ Visualization Manager:
                         bounds_candidate_top_text: false
                         constrain_rect: false
                         constrain_rect_text: false
-                        constrain_rectlocation: false
+                        constrain_rect_location: false
                         current_vehicle_footprint: false
                         extending_constrain_rect: false
                         extending_constrain_rect_text: false
-                        extending_constrain_rectlocation: false
+                        extending_constrain_rect_location: false
                         fixed_mpt_points: false
                         fixed_points_for_extending: false
                         fixed_points_marker: false

diff --git a/control_launch/config/mpc_follower/mpc_follower.param.yaml b/control_launch/config/mpc_follower/mpc_follower.param.yaml
@@ -3,15 +3,15 @@
     # -- system --
     ctrl_period: 0.03               # control period [s]
     traj_resample_dist: 0.1         # path resampling interval [m]
-    enable_yaw_recalculation: false # flag for recalculation of yaw angle after resampling
     use_steer_prediction: false     # flag for using steer prediction (do not use steer measurement)
     admissible_position_error: 5.0  # stop mpc calculation when error is larger than the following value
     admissible_yaw_error_rad: 1.57  # stop mpc calculation when error is larger than the following value
 
     # -- path smoothing --
-    enable_path_smoothing: false   # flag for path smoothing
-    path_filter_moving_ave_num: 25 # param of moving average filter for path smoothing
-    curvature_smoothing_num: 15    # point-to-point index distance used in curvature calculation : curvature is calculated from three points p(i-num), p(i), p(i+num)
+    enable_path_smoothing: false            # flag for path smoothing
+    path_filter_moving_ave_num: 25          # param of moving average filter for path smoothing
+    curvature_smoothing_num_ref_steer: 15   # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num)
+    curvature_smoothing_num_traj: 15        # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num)
 
     # -- mpc optimization --
     qpoases_max_iter: 500                        # max iteration number for quadratic programming

diff --git a/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml b/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml
@@ -3,7 +3,8 @@
     update_rate: 10.0
     system_emergency_heartbeat_timeout: 0.5
     external_emergency_stop_heartbeat_timeout: 0.0
-
+    stop_hold_acceleration: -1.5
+    emergency_acceleration: -2.4
     vel_lim: 25.0
     lon_acc_lim: 5.0
     lon_jerk_lim: 5.0

diff --git a/control_launch/config/velocity_controller/velocity_controller.param.yaml b/control_launch/config/velocity_controller/velocity_controller.param.yaml
@@ -1,36 +1,57 @@
 /**:
   ros__parameters:
-    # closest waypoint threshold
-    closest_waypoint_distance_threshold: 3.0
-    closest_waypoint_angle_threshold: 0.7854
-
-    # stop state
-    stop_state_velocity: 0.0
-    stop_state_acc: -3.4
-    stop_state_entry_ego_speed: 0.2
-    stop_state_entry_target_speed: 0.1
-    stop_state_keep_stopping_dist: 0.3
-
     # delay compensation
     delay_compensation_time: 0.50
 
-    # emergency stop by this controller
-    emergency_stop_acc: -5.0
-    emergency_stop_jerk: -3.0
+    # slope compensation
+    enable_slope_compensation: true
+
+    # state transition
+    drive_state_stop_dist: 0.3
+    stopping_state_stop_dist: 0.5
+    stopped_state_entry_vel: 0.01
+    stopped_state_entry_acc: 0.1
+    emergency_state_overshoot_stop_dist: 1.5
+    emergency_state_traj_trans_dev: 3.0
+    emergency_state_traj_rot_dev: 0.7854
+
+    # drive state
+    kp: 1.0
+    ki: 0.1
+    kd: 0.0
+    max_out: 1.0
+    min_out: -1.0
+    max_p_effort: 1.0
+    min_p_effort: -1.0
+    max_i_effort: 0.3
+    min_i_effort: -0.3
+    max_d_effort: 0.0
+    min_d_effort: 0.0
+    lpf_vel_error_gain: 0.9
+    current_velocity_threshold_pid_integration: 0.5
+
+    # smooth stop state
+    smooth_stop_max_strong_acc: -0.5
+    smooth_stop_min_strong_acc: -0.8
+    smooth_stop_weak_acc: -0.3
+    smooth_stop_weak_stop_acc: -0.8
+    smooth_stop_strong_stop_acc: -3.4
+    smooth_stop_min_fast_vel: 0.5
+    smooth_stop_min_running_vel: 0.01
+    smooth_stop_min_running_acc: 0.01
+    smooth_stop_weak_stop_time: 0.8
+    smooth_stop_weak_stop_dist: -0.3
+    smooth_stop_strong_stop_dist: -0.5
 
-    # smooth stop
-    smooth_stop:
-      stop_dist: 0.5
-      exit_ego_speed: 1.0
-      entry_ego_speed: 0.8
-      exit_target_speed: 1.0
-      entry_target_speed: 0.2
-      weak_brake_time: 1.0
-      weak_brake_acc: -1.0
-      increasing_brake_time: 1.0
-      increasing_brake_gradient: -0.1
-      stop_brake_time: 1.0
-      stop_brake_acc: -3.4
+    # stopped state
+    stopped_vel: 0.0
+    stopped_acc: -3.4
+    stopped_jerk: -5.0
+
+    # emergency state
+    emergency_vel: 0.0
+    emergency_acc: -5.0
+    emergency_jerk: -3.0
 
     # acceleration limit
     max_acc: 3.0
@@ -40,25 +61,8 @@
     max_jerk: 2.0
     min_jerk: -5.0
 
-    # slope compensation
-    enable_slope_compensation: true
+    # pitch
     use_trajectory_for_pitch_calculation: false
+    lpf_pitch_gain: 0.95
     max_pitch_rad: 0.1
     min_pitch_rad: -0.1
-    lpf_pitch_gain: 0.95
-
-    # velocity feedback
-    pid_controller:
-      kp: 1.0
-      ki: 0.1
-      kd: 0.0
-      max_out: 1.0
-      min_out: -1.0
-      max_p_effort: 1.0
-      min_p_effort: -1.0
-      max_i_effort: 0.3
-      min_i_effort: -0.3
-      max_d_effort: 0.0
-      min_d_effort: 0.0
-      current_velocity_threshold_pid_integration: 0.5
-      lpf_velocity_error_gain: 0.9
diff --git a/localization_launch/launch/pose_estimator/pose_estimator.launch.xml b/localization_launch/launch/pose_estimator/pose_estimator.launch.xml
@@ -1,5 +1,6 @@
 <?xml version="1.0"?>
 <launch>
+
   <include file="$(find-pkg-share ndt_scan_matcher)/launch/ndt_scan_matcher.launch.xml">
     <arg name="input_map_points_topic" value="/map/pointcloud_map"/>
     <arg name="input_sensor_points_topic" value="/localization/util/downsample/pointcloud"/>

diff --git a/...launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/...launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
@@ -1,5 +1,4 @@
 <?xml version="1.0"?>
-
 <launch>
   <arg name="image_raw0" default="/image_raw" description="image raw topic name"/>
   <arg name="camera_info0" default="/camera_info" description="camera info topic name"/>
@@ -19,6 +18,7 @@
   <arg name="camera_info7" default="/camera_info7"/>
   <arg name="image_number" default="1" description="choose image raw number(0-7)"/>
   <arg name="use_vector_map" default="true" description="use vector map in prediction"/>
+  <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`"/>
 
   <!-- Jetson AGX -->
   <!-- <include file="$(find-pkg-share tensorrt_yolo)/launch/yolo.launch.xml">
@@ -77,67 +77,29 @@
       <arg name="input/object" value="camera_lidar_fusion/objects"/>
       <arg name="output/long_range_object" value="camera_lidar_fusion/long_range_objects"/>
       <arg name="output/short_range_object" value="camera_lidar_fusion/short_range_objects"/>
-      <arg name="split_range" value="10.0"/>
+      <arg name="split_range" value="30.0"/>
     </include>
   </group>
 
-  <group>
-    <push-ros-namespace namespace="apollo"/>
-    <include file="$(find-pkg-share lidar_apollo_instance_segmentation)/launch/lidar_apollo_instance_segmentation.launch.xml">
-      <arg name="output/objects" value="lidar/clusters"/>
+  <group if="$(eval &quot;'$(var lidar_detection_model)'=='centerpoint'&quot;)">
+    <push-ros-namespace namespace="centerpoint"/>
+    <include file="$(find-pkg-share lidar_centerpoint)/launch/lidar_centerpoint.launch.xml">
+      <arg name="output/objects" value="objects" />
     </include>
+  </group>
+
+  <group if="$(eval &quot;'$(var lidar_detection_model)'=='apollo'&quot;)">
+    <push-ros-namespace namespace="apollo"/>
+    <include file="$(find-pkg-share lidar_apollo_instance_segmentation)/launch/lidar_apollo_instance_segmentation.launch.xml" />
     <include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
+      <arg name="output/objects" value="objects"/>
       <arg name="use_map_current" value="$(var use_vector_map)"/>
-      <arg name="input/objects" value="lidar/clusters" />
-      <arg name="output/objects" value="lidar/objects" />
-      <arg name="orientation_reliable" value="true"/>
-    </include>
-    <include file="$(find-pkg-share object_range_splitter)/launch/object_range_splitter.launch.xml">
-      <arg name="input/object" value="lidar/objects"/>
-      <arg name="output/long_range_object" value="lidar/long_range_objects"/>
-      <arg name="output/short_range_object" value="lidar/short_range_objects"/>
-      <arg name="split_range" value="15.0"/>
-    </include>
-    <include file="$(find-pkg-share roi_cluster_fusion)/launch/roi_cluster_fusion.launch.xml">
-      <arg name="input/camera_info0" value="$(var camera_info0)"/>
-      <arg name="input/rois0" value="/perception/object_recognition/detection/rois0"/>
-      <arg name="input/camera_info1" value="$(var camera_info1)"/>
-      <arg name="input/rois1" value="/perception/object_recognition/detection/rois1"/>
-      <arg name="input/camera_info2" value="$(var camera_info2)"/>
-      <arg name="input/rois2" value="/perception/object_recognition/detection/rois2"/>
-      <arg name="input/camera_info3" value="$(var camera_info3)"/>
-      <arg name="input/rois3" value="/perception/object_recognition/detection/rois3"/>
-      <arg name="input/camera_info4" value="$(var camera_info4)"/>
-      <arg name="input/rois4" value="/perception/object_recognition/detection/rois4"/>
-      <arg name="input/camera_info5" value="$(var camera_info5)"/>
-      <arg name="input/rois5" value="/perception/object_recognition/detection/rois5"/>
-      <arg name="input/camera_info6" value="$(var camera_info6)"/>
-      <arg name="input/rois6" value="/perception/object_recognition/detection/rois6"/>
-      <arg name="input/camera_info7" value="$(var camera_info7)"/>
-      <arg name="input/rois7" value="/perception/object_recognition/detection/rois7"/>
-      <arg name="input/rois_number" value="$(var image_number)"/>
-      <arg name="input/clusters" value="lidar/short_range_objects"/>
-      <arg name="output" value="camera_lidar_fusion/short_range_objects"/>
-      <arg name="input/image0" value="$(var image_raw0)"/>
-      <arg name="input/image1" value="$(var image_raw1)"/>
-      <arg name="input/image2" value="$(var image_raw2)"/>
-      <arg name="input/image3" value="$(var image_raw3)"/>
-      <arg name="input/image4" value="$(var image_raw4)"/>
-      <arg name="input/image5" value="$(var image_raw5)"/>
-      <arg name="input/image6" value="$(var image_raw6)"/>
-      <arg name="input/image7" value="$(var image_raw7)"/>
-    </include>
-    <include file="$(find-pkg-share object_merger)/launch/object_association_merger.launch.xml">
-      <arg name="input/object0" value="camera_lidar_fusion/short_range_objects"/>
-      <arg name="input/object1" value="lidar/long_range_objects"/>
-      <arg name="output/object" value="camera_lidar_fusion/objects"/>
     </include>
   </group>
 
   <include file="$(find-pkg-share object_merger)/launch/object_association_merger.launch.xml">
-    <arg name="input/object0" value="apollo/camera_lidar_fusion/objects"/>
+    <arg name="input/object0" value="$(var lidar_detection_model)/objects"/>
     <arg name="input/object1" value="euclidean/camera_lidar_fusion/short_range_objects"/>
     <arg name="output/object" value="objects"/>
   </include>
-
 </launch>
diff --git a/perception_launch/launch/object_recognition/detection/detection.launch.xml b/perception_launch/launch/object_recognition/detection/detection.launch.xml
@@ -2,6 +2,7 @@
 
 <launch>
   <arg name="mode" default="camera_lidar_fusion" description="options: `camera_lidar_fusion`, `lidar` or `camera`"/>
+  <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`"/>
   <!-- "camera_lidar_fusion", "lidar" or "camera" -->
   <arg name="image_raw0" default="/image_raw" description="image raw topic name"/>
   <arg name="camera_info0" default="/camera_info" description="camera info topic name"/>
@@ -43,22 +44,24 @@
       <arg name="camera_info7" value="$(var camera_info7)"/>
       <arg name="image_number" value="$(var image_number)"/>
       <arg name="use_vector_map" value="$(var use_vector_map)"/>
+      <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
     </include>
   </group>
   <!-- lidar based detection-->
   <group if="$(eval '&quot;$(var mode)&quot;==&quot;lidar&quot;')">
     <include file="$(find-pkg-share perception_launch)/launch/object_recognition/detection/lidar_based_detection.launch.xml">
       <arg name="use_vector_map" value="$(var use_vector_map)"/>
+      <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
     </include>
   </group>
   <!-- camera based detection-->
   <group if="$(eval '&quot;$(var mode)&quot;==&quot;camera&quot;')">
   </group>
 
   <!-- visualization -->
-  <!-- <include file="$(find-pkg-share dynamic_object_visualization)/launch/dynamic_object_visualizer.launch.xml">
+  <include file="$(find-pkg-share dynamic_object_visualization)/launch/dynamic_object_visualizer.launch.xml">
     <arg name="input" value="objects"/>
     <arg name="with_feature" value="true"/>
     <arg name="only_known_objects" value="false"/>
-  </include> -->
+  </include>
 </launch>