add optical frames for all rgb and rgbd cameras (#350)

usnistgov · May 23, 2024 · 6c0307b · 6c0307b
1 parent fce8690
commit 6c0307b
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 0 deletions.
diff --git a/ariac_description/urdf/vacuum_gripper/vacuum_gripper_macro.xacro b/ariac_description/urdf/vacuum_gripper/vacuum_gripper_macro.xacro
@@ -107,6 +107,9 @@
         </visual>
       </link>
 
+      <link name ="${prefix}_robot_camera_optical_link">
+      </link>
+
       <!-- Add camera mount joint -->
       <joint name="${prefix}_robot_camera_mount_joint" type="fixed">
         <axis xyz="0 1 0" />
@@ -123,6 +126,14 @@
         <child link="${prefix}_robot_camera_link"/>
       </joint>
 
+      <!-- Add optical joint -->
+      <joint name="${prefix}_robot_camera_optical_joint" type="fixed">
+        <axis xyz="0 1 0" />
+        <origin xyz="0 0 0" rpy="-1.57 0 -1.57"/>
+        <parent link="${prefix}_robot_camera_link"/>
+        <child link="${prefix}_robot_camera_optical_link"/>
+      </joint>
+
       <!-- Add color to links -->
       <gazebo reference="${prefix}_robot_camera_mount_link">
         <material>Gazebo/Black</material>

diff --git a/ariac_gazebo/nodes/sensor_tf_broadcaster.py b/ariac_gazebo/nodes/sensor_tf_broadcaster.py
@@ -40,6 +40,20 @@ def main():
 
         sensor_tf_broadcaster.generate_transform('world', sensor_name + "_frame", pose)
 
+        # Publish optical frame for cameras
+        try:
+            if 'rgb' in sensors[sensor_name]['type']:
+                xyz = [0, 0, 0]
+                rpy = ['-pi/2', 0, '-pi/2']
+
+                optical_pose = pose_info(xyz, rpy)
+
+                sensor_tf_broadcaster.generate_transform(
+                    sensor_name + "_frame", sensor_name + "_optical_frame", optical_pose)
+
+        except KeyError:
+            pass
+
     # Send tf transforms
     sensor_tf_broadcaster.send_transforms()