feat: update for upstream

Signed-off-by: 1222-takeshi <[email protected]>
tier4 · Dec 13, 2023 · aa96913 · aa96913
1 parent bba52b7
commit aa96913
Show file tree

Hide file tree

Showing 4 changed files with 41 additions and 14 deletions.
diff --git a/aip_x1_1_launch/launch/imu.launch.xml b/aip_x1_1_launch/launch/imu.launch.xml
@@ -1,11 +1,12 @@
 <launch>
-  <arg name="launch_driver" default="true" />
-  <arg name="interface" default="canIMU"/>
-  <arg name="receiver_interval_sec" default="0.01"/>
-
   <group>
     <push-ros-namespace namespace="imu"/>
 
+    <arg name="launch_driver" default="true" />
+    <arg name="interface" default="canIMU"/>
+    <arg name="receiver_interval_sec" default="0.01"/>
+    <arg name ="vehicle_id" default="$(env VEHICLE_ID default)" />
+
     <group>
       <push-ros-namespace namespace="tamagawa"/>
       <include file="$(find-pkg-share ros2_socketcan)/launch/socket_can_receiver.launch.py">
@@ -19,10 +20,18 @@
       </node>
     </group>
 
+    <arg name="imu_raw_name" default="tamagawa/imu_raw"/>
+    <arg name="imu_corrector_param_file" default="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x1_1/imu_corrector.param.yaml"/>
     <include file="$(find-pkg-share imu_corrector)/launch/imu_corrector.launch.xml">
-      <arg name="input_topic" value="tamagawa/imu_raw" />
+      <arg name="input_topic" value="$(var imu_raw_name)"/>
       <arg name="output_topic" value="imu_data" />
-      <arg name="param_file" value="$(find-pkg-share individual_params)/config/$(env VEHICLE_ID default)/aip_x1_1/imu_corrector.param.yaml" />
+      <arg name="param_file" value="$(var imu_corrector_param_file)"/>
+    </include>
+
+    <include file="$(find-pkg-share imu_corrector)/launch/gyro_bias_estimator.launch.xml">
+      <arg name="input_imu_raw" value="$(var imu_raw_name)"/>
+      <arg name="input_odom" value="/localization/kinematic_state"/>
+      <arg name="imu_corrector_param_file" value="$(var imu_corrector_param_file)"/>
     </include>
   </group>
 

diff --git a/aip_x1_1_launch/launch/pandar_node_container.launch.py b/aip_x1_1_launch/launch/pandar_node_container.launch.py
@@ -39,7 +39,6 @@ def get_pandar_monitor_info():
         p = yaml.safe_load(f)["/**"]["ros__parameters"]
     return p
 
-
 def launch_setup(context, *args, **kwargs):
     def create_parameter_dict(*args):
         result = {}

diff --git a/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py b/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py
@@ -30,7 +30,10 @@ def launch_setup(context, *args, **kwargs):
         package="pointcloud_preprocessor",
         plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent",
         name="concatenate_data",
-        remappings=[("output", "concatenated/pointcloud")],
+        remappings=[
+            ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"),
+            ("output", "concatenated/pointcloud")
+        ],
         parameters=[
             {
                 "input_topics": [
@@ -39,6 +42,7 @@ def launch_setup(context, *args, **kwargs):
                 ],
                 "output_frame": LaunchConfiguration("base_frame"),
                 "timeout_sec": 1.0,
+                "input_twist_topic_type": "twist",
             }
         ],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],

diff --git a/aip_x1_1_launch/launch/velodyne_node_container.launch.py b/aip_x1_1_launch/launch/velodyne_node_container.launch.py
@@ -104,15 +104,30 @@ def create_parameter_dict(*args):
         )
     )
 
+    # nodes.append(
+    #     ComposableNode(
+    #         package="velodyne_pointcloud",
+    #         plugin="velodyne_pointcloud::Interpolate",
+    #         name="velodyne_interpolate_node",
+    #         remappings=[
+    #             ("velodyne_points_ex", "self_cropped/pointcloud_ex"),
+    #             ("velodyne_points_interpolate", "rectified/pointcloud"),
+    #             ("velodyne_points_interpolate_ex", "rectified/pointcloud_ex"),
+    #         ],
+    #         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+    #     )
+    # )
+
     nodes.append(
         ComposableNode(
-            package="velodyne_pointcloud",
-            plugin="velodyne_pointcloud::Interpolate",
-            name="velodyne_interpolate_node",
+            package="pointcloud_preprocessor",
+            plugin="pointcloud_preprocessor::DistortionCorrectorComponent",
+            name="distortion_corrector_node",
             remappings=[
-                ("velodyne_points_ex", "self_cropped/pointcloud_ex"),
-                ("velodyne_points_interpolate", "rectified/pointcloud"),
-                ("velodyne_points_interpolate_ex", "rectified/pointcloud_ex"),
+                ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"),
+                ("~/input/imu", "/sensing/imu/imu_data"),
+                ("~/input/pointcloud", "self_cropped/pointcloud_ex"),
+                ("~/output/pointcloud", "rectified/pointcloud_ex"),
             ],
             extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
         )