feat(aip_launcher): initialize parameters for new concatenate node (#337

) * feat: initialize parameters for concatenate Signed-off-by: vividf <[email protected]> * chore: add xx1 gen2 parameters Signed-off-by: vividf <[email protected]> * chore: fix aip x1 parameter Signed-off-by: vividf <[email protected]> * chore: fix x2 parameter Signed-off-by: vividf <[email protected]> * chore: increase noise window value for xx1 Signed-off-by: vividf <[email protected]> * chore: update parameters Signed-off-by: vividf <[email protected]> * chore: add use_naive_approach Signed-off-by: vividf <[email protected]> * chore: add matching strategy parameter Signed-off-by: vividf <[email protected]> * ci(pre-commit): autofix --------- Signed-off-by: vividf <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
tier4 · Jan 21, 2025 · 01d06ab · 01d06ab
1 parent a03c37d
commit 01d06ab
Show file tree

Hide file tree

Showing 8 changed files with 187 additions and 79 deletions.
diff --git a/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_x1_launch/config/concatenate_and_time_sync_node.param.yaml
@@ -0,0 +1,22 @@
+/**:
+  ros__parameters:
+    debug_mode: false
+    has_static_tf_only: false
+    rosbag_length: 10.0
+    maximum_queue_size: 5
+    timeout_sec: 0.2
+    is_motion_compensated: true
+    publish_synchronized_pointcloud: true
+    keep_input_frame_in_synchronized_pointcloud: true
+    publish_previous_but_late_pointcloud: false
+    synchronized_pointcloud_postfix: pointcloud
+    input_twist_topic_type: twist
+    input_topics: [
+                    "/sensing/lidar/front_center/pointcloud_before_sync", # 0.025
+                    "/sensing/lidar/top/pointcloud_before_sync", # 0.055
+                ]
+    output_frame: base_link
+    matching_strategy:
+      type: advanced
+      lidar_timestamp_offsets: [0.0, 0.03]
+      lidar_timestamp_noise_window: [0.01, 0.01]
diff --git a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py
@@ -12,6 +12,9 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import os
+
+from ament_index_python.packages import get_package_share_directory
 import launch
 from launch.actions import DeclareLaunchArgument
 from launch.actions import OpaqueFunction
@@ -21,9 +24,17 @@
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import LoadComposableNodes
 from launch_ros.descriptions import ComposableNode
+from launch_ros.parameter_descriptions import ParameterFile
 
 
 def launch_setup(context, *args, **kwargs):
+    # concatenate node parameters
+    concatenate_and_time_sync_node_param = ParameterFile(
+        param_file=LaunchConfiguration("concatenate_and_time_sync_node_param_path").perform(
+            context
+        ),
+        allow_substs=True,
+    )
     # set concat filter as a component
     concat_component = ComposableNode(
         package="autoware_pointcloud_preprocessor",
@@ -33,22 +44,7 @@ def launch_setup(context, *args, **kwargs):
             ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"),
             ("output", "concatenated/pointcloud"),
         ],
-        parameters=[
-            {
-                "input_topics": [
-                    "/sensing/lidar/top/pointcloud_before_sync",
-                    "/sensing/lidar/front_center/pointcloud_before_sync",
-                ],
-                "output_frame": LaunchConfiguration("base_frame"),
-                "input_offset": [
-                    0.055,
-                    0.025,
-                ],
-                "timeout_sec": 0.095,
-                "input_twist_topic_type": "twist",
-                "publish_synchronized_pointcloud": True,
-            }
-        ],
+        parameters=[concatenate_and_time_sync_node_param],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
@@ -68,10 +64,19 @@ def generate_launch_description():
     def add_launch_arg(name: str, default_value=None):
         launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value))
 
-    add_launch_arg("base_frame", "base_link")
+    aip_x1_launch_share_dir = get_package_share_directory("aip_x1_launch")
+
     add_launch_arg("use_multithread", "False")
     add_launch_arg("use_intra_process", "False")
     add_launch_arg("pointcloud_container_name", "pointcloud_container")
+    add_launch_arg(
+        "concatenate_and_time_sync_node_param_path",
+        os.path.join(
+            aip_x1_launch_share_dir,
+            "config",
+            "concatenate_and_time_sync_node.param.yaml",
+        ),
+    )
 
     set_container_executable = SetLaunchConfiguration(
         "container_executable",

diff --git a/aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_x2_launch/config/concatenate_and_time_sync_node.param.yaml
@@ -0,0 +1,28 @@
+/**:
+  ros__parameters:
+    debug_mode: false
+    has_static_tf_only: false
+    rosbag_length: 10.0
+    maximum_queue_size: 5
+    timeout_sec: 0.2
+    is_motion_compensated: true
+    publish_synchronized_pointcloud: true
+    keep_input_frame_in_synchronized_pointcloud: true
+    publish_previous_but_late_pointcloud: false
+    synchronized_pointcloud_postfix: pointcloud
+    input_twist_topic_type: twist
+    input_topics: [
+                    "/sensing/lidar/rear_upper/pointcloud_before_sync", # 0.044
+                    "/sensing/lidar/rear_lower/pointcloud_before_sync", # 0.049
+                    "/sensing/lidar/left_upper/pointcloud_before_sync", # 0.05
+                    "/sensing/lidar/left_lower/pointcloud_before_sync", # 0.05
+                    "/sensing/lidar/front_upper/pointcloud_before_sync", # 0.075
+                    "/sensing/lidar/front_lower/pointcloud_before_sync", # 0.074
+                    "/sensing/lidar/right_upper/pointcloud_before_sync", # 0.090
+                    "/sensing/lidar/right_lower/pointcloud_before_sync", # 0.00
+                ]
+    output_frame: base_link
+    matching_strategy:
+      type: advanced
+      lidar_timestamp_offsets: [0.0, 0.005, 0.006, 0.006, 0.031, 0.03, 0.046, 0.056]
+      lidar_timestamp_noise_window: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01]
diff --git a/aip_x2_launch/launch/pointcloud_preprocessor.launch.py b/aip_x2_launch/launch/pointcloud_preprocessor.launch.py
@@ -12,7 +12,9 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import os
 
+from ament_index_python.packages import get_package_share_directory
 import launch
 from launch.actions import DeclareLaunchArgument
 from launch.actions import OpaqueFunction
@@ -22,9 +24,18 @@
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import LoadComposableNodes
 from launch_ros.descriptions import ComposableNode
+from launch_ros.parameter_descriptions import ParameterFile
 
 
 def launch_setup(context, *args, **kwargs):
+    # concatenate node parameters
+    concatenate_and_time_sync_node_param = ParameterFile(
+        param_file=LaunchConfiguration("concatenate_and_time_sync_node_param_path").perform(
+            context
+        ),
+        allow_substs=True,
+    )
+
     # set concat filter as a component
     concat_component = ComposableNode(
         package="autoware_pointcloud_preprocessor",
@@ -34,25 +45,7 @@ def launch_setup(context, *args, **kwargs):
             ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"),
             ("output", "concatenated/pointcloud"),
         ],
-        parameters=[
-            {
-                "input_topics": [
-                    "/sensing/lidar/front_upper/pointcloud_before_sync",
-                    "/sensing/lidar/front_lower/pointcloud_before_sync",
-                    "/sensing/lidar/left_upper/pointcloud_before_sync",
-                    "/sensing/lidar/left_lower/pointcloud_before_sync",
-                    "/sensing/lidar/right_upper/pointcloud_before_sync",
-                    "/sensing/lidar/right_lower/pointcloud_before_sync",
-                    "/sensing/lidar/rear_upper/pointcloud_before_sync",
-                    "/sensing/lidar/rear_lower/pointcloud_before_sync",
-                ],
-                "input_offset": [0.025, 0.025, 0.01, 0.0, 0.05, 0.05, 0.05, 0.05],
-                "timeout_sec": 0.075,
-                "output_frame": LaunchConfiguration("base_frame"),
-                "input_twist_topic_type": "twist",
-                "publish_synchronized_pointcloud": True,
-            }
-        ],
+        parameters=[concatenate_and_time_sync_node_param],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
@@ -72,11 +65,21 @@ def generate_launch_description():
     def add_launch_arg(name: str, default_value=None):
         launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value))
 
-    add_launch_arg("base_frame", "base_link")
+    aip_x2_launch_share_dir = get_package_share_directory("aip_x2_launch")
+
     add_launch_arg("use_multithread", "True")
     add_launch_arg("use_intra_process", "True")
     add_launch_arg("pointcloud_container_name", "pointcloud_container")
 
+    add_launch_arg(
+        "concatenate_and_time_sync_node_param_path",
+        os.path.join(
+            aip_x2_launch_share_dir,
+            "config",
+            "concatenate_and_time_sync_node.param.yaml",
+        ),
+    )
+
     set_container_executable = SetLaunchConfiguration(
         "container_executable",
         "component_container",

diff --git a/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_xx1_gen2_launch/config/concatenate_and_time_sync_node.param.yaml
@@ -0,0 +1,25 @@
+/**:
+  ros__parameters:
+    debug_mode: false
+    has_static_tf_only: false
+    rosbag_length: 10.0
+    maximum_queue_size: 5
+    timeout_sec: 0.2
+    is_motion_compensated: false # no need to compensate for motion as lidar scan at the same time.
+    publish_synchronized_pointcloud: true
+    keep_input_frame_in_synchronized_pointcloud: true
+    publish_previous_but_late_pointcloud: false
+    synchronized_pointcloud_postfix: pointcloud
+    input_twist_topic_type: twist
+    input_topics: [
+                    "/sensing/lidar/top/pointcloud_before_sync", # 0.99
+                    "/sensing/lidar/front_left/pointcloud_before_sync",  # 0.99
+                    "/sensing/lidar/front_right/pointcloud_before_sync", # 0.99
+                    "/sensing/lidar/side_left/pointcloud_before_sync",   # 0.99
+                    "/sensing/lidar/side_right/pointcloud_before_sync",  # 0.99
+                ]
+    output_frame: base_link
+    matching_strategy:
+      type: advanced
+      lidar_timestamp_offsets: [0.0, 0.0, 0.0, 0.0, 0.0]
+      lidar_timestamp_noise_window: [0.01, 0.01, 0.01, 0.01, 0.01]
diff --git a/aip_xx1_gen2_launch/launch/pointcloud_preprocessor.launch.py b/aip_xx1_gen2_launch/launch/pointcloud_preprocessor.launch.py
@@ -12,7 +12,9 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import os
 
+from ament_index_python.packages import get_package_share_directory
 import launch
 from launch.actions import DeclareLaunchArgument
 from launch.actions import OpaqueFunction
@@ -22,9 +24,18 @@
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import LoadComposableNodes
 from launch_ros.descriptions import ComposableNode
+from launch_ros.parameter_descriptions import ParameterFile
 
 
 def launch_setup(context, *args, **kwargs):
+    # concatenate node parameters
+    concatenate_and_time_sync_node_param = ParameterFile(
+        param_file=LaunchConfiguration("concatenate_and_time_sync_node_param_path").perform(
+            context
+        ),
+        allow_substs=True,
+    )
+
     # set concat filter as a component
     concat_component = ComposableNode(
         package="autoware_pointcloud_preprocessor",
@@ -34,20 +45,7 @@ def launch_setup(context, *args, **kwargs):
             ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"),
             ("output", "concatenated/pointcloud"),
         ],
-        parameters=[
-            {
-                "input_topics": LaunchConfiguration("input_topics"),
-                "output_frame": LaunchConfiguration("base_frame"),
-                "input_offset": LaunchConfiguration(
-                    "input_offset"
-                ),  # each sensor will wait 60, 70, 70, 70ms
-                "timeout_sec": LaunchConfiguration("timeout_sec"),  # set shorter than 100ms
-                "input_twist_topic_type": LaunchConfiguration("input_twist_topic_type"),
-                "publish_synchronized_pointcloud": LaunchConfiguration(
-                    "publish_synchronized_pointcloud"
-                ),
-            }
-        ],
+        parameters=[concatenate_and_time_sync_node_param],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
@@ -67,19 +65,21 @@ def generate_launch_description():
     def add_launch_arg(name: str, default_value=None):
         launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value))
 
-    add_launch_arg("base_frame", "base_link")
+    aip_xx1_gen2_launch_share_dir = get_package_share_directory("aip_xx1_gen2_launch")
+
     add_launch_arg("use_multithread", "False")
     add_launch_arg("use_intra_process", "False")
     add_launch_arg("pointcloud_container_name", "pointcloud_container")
     add_launch_arg("individual_container_name", "concatenate_container")
+
     add_launch_arg(
-        "input_topics",
-        "[/sensing/lidar/top/pointcloud, /sensing/lidar/left/pointcloud, /sensing/lidar/right/pointcloud, /sensing/lidar/rear/pointcloud]",
+        "concatenate_and_time_sync_node_param_path",
+        os.path.join(
+            aip_xx1_gen2_launch_share_dir,
+            "config",
+            "concatenate_and_time_sync_node.param.yaml",
+        ),
     )
-    add_launch_arg("input_offset", "[0.035, 0.025, 0.025, 0.025]")
-    add_launch_arg("timeout_sec", "0.095")
-    add_launch_arg("input_twist_topic_type", "twist")
-    add_launch_arg("publish_synchronized_pointcloud", "False")
 
     set_container_executable = SetLaunchConfiguration(
         "container_executable",

diff --git a/aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml b/aip_xx1_launch/config/concatenate_and_time_sync_node.param.yaml
@@ -0,0 +1,24 @@
+/**:
+  ros__parameters:
+    debug_mode: false
+    has_static_tf_only: false
+    rosbag_length: 10.0
+    maximum_queue_size: 5
+    timeout_sec: 0.2
+    is_motion_compensated: true
+    publish_synchronized_pointcloud: true
+    keep_input_frame_in_synchronized_pointcloud: true
+    publish_previous_but_late_pointcloud: false
+    synchronized_pointcloud_postfix: pointcloud
+    input_twist_topic_type: twist
+    input_topics: [
+                    "/sensing/lidar/top/pointcloud_before_sync",
+                    "/sensing/lidar/left/pointcloud_before_sync",
+                    "/sensing/lidar/right/pointcloud_before_sync",
+                    "/sensing/lidar/rear/pointcloud_before_sync",
+                ]
+    output_frame: base_link
+    matching_strategy:
+      type: advanced
+      lidar_timestamp_offsets: [0.0, 0.02, 0.02, 0.02]
+      lidar_timestamp_noise_window: [0.02, 0.02, 0.02, 0.02]