feat: add GNSS

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

Hide file tree

Showing 4 changed files with 27 additions and 8 deletions.
diff --git a/aip_x1_1_description/urdf/sensor_kit.xacro b/aip_x1_1_description/urdf/sensor_kit.xacro
@@ -2,6 +2,7 @@
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
   <xacro:macro name="sensor_kit_macro" params="parent x y z roll pitch yaw">
     <xacro:include filename="$(find velodyne_description)/urdf/VLP-16.urdf.xacro"/>
+    <xacro:include filename="$(find imu_description)/urdf/imu.xacro"/>
 
     <xacro:arg name="gpu" default="false"/>
     <xacro:arg name="config_dir" default="$(find aip_x1_1_description)/config"/>
@@ -48,8 +49,20 @@
       pitch="${calibration['sensor_kit_base_link']['pandar_xt32_front_center_base_link']['pitch']}"
       yaw="${calibration['sensor_kit_base_link']['pandar_xt32_front_center_base_link']['yaw']}"
     />
-
-    <xacro:include filename="$(find imu_description)/urdf/imu.xacro"/>
+    <!-- gnss -->
+    <xacro:imu_macro
+      name="gnss"
+      parent="sensor_kit_base_link"
+      namespace=""
+      x="${calibration['sensor_kit_base_link']['gnss_link']['x']}"
+      y="${calibration['sensor_kit_base_link']['gnss_link']['y']}"
+      z="${calibration['sensor_kit_base_link']['gnss_link']['z']}"
+      roll="${calibration['sensor_kit_base_link']['gnss_link']['roll']}"
+      pitch="${calibration['sensor_kit_base_link']['gnss_link']['pitch']}"
+      yaw="${calibration['sensor_kit_base_link']['gnss_link']['yaw']}"
+      fps="100"
+    />
+    <!-- imu -->
     <xacro:imu_macro
       name="tamagawa/imu"
       parent="sensor_kit_base_link"

diff --git a/aip_x1_1_launch/launch/sensing.launch.xml b/aip_x1_1_launch/launch/sensing.launch.xml
@@ -4,29 +4,37 @@
   <arg name="vehicle_mirror_param_file" description="path to the file of vehicle mirror position yaml"/>
   <arg name="use_pointcloud_container" default="false" description="launch pointcloud container"/>
   <arg name="pointcloud_container_name" default="pointcloud_container"/>
+  <arg name ="vehicle_id" default="$(env VEHICLE_ID default)" />
+  <arg name="use_awsim" default="false"/>
+  <let name="imu_raw_name" value="tamagawa/imu_raw" if ="$(var use_awsim)"/>
+  <let name="imu_raw_name" value="/sensing/lidar/front_center/livox/imu" unless ="$(var use_awsim)"/>
 
   <group>
-
     <!-- LiDAR Driver -->
     <include file="$(find-pkg-share aip_x1_1_launch)/launch/lidar.launch.xml">
       <arg name="launch_driver" value="$(var launch_driver)" />
       <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)" />
       <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
+      <arg name="use_awsim" value="$(var use_awsim)"/>
     </include>
 
     <!-- IMU Driver -->
     <include file="$(find-pkg-share aip_x1_1_launch)/launch/imu.launch.xml">
       <arg name="launch_driver" value="$(var launch_driver)" />
     </include>
 
+    <!-- GNSS Driver -->
+    <include file="$(find-pkg-share aip_x1_launch)/launch/gnss.launch.xml" unless="$(var use_awsim)">
+      <arg name="launch_driver" value="$(var launch_driver)" />
+    </include>
+
     <!-- Vehicle twist -->
     <include file="$(find-pkg-share vehicle_velocity_converter)/launch/vehicle_velocity_converter.launch.xml">
       <arg name="input_vehicle_velocity_topic" value="/vehicle/status/velocity_status"/>
       <arg name="output_twist_with_covariance" value="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
-      <arg name="config_file" value="$(find-pkg-share individual_params)/config/$(env VEHICLE_ID default)/aip_x1_1/vehicle_velocity_converter.param.yaml" />
+      <arg name="config_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x1_1/vehicle_velocity_converter.param.yaml" />
     </include>
-
   </group>
 
 </launch>
diff --git a/aip_x1_launch/launch/sensing.launch.xml b/aip_x1_launch/launch/sensing.launch.xml
@@ -32,7 +32,7 @@
     </include>
 
     <!-- GNSS Driver -->
-    <include file="$(find-pkg-share aip_x1_launch)/launch/gnss.launch.xml">
+    <include file="$(find-pkg-share aip_x1_launch)/launch/gnss.launch.xml" unless="$(var use_awsim)">
       <arg name="launch_driver" value="$(var launch_driver)" />
     </include>
 

diff --git a/aip_x1_launch/launch/velodyne_node_container.launch.py b/aip_x1_launch/launch/velodyne_node_container.launch.py
@@ -25,8 +25,6 @@
 
 
 def get_vehicle_info(context):
-    # TODO(TIER IV): Use Parameter Substitution after we drop Galactic support
-    # https://github.com/ros2/launch_ros/blob/master/launch_ros/launch_ros/substitutions/parameter.py
     gp = context.launch_configurations.get("ros_params", {})
     if not gp:
         gp = dict(context.launch_configurations.get("global_params", {}))