feat(aip_x2): update all sensor configuration (#140)

* feat: update gnss_link * fix: crop param * fix: LiDAR settings * feat: add ars408 radar launcher Signed-off-by: Tomohito Ando <[email protected]> * chore: add tf for radar Signed-off-by: Tomohito Ando <[email protected]> * chore: place the arg in the group to prevent affecting other launcher Signed-off-by: Tomohito Ando <[email protected]> * chore: change the IMU interface name to CAN1 Signed-off-by: Tomohito Ando <[email protected]> * fix: rename tlr camera * feat: remove camera launch * feat: hierarchized radar launch * fix: add multiple radar * chore: add comment * chore: change CAN port for IMU * feat: add radar * update lidar scan phase params Signed-off-by: Shunsuke Miura <[email protected]> * ci(pre-commit): autofix --------- Signed-off-by: Tomohito Ando <[email protected]> Signed-off-by: Shunsuke Miura <[email protected]> Co-authored-by: Tomohito Ando <[email protected]> Co-authored-by: Shunsuke Miura <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
tier4 · May 22, 2023 · ff3fe45 · ff3fe45
1 parent 51fdaa1
commit ff3fe45
Show file tree

Hide file tree

Showing 12 changed files with 257 additions and 83 deletions.
diff --git a/aip_x2_description/urdf/front_unit.xacro b/aip_x2_description/urdf/front_unit.xacro
@@ -4,6 +4,7 @@
     <xacro:include filename="$(find pandar_description)/urdf/pandar_40p.xacro"/>
     <xacro:include filename="$(find pandar_description)/urdf/pandar_qt.xacro"/>
     <xacro:include filename="$(find camera_description)/urdf/monocular_camera.xacro"/>
+    <xacro:include filename="$(find aip_x2_description)/urdf/radar.xacro"/>
 
     <xacro:arg name="gpu" default="false"/>
     <xacro:arg name="config_dir" default="$(find aip_x2_description)/config"/>
@@ -22,7 +23,7 @@
     <!-- sensor -->
     <xacro:property name="calibration" value="${xacro.load_yaml('$(arg config_dir)/front_unit_calibration.yaml')}"/>
 
-    <!-- BFS7(LR) -->
+    <!-- camera -->
     <xacro:monocular_camera_macro
       name="camera6/camera"
       parent="front_unit_base_link"
@@ -62,5 +63,39 @@
       yaw="${calibration['front_unit_base_link']['pandar_qt_front_base_link']['yaw']}"
     />
 
+    <!-- radar -->
+    <xacro:radar_macro
+      name="front_center/radar"
+      parent="front_unit_base_link"
+      x="${calibration['front_unit_base_link']['front_center/radar_link']['x']}"
+      y="${calibration['front_unit_base_link']['front_center/radar_link']['y']}"
+      z="${calibration['front_unit_base_link']['front_center/radar_link']['z']}"
+      roll="${calibration['front_unit_base_link']['front_center/radar_link']['roll']}"
+      pitch="${calibration['front_unit_base_link']['front_center/radar_link']['pitch']}"
+      yaw="${calibration['front_unit_base_link']['front_center/radar_link']['yaw']}"
+    />
+
+    <xacro:radar_macro
+      name="front_left/radar"
+      parent="front_unit_base_link"
+      x="${calibration['front_unit_base_link']['front_left/radar_link']['x']}"
+      y="${calibration['front_unit_base_link']['front_left/radar_link']['y']}"
+      z="${calibration['front_unit_base_link']['front_left/radar_link']['z']}"
+      roll="${calibration['front_unit_base_link']['front_left/radar_link']['roll']}"
+      pitch="${calibration['front_unit_base_link']['front_left/radar_link']['pitch']}"
+      yaw="${calibration['front_unit_base_link']['front_left/radar_link']['yaw']}"
+    />
+
+    <xacro:radar_macro
+      name="front_right/radar"
+      parent="front_unit_base_link"
+      x="${calibration['front_unit_base_link']['front_right/radar_link']['x']}"
+      y="${calibration['front_unit_base_link']['front_right/radar_link']['y']}"
+      z="${calibration['front_unit_base_link']['front_right/radar_link']['z']}"
+      roll="${calibration['front_unit_base_link']['front_right/radar_link']['roll']}"
+      pitch="${calibration['front_unit_base_link']['front_right/radar_link']['pitch']}"
+      yaw="${calibration['front_unit_base_link']['front_right/radar_link']['yaw']}"
+    />
+
   </xacro:macro>
 </robot>
diff --git a/aip_x2_description/urdf/radar.xacro b/aip_x2_description/urdf/radar.xacro
@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="radar_macro" params="name parent x y z roll pitch yaw">
+    <xacro:property name="mass" value="0.01" />
+
+    <joint name="${name}_base_mount_joint" type="fixed">
+      <origin rpy="${roll} ${pitch} ${yaw}" xyz="${x} ${y} ${z}"/>
+      <parent link="${parent}"/>
+      <child  link="${name}"/>
+    </joint>
+
+    <link name="${name}">
+      <visual>
+        <geometry>
+          <box size="0.03 0.1 0.1"/>
+        </geometry>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <material name="black">
+          <color rgba="0.1 0.1 0.1 1.0"/>
+        </material>
+      </visual>
+      <inertial>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <mass value="${mass}"/>
+        <inertia ixx="${(0.03*0.03+0.03*0.03)*mass/12.0}" ixy="0.0" ixz="0.0"
+                 iyy="${(0.1*0.1+0.1*0.1)*mass/12.0}" iyz="0.0"
+                 izz="${(0.1*0.1+0.1*0.1)*mass/12.0}"/>
+      </inertial>
+    </link>
+  </xacro:macro>
+</robot>
diff --git a/aip_x2_description/urdf/rear_unit.xacro b/aip_x2_description/urdf/rear_unit.xacro
@@ -4,8 +4,9 @@
     <xacro:include filename="$(find pandar_description)/urdf/pandar_40p.xacro"/>
     <xacro:include filename="$(find pandar_description)/urdf/pandar_qt.xacro"/>
     <xacro:include filename="$(find camera_description)/urdf/monocular_camera.xacro"/>
+    <xacro:include filename="$(find aip_x2_description)/urdf/radar.xacro"/>
 
-     <xacro:arg name="gpu" default="false"/>
+    <xacro:arg name="gpu" default="false"/>
     <xacro:arg name="config_dir" default="$(find aip_x2_description)/config"/>
 
     <xacro:property name="rear_unit_base_link" default="rear_unit_base_link"/>
@@ -61,6 +62,40 @@
       pitch="${calibration['rear_unit_base_link']['pandar_qt_rear_base_link']['pitch']}"
       yaw="${calibration['rear_unit_base_link']['pandar_qt_rear_base_link']['yaw']}"
     />
-  </xacro:macro>
 
+    <!-- radar -->
+    <xacro:radar_macro
+      name="rear_center/radar"
+      parent="rear_unit_base_link"
+      x="${calibration['rear_unit_base_link']['rear_center/radar_link']['x']}"
+      y="${calibration['rear_unit_base_link']['rear_center/radar_link']['y']}"
+      z="${calibration['rear_unit_base_link']['rear_center/radar_link']['z']}"
+      roll="${calibration['rear_unit_base_link']['rear_center/radar_link']['roll']}"
+      pitch="${calibration['rear_unit_base_link']['rear_center/radar_link']['pitch']}"
+      yaw="${calibration['rear_unit_base_link']['rear_center/radar_link']['yaw']}"
+    />
+
+    <xacro:radar_macro
+      name="rear_left/radar"
+      parent="rear_unit_base_link"
+      x="${calibration['rear_unit_base_link']['rear_left/radar_link']['x']}"
+      y="${calibration['rear_unit_base_link']['rear_left/radar_link']['y']}"
+      z="${calibration['rear_unit_base_link']['rear_left/radar_link']['z']}"
+      roll="${calibration['rear_unit_base_link']['rear_left/radar_link']['roll']}"
+      pitch="${calibration['rear_unit_base_link']['rear_left/radar_link']['pitch']}"
+      yaw="${calibration['rear_unit_base_link']['rear_left/radar_link']['yaw']}"
+    />
+
+    <xacro:radar_macro
+      name="rear_right/radar"
+      parent="rear_unit_base_link"
+      x="${calibration['rear_unit_base_link']['rear_right/radar_link']['x']}"
+      y="${calibration['rear_unit_base_link']['rear_right/radar_link']['y']}"
+      z="${calibration['rear_unit_base_link']['rear_right/radar_link']['z']}"
+      roll="${calibration['rear_unit_base_link']['rear_right/radar_link']['roll']}"
+      pitch="${calibration['rear_unit_base_link']['rear_right/radar_link']['pitch']}"
+      yaw="${calibration['rear_unit_base_link']['rear_right/radar_link']['yaw']}"
+    />
+
+  </xacro:macro>
 </robot>
diff --git a/aip_x2_description/urdf/sensors.xacro b/aip_x2_description/urdf/sensors.xacro
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <robot name="vehicle" xmlns:xacro="http://ros.org/wiki/xacro">
-
+  <xacro:include filename="$(find imu_description)/urdf/imu.xacro"/>
   <xacro:arg name="config_dir" default="$(find aip_x2_description)/config"/>
   <xacro:property name="calibration" value="${xacro.load_yaml('$(arg config_dir)/sensors_calibration.yaml')}"/>
 
@@ -40,4 +40,32 @@
     yaw="${calibration['base_link']['rear_unit_base_link']['yaw']}"
   />
 
+  <!-- gnss -->
+  <xacro:imu_macro
+    name="gnss"
+    parent="base_link"
+    namespace=""
+    x="${calibration['base_link']['gnss_link']['x']}"
+    y="${calibration['base_link']['gnss_link']['y']}"
+    z="${calibration['base_link']['gnss_link']['z']}"
+    roll="${calibration['base_link']['gnss_link']['roll']}"
+    pitch="${calibration['base_link']['gnss_link']['pitch']}"
+    yaw="${calibration['base_link']['gnss_link']['yaw']}"
+    fps="100"
+  />
+
+  <!-- radar -->
+  <xacro:imu_macro
+    name="ars408_front"
+    parent="base_link"
+    namespace=""
+    x="5.6221"
+    y="0"
+    z="0.7547"
+    roll="0"
+    pitch="0"
+    yaw="0"
+    fps="100"
+  />
+
 </robot>
diff --git a/aip_x2_description/urdf/top_unit.xacro b/aip_x2_description/urdf/top_unit.xacro
@@ -73,15 +73,15 @@
     <!-- camera -->
     <!-- TLR -->
     <xacro:monocular_camera_macro
-      name="traffic_light_camera/camera"
+      name="camera7/camera"
       parent="top_unit_base_link"
       namespace=""
-      x="${calibration['top_unit_base_link']['traffic_light_camera/camera_link']['x']}"
-      y="${calibration['top_unit_base_link']['traffic_light_camera/camera_link']['y']}"
-      z="${calibration['top_unit_base_link']['traffic_light_camera/camera_link']['z']}"
-      roll="${calibration['top_unit_base_link']['traffic_light_camera/camera_link']['roll']}"
-      pitch="${calibration['top_unit_base_link']['traffic_light_camera/camera_link']['pitch']}"
-      yaw="${calibration['top_unit_base_link']['traffic_light_camera/camera_link']['yaw']}"
+      x="${calibration['top_unit_base_link']['camera7/camera_link']['x']}"
+      y="${calibration['top_unit_base_link']['camera7/camera_link']['y']}"
+      z="${calibration['top_unit_base_link']['camera7/camera_link']['z']}"
+      roll="${calibration['top_unit_base_link']['camera7/camera_link']['roll']}"
+      pitch="${calibration['top_unit_base_link']['camera7/camera_link']['pitch']}"
+      yaw="${calibration['top_unit_base_link']['camera7/camera_link']['yaw']}"
       fps="30"
       width="800"
       height="400"
@@ -176,35 +176,6 @@
       fov="1.3"
     />
 
-    <!-- gnss -->
-    <!-- left -->
-    <xacro:imu_macro
-      name="gnss_right"
-      parent="top_unit_base_link"
-      namespace=""
-      x="${calibration['top_unit_base_link']['gnss_right_link']['x']}"
-      y="${calibration['top_unit_base_link']['gnss_right_link']['y']}"
-      z="${calibration['top_unit_base_link']['gnss_right_link']['z']}"
-      roll="${calibration['top_unit_base_link']['gnss_right_link']['roll']}"
-      pitch="${calibration['top_unit_base_link']['gnss_right_link']['pitch']}"
-      yaw="${calibration['top_unit_base_link']['gnss_right_link']['yaw']}"
-      fps="100"
-    />
-
-    <!-- right -->
-    <xacro:imu_macro
-      name="gnss_left"
-      parent="top_unit_base_link"
-      namespace=""
-      x="${calibration['top_unit_base_link']['gnss_left_link']['x']}"
-      y="${calibration['top_unit_base_link']['gnss_left_link']['y']}"
-      z="${calibration['top_unit_base_link']['gnss_left_link']['z']}"
-      roll="${calibration['top_unit_base_link']['gnss_left_link']['roll']}"
-      pitch="${calibration['top_unit_base_link']['gnss_left_link']['pitch']}"
-      yaw="${calibration['top_unit_base_link']['gnss_left_link']['yaw']}"
-      fps="100"
-    />
-
     <!-- imu -->
     <xacro:imu_macro
       name="tamagawa/imu"

diff --git a/aip_x2_launch/launch/ars408.launch.xml b/aip_x2_launch/launch/ars408.launch.xml
@@ -0,0 +1,34 @@
+<launch>
+  <arg name="launch_driver" default="true" />
+  <arg name="input/frame" default="from_can_bus" />
+  <arg name="output/objects" default="objects_raw" />
+  <arg name="output_frame" default="ars408_front_link" />
+  <arg name="interface" default="can0"/>
+  <arg name="receiver_interval_sec" default="0.01"/>
+
+  <group if="$(var launch_driver)">
+    <!-- socket can -->
+    <include file="$(find-pkg-share ros2_socketcan)/launch/socket_can_receiver.launch.py">
+      <arg name="interface" value="$(var interface)"/>
+      <arg name="interval_sec" value="$(var receiver_interval_sec)"/>
+    </include>
+
+    <!-- ars408 driver-->
+    <include file="$(find-pkg-share pe_ars408_ros)/launch/continental_ars408.launch.xml">
+      <arg name="input/frame" value="$(var input/frame)" />
+      <arg name="output/objects" value="$(var output/objects)" />
+      <arg name="output_frame" value="$(var output_frame)" />
+      <arg name="launch_driver" value="$(var launch_driver)" />
+    </include>
+  </group>
+
+  <!-- *** TEMPORARY ***
+    * Launching for visualization purposes.
+    * This should be launched from perception. ex) radar_based_detection.launch.xml
+  -->
+  <include file="$(find-pkg-share radar_tracks_msgs_converter)/launch/radar_tracks_msgs_converter.launch.xml">
+    <arg name="input/radar_objects" value="$(var output/objects)" />
+    <arg name="output/radar_detected_objects" value="debug/detected_objects"/>
+    <arg name="output/radar_tracked_objects" value="debug/tracked_objects"/>
+  </include>
+</launch>
diff --git a/aip_x2_launch/launch/camera.launch.xml b/aip_x2_launch/launch/camera.launch.xml
diff --git a/aip_x2_launch/launch/imu.launch.xml b/aip_x2_launch/launch/imu.launch.xml
@@ -1,11 +1,12 @@
 <launch>
-  <arg name="launch_driver" default="true" />
-  <arg name="interface" default="can1"/>
-  <arg name="receiver_interval_sec" default="0.01"/>
 
   <group>
     <push-ros-namespace namespace="imu"/>
 
+    <arg name="launch_driver" default="true" />
+    <arg name="interface" default="pcan1"/>
+    <arg name="receiver_interval_sec" default="0.01"/>
+
     <group>
       <push-ros-namespace namespace="tamagawa"/>
       <include file="$(find-pkg-share ros2_socketcan)/launch/socket_can_receiver.launch.py">

diff --git a/aip_x2_launch/launch/lidar.launch.xml b/aip_x2_launch/launch/lidar.launch.xml
@@ -40,8 +40,8 @@
         <arg name="device_ip" value="192.168.120.211" />
         <arg name="lidar_port" value="2331" />
         <arg name="gps_port" value="10131" />
-        <arg name="scan_phase" value="270.0" />
-        <arg name="angle_range" value="[90.0, 270.0]"/>
+        <arg name="scan_phase" value="300.0" />
+        <arg name="angle_range" value="[120.0, 300.0]"/>
         <arg name="distance_range" value="[0.1, 7.5]"/>
         <arg name="vertical_bins" value ="64" />
         <arg name="horizontal_ring_id" value="40" />
@@ -61,8 +61,8 @@
         <arg name="device_ip" value="192.168.110.202" />
         <arg name="lidar_port" value="2322" />
         <arg name="gps_port" value="10122" />
-        <arg name="scan_phase" value="357.0" />
-        <arg name="angle_range" value="[90.0, 357.0]"/>
+        <arg name="scan_phase" value="305.0" />
+        <arg name="angle_range" value="[90.0, 305.0]"/>
         <arg name="distance_range" value="[0.5, 200.0]"/>
         <arg name="vertical_bins" value ="40" />
         <arg name="horizontal_ring_id" value="12" />
@@ -85,7 +85,7 @@
         <arg name="lidar_port" value="2332" />
         <arg name="gps_port" value="10132" />
         <arg name="scan_phase" value="270.0" />
-        <arg name="angle_range" value="[90.0, 220.0]"/>
+        <arg name="angle_range" value="[90.0, 270.0]"/>
         <arg name="distance_range" value="[0.1, 7.5]"/>
         <arg name="vertical_bins" value ="64" />
         <arg name="horizontal_ring_id" value="54" />
@@ -106,7 +106,7 @@
         <arg name="lidar_port" value="2323" />
         <arg name="gps_port" value="10123" />
         <arg name="scan_phase" value="270.0" />
-        <arg name="angle_range" value="[3.0, 270.0]"/>
+        <arg name="angle_range" value="[55.0, 270.0]"/>
         <arg name="distance_range" value="[0.5, 200.0]"/>
         <arg name="vertical_bins" value ="40" />
         <arg name="horizontal_ring_id" value="12" />
@@ -129,7 +129,7 @@
         <arg name="lidar_port" value="2333" />
         <arg name="gps_port" value="10133" />
         <arg name="scan_phase" value="270.0" />
-        <arg name="angle_range" value="[140.0, 270.0]"/>
+        <arg name="angle_range" value="[90.0, 270.0]"/>
         <arg name="distance_range" value="[0.1, 7.5]"/>
         <arg name="vertical_bins" value ="64" />
         <arg name="horizontal_ring_id" value="54" />
@@ -172,8 +172,8 @@
         <arg name="device_ip" value="192.168.120.214" />
         <arg name="lidar_port" value="2334" />
         <arg name="gps_port" value="10134" />
-        <arg name="scan_phase" value="180.0" />
-        <arg name="angle_range" value="[90.0, 270.0]"/>
+        <arg name="scan_phase" value="200.0" />
+        <arg name="angle_range" value="[110.0, 290.0]"/>
         <arg name="distance_range" value="[0.1, 7.5]"/>
         <arg name="vertical_bins" value ="64" />
         <arg name="horizontal_ring_id" value="40" />

diff --git a/aip_x2_launch/launch/pandar_node_container.launch.py b/aip_x2_launch/launch/pandar_node_container.launch.py
@@ -134,8 +134,8 @@ def create_parameter_dict(*args):
     cropbox_parameters["negative"] = True
 
     vehicle_info = get_vehicle_info(context)
-    cropbox_parameters["min_x"] = vehicle_info["min_longitudinal_offset"] - 0.15
-    cropbox_parameters["max_x"] = vehicle_info["max_longitudinal_offset"] + 0.15
+    cropbox_parameters["min_x"] = vehicle_info["min_longitudinal_offset"]
+    cropbox_parameters["max_x"] = vehicle_info["max_longitudinal_offset"]
     cropbox_parameters["min_y"] = vehicle_info["min_lateral_offset"]
     cropbox_parameters["max_y"] = vehicle_info["max_lateral_offset"]
     cropbox_parameters["min_z"] = vehicle_info["min_height_offset"]