Merge pull request #510 from tier4/sync-awf-latest

chore: sync awf-latest

autoware_launch/launch/planning_simulator.launch.xml

@@ -63,6 +63,8 @@
     <include file="$(find-pkg-share control_launch)/launch/control.launch.xml">
       <!-- options for lateral_controller_mode: mpc_follower, pure_pursuit -->
       <arg name="lateral_controller_mode" value="mpc_follower" />
+      <!-- options for longitudinal_controller_mode: pid -->
+      <arg name="longitudinal_controller_mode" value="pid" />
     </include>
   </group>
 

control_launch/launch/control.launch.py

@@ -80,6 +80,7 @@ def launch_setup(context, *args, **kwargs):
             {
                 "ctrl_period": 0.03,
                 "lateral_controller_mode": LaunchConfiguration("lateral_controller_mode"),
+                "longitudinal_controller_mode": LaunchConfiguration("longitudinal_controller_mode"),
             },
             nearest_search_param,
             lon_controller_param,
@@ -262,6 +263,13 @@ def add_launch_arg(name: str, default_value=None, description=None):
         "lateral controller mode: `mpc_follower` or `pure_pursuit`",
     )
 
+    # longitudinal controller mode
+    add_launch_arg(
+        "longitudinal_controller_mode",
+        "pid",
+        "longitudinal controller mode: `pid`",
+    )
+
     # parameter file path
     add_launch_arg(
         "nearest_search_param_path",

localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml

@@ -1,28 +1,40 @@
 <?xml version="1.0"?>
 <launch>
-  <include file="$(find-pkg-share ekf_localizer)/launch/ekf_localizer.launch.xml">
-    <arg name="enable_yaw_bias_estimation" value="true"/>
-    <arg name="tf_rate" value="50.0"/>
-    <arg name="twist_smoothing_steps" value="2"/>
-    <arg name="input_initial_pose_name" value="/initialpose3d"/>
+  <group>
+    <include file="$(find-pkg-share ekf_localizer)/launch/ekf_localizer.launch.xml">
+      <arg name="enable_yaw_bias_estimation" value="true"/>
+      <arg name="tf_rate" value="50.0"/>
+      <arg name="twist_smoothing_steps" value="2"/>
+      <arg name="input_initial_pose_name" value="/initialpose3d"/>
+      <arg name="input_pose_with_cov_name" value="/localization/pose_estimator/pose_with_covariance"/>
+      <arg name="input_twist_with_cov_name" value="/localization/twist_estimator/twist_with_covariance"/>
+      <arg name="output_odom_name" value="kinematic_state"/>
+      <arg name="output_pose_name" value="pose"/>
+      <arg name="output_pose_with_covariance_name" value="/localization/pose_with_covariance"/>
+      <arg name="output_biased_pose_name" value="biased_pose"/>
+      <arg name="output_biased_pose_with_covariance_name" value="biased_pose_with_covariance"/>
+      <arg name="output_twist_name" value="twist"/>
+      <arg name="output_twist_with_covariance_name" value="twist_with_covariance"/>
+      <arg name="proc_stddev_vx_c" value="10.0"/>
+      <arg name="proc_stddev_wz_c" value="5.0"/>
+    </include>
+  </group>
 
-    <arg name="input_pose_with_cov_name" value="/localization/pose_estimator/pose_with_covariance"/>
-    <arg name="input_twist_with_cov_name" value="/localization/twist_estimator/twist_with_covariance"/>
-    <arg name="output_odom_name" value="kinematic_state"/>
-    <arg name="output_pose_name" value="pose"/>
-    <arg name="output_pose_with_covariance_name" value="/localization/pose_with_covariance"/>
-    <arg name="output_biased_pose_name" value="biased_pose"/>
-    <arg name="output_biased_pose_with_covariance_name" value="biased_pose_with_covariance"/>
-    <arg name="output_twist_name" value="twist"/>
-    <arg name="output_twist_with_covariance_name" value="twist_with_covariance"/>
+  <group>
+    <include file="$(find-pkg-share stop_filter)/launch/stop_filter.launch.xml">
+      <arg name="use_twist_with_covariance" value="True"/>
+      <arg name="input_odom_name" value="/localization/pose_twist_fusion_filter/kinematic_state"/>
+      <arg name="input_twist_with_covariance_name" value="/localization/pose_twist_fusion_filter/twist_with_covariance"/>
+      <arg name="output_odom_name" value="/localization/kinematic_state"/>
+    </include>
+  </group>
 
-    <arg name="proc_stddev_vx_c" value="10.0"/>
-    <arg name="proc_stddev_wz_c" value="5.0"/>
-  </include>
-  <include file="$(find-pkg-share stop_filter)/launch/stop_filter.launch.xml">
-    <arg name="use_twist_with_covariance" value="True"/>
-    <arg name="input_odom_name" value="/localization/pose_twist_fusion_filter/kinematic_state"/>
-    <arg name="input_twist_with_covariance_name" value="/localization/pose_twist_fusion_filter/twist_with_covariance"/>
-    <arg name="output_odom_name" value="/localization/kinematic_state"/>
-  </include>
+  <group>
+    <include file="$(find-pkg-share twist2accel)/launch/twist2accel.launch.xml">
+      <arg name="use_odom" value="true"/>
+      <arg name="in_odom" value="/localization/kinematic_state"/>
+      <arg name="in_twist" value="/localization/twist_estimator/twist_with_covariance"/>
+      <arg name="out_accel" value="/localization/acceleration"/>
+    </include>
+  </group>
 </launch>

perception_launch/config/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml

@@ -11,13 +11,21 @@
         max_x: 100.0
         min_y: -50.0
         max_y: 50.0
+        max_z: 10.7  # recommended 2.5 for non elevation_grid_mode
+        min_z: -8.7 # recommended 0.0 for non elevation_grid_mode
         negative: False
 
     common_ground_filter:
       plugin: "ground_segmentation::ScanGroundFilterComponent"
       parameters:
         global_slope_max_angle_deg: 10.0
-        local_slope_max_angle_deg: 30.0
+        local_slope_max_angle_deg: 13.0 # recommended 30.0 for non elevation_grid_mode
         split_points_distance_tolerance: 0.2
+        use_virtual_ground_point: True
         split_height_distance: 0.2
-        use_virtual_ground_point: False
+        non_ground_height_threshold: 0.20
+        grid_size_m: 0.1
+        grid_mode_switch_radius: 20.0
+        gnd_grid_buffer_size: 4
+        detection_range_z_max: 2.5
+        elevation_grid_mode: true

perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py

@@ -85,8 +85,6 @@ def create_additional_pipeline(self, lidar_name):
                     {
                         "input_frame": LaunchConfiguration("base_frame"),
                         "output_frame": LaunchConfiguration("base_frame"),
-                        "min_z": self.vehicle_info["min_height_offset"],
-                        "max_z": self.vehicle_info["max_height_offset"],
                     },
                     self.ground_segmentation_param[f"{lidar_name}_crop_box_filter"]["parameters"],
                 ],
@@ -219,8 +217,6 @@ def create_common_pipeline(self, input_topic, output_topic):
                     {
                         "input_frame": LaunchConfiguration("base_frame"),
                         "output_frame": LaunchConfiguration("base_frame"),
-                        "min_z": self.vehicle_info["min_height_offset"],
-                        "max_z": self.vehicle_info["max_height_offset"],
                     },
                     self.ground_segmentation_param["common_crop_box_filter"]["parameters"],
                 ],
@@ -239,7 +235,12 @@ def create_common_pipeline(self, input_topic, output_topic):
                     ("input", "range_cropped/pointcloud"),
                     ("output", output_topic),
                 ],
-                parameters=[self.ground_segmentation_param["common_ground_filter"]["parameters"]],
+                parameters=[
+                    self.ground_segmentation_param["common_ground_filter"]["parameters"],
+                    self.vehicle_info,
+                    {"input_frame": "base_link"},
+                    {"output_frame": "base_link"},
+                ],
                 extra_arguments=[
                     {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
                 ],
@@ -268,7 +269,7 @@ def create_single_frame_obstacle_segmentation_components(self, input_topic, outp
             components.append(
                 self.get_additional_lidars_concatenated_component(
                     input_topics=[common_pipeline_output]
-                    + [f"{x}/pointcloud" for x in additional_lidars],
+                    + list(map(lambda x: f"{x}/pointcloud"), additional_lidars),
                     output_topic=relay_topic if use_ransac else output_topic,
                 )
             )
@@ -476,6 +477,7 @@ def launch_setup(context, *args, **kwargs):
                 output_topic=relay_topic
                 if pipeline.use_time_series_filter
                 else pipeline.output_topic,
+                context=context,
             )
         )
     if pipeline.use_time_series_filter:
@@ -516,7 +518,7 @@ def add_launch_arg(name: str, default_value=None):
     add_launch_arg("use_intra_process", "True")
     add_launch_arg("use_pointcloud_container", "False")
     add_launch_arg("container_name", "perception_pipeline_container")
-    add_launch_arg("input/pointcloud", "sensing/lidar/concatenated/pointcloud")
+    add_launch_arg("input/pointcloud", "/sensing/lidar/concatenated/pointcloud")
 
     set_container_executable = SetLaunchConfiguration(
         "container_executable",

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

@@ -15,7 +15,13 @@
     drivable_area_margin: 6.0
 
     refine_goal_search_radius_range: 7.5
-    intersection_search_distance: 30.0
+
+    # parameters for turn signal decider
+    turn_signal_intersection_search_distance: 30.0
+    turn_signal_intersection_angle_threshold_deg: 15.0
+    turn_signal_minimum_search_distance: 10.0
+    turn_signal_search_time: 3.0
+    turn_signal_shift_length_threshold: 0.3
 
     path_interval: 2.0
 

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

@@ -16,9 +16,12 @@
       forward_goal_search_length: 20.0
       backward_goal_search_length: 20.0
       goal_search_interval: 2.0
-      goal_to_obstacle_margin: 3.0
+      longitudinal_margin: 3.0
+      max_lateral_offset: 1.0
+      lateral_offset_interval: 0.25
       # occupancy grid map
       use_occupancy_grid: true
+      use_occupancy_grid_for_longitudinal_margin: false
       occupancy_grid_collision_check_margin: 0.0
       theta_size: 360
       obstacle_threshold: 60

planning_launch/config/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml

@@ -1,7 +1,7 @@
 /**:
   ros__parameters:
     # -- Node Configurations --
-    planning_algorithm: "astar"
+    planning_algorithm: "astar" # options: astar, rrtstar
     waypoints_velocity: 5.0
     update_rate: 10.0
     th_arrived_distance_m: 1.0
@@ -15,10 +15,6 @@
     # -- Configurations common to the all planners --
     # base configs
     time_limit: 30000.0
-    # robot configs # TODO replace by vehicle_info
-    robot_length: 4.5
-    robot_width: 1.75
-    robot_base2back: 1.0
     minimum_turning_radius: 9.0
     maximum_turning_radius: 9.0
     turning_radius_size: 1