rgb_cam: update camera tf frame and urdf

westonrobot · Sep 10, 2024 · 47fe404 · 47fe404
1 parent bf4484f
commit 47fe404
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 4 deletions.
diff --git a/src/kits/top/perception_sensor_kit_bringup/config/rgb_camera.yaml b/src/kits/top/perception_sensor_kit_bringup/config/rgb_camera.yaml
@@ -3,7 +3,7 @@
       video_device: ""
       framerate: 25.0
       io_method: "mmap"
-      frame_id: "camera"
+      frame_id: "front_rgb_link"
       pixel_format: "mjpeg2rgb"  # see usb_cam/supported_formats for list of supported formats
       av_device_format: "YUV422P"
       image_width: 640
@@ -27,7 +27,7 @@
       video_device: ""
       framerate: 25.0
       io_method: "mmap"
-      frame_id: "camera"
+      frame_id: "rear_rgb_link"
       pixel_format: "mjpeg2rgb"  # see usb_cam/supported_formats for list of supported formats
       av_device_format: "YUV422P"
       image_width: 640
@@ -51,7 +51,7 @@
       video_device: "/dev/video4"
       framerate: 25.0
       io_method: "mmap"
-      frame_id: "camera"
+      frame_id: "left_rgb_link"
       pixel_format: "mjpeg2rgb"  # see usb_cam/supported_formats for list of supported formats
       av_device_format: "YUV422P"
       image_width: 640
@@ -75,7 +75,7 @@
       video_device: "/dev/video14"
       framerate: 25.0
       io_method: "mmap"
-      frame_id: "camera"
+      frame_id: "right_rgb_link"
       pixel_format: "mjpeg2rgb"  # see usb_cam/supported_formats for list of supported formats
       av_device_format: "YUV422P"
       image_width: 640

diff --git a/src/kits/top/perception_sensor_kit_bringup/urdf/sensor_kit_macro.urdf.xacro b/src/kits/top/perception_sensor_kit_bringup/urdf/sensor_kit_macro.urdf.xacro
@@ -46,6 +46,17 @@
         <origin xyz="-0.005 0.006 -0.0085" rpy="0 0 0"/>
       </xacro:sensor_d435>
     </xacro:if>
+
+    <xacro:if value="${type in ['rgb_camera']}">
+      <link name="${camera_pos}_rgb_link">
+      </link>
+
+      <joint name="${camera_pos}_rgb_joint" type="fixed">
+        <origin xyz="0.007 0 0.004" rpy="0 0 0" />
+        <parent link="${camera_pos}_camera_link" />
+        <child link="${camera_pos}_rgb_link" />
+      </joint>
+    </xacro:if>
   </xacro:macro>
 
 </robot>