Merge pull request #27 from husarion/ros2-astra

Unify astra description with real camera
husarion · Jul 31, 2023 · 5f94c33 · 5f94c33
2 parents 6fd84c0 + 8b627aa
commit 5f94c33
Showing 1 changed file with 39 additions and 16 deletions.
diff --git a/urdf/orbbec_astra.urdf.xacro b/urdf/orbbec_astra.urdf.xacro
@@ -3,8 +3,7 @@
   <xacro:macro name="orbbec_astra"
                params="parent_link xyz rpy
                        tf_prefix:=None
-                       topic:=camera
-                       frame_id:=depth
+                       camera_name:=camera
                        simulation_engine:=gazebo-classic">
 
     <xacro:if value="${tf_prefix == 'None'}">
@@ -14,13 +13,13 @@
       <xacro:property name="tf_prefix_ext" value="${tf_prefix}_" />
     </xacro:unless>
 
-    <joint name="${parent_link.rstrip('_link')}_to_${tf_prefix_ext}orbbec_astra_joint" type="fixed">
+    <joint name="${parent_link.rstrip('_link')}_to_${tf_prefix_ext}${camera_name}_orbbec_astra_joint" type="fixed">
       <origin xyz="${xyz}" rpy="${rpy}" />
       <parent link="${parent_link}" />
-      <child link="${tf_prefix_ext}orbbec_astra_link" />
+      <child link="${tf_prefix_ext}${camera_name}_orbbec_astra_link" />
     </joint>
 
-    <link name="${tf_prefix_ext}orbbec_astra_link">
+    <link name="${tf_prefix_ext}${camera_name}_orbbec_astra_link">
       <visual>
         <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
         <geometry>
@@ -53,24 +52,48 @@
       </inertial>
     </link>
 
-    <joint name="${tf_prefix_ext}orbbec_astra_to_${tf_prefix_ext}${frame_id.rstrip('_link')}_joint" type="fixed">
-      <origin xyz="0.01 0.0 0.035" rpy="${-pi/2.0} 0.0 ${-pi/2.0}" />
-      <parent link="${tf_prefix_ext}orbbec_astra_link" />
-      <child link="${tf_prefix_ext}${frame_id}" />
+    <joint name="${parent_link.rstrip('_link')}_to_${tf_prefix_ext}${camera_name}_color_joint" type="fixed">
+      <origin xyz="-0.002 0.0125 0.03325" rpy="-0.003 0.0 -0.003" />
+      <parent link="${parent_link}" />
+      <child link="${tf_prefix_ext}${camera_name}_color_frame" />
+    </joint>
+
+    <link name="${tf_prefix_ext}${camera_name}_depth_frame" />
+
+    <joint name="${parent_link.rstrip('_link')}_to_${tf_prefix_ext}${camera_name}_depth_joint" type="fixed">
+      <origin xyz="0.0 0.0 0.03325" rpy="0.0 0.0 0.0" />
+      <parent link="${parent_link}" />
+      <child link="${tf_prefix_ext}${camera_name}_depth_frame" />
+    </joint>
+
+    <link name="${tf_prefix_ext}${camera_name}_color_frame" />
+
+    <joint name="${tf_prefix_ext}${camera_name}_color_to_${tf_prefix_ext}${camera_name}_color_optical_joint" type="fixed">
+      <origin xyz="0.0 0.0 0.0" rpy="${-pi/2} 0.0 ${-pi/2}" />
+      <parent link="${tf_prefix_ext}${camera_name}_color_frame" />
+      <child link="${tf_prefix_ext}${camera_name}_color_optical_frame" />
+    </joint>
+
+    <link name="${tf_prefix_ext}${camera_name}_depth_optical_frame" />
+
+    <joint name="${tf_prefix_ext}${camera_name}_depth_to_${tf_prefix_ext}${camera_name}_depth_optical_joint" type="fixed">
+      <origin xyz="0.0 0.0 0.0" rpy="${-pi/2} 0.0 ${-pi/2}" />
+      <parent link="${tf_prefix_ext}${camera_name}_depth_frame" />
+      <child link="${tf_prefix_ext}${camera_name}_depth_optical_frame" />
     </joint>
 
-    <link name="${tf_prefix_ext}${frame_id}" />
+    <link name="${tf_prefix_ext}${camera_name}_color_optical_frame" />
 
     <xacro:if value="${simulation_engine == 'ignition-gazebo'}">
-      <gazebo reference="${tf_prefix_ext}orbbec_astra_link">
-        <sensor type="rgbd_camera" name="${tf_prefix_ext}orbbec_astra_camera">
+      <gazebo reference="${tf_prefix_ext}${camera_name}_orbbec_astra_link">
+        <sensor type="rgbd_camera" name="${tf_prefix_ext}${camera_name}_orbbec_astra_camera">
           <always_on>true</always_on>
           <update_rate>20.0</update_rate>
 
-          <topic>${topic}</topic>
-          <!-- <frame_id>${tf_prefix_ext}${frame_id}</frame_id> -->
-          <optical_frame_id>${tf_prefix_ext}${frame_id}</optical_frame_id>
-          <ignition_frame_id>${tf_prefix_ext}${frame_id}</ignition_frame_id>
+          <topic>${camera_name}</topic>
+          <!-- It is not possible to split the frame of the color and the depth stream.
+               The frame of color stream should be ${tf_prefix_ext}${camera_name}_color_optical_frame -->
+          <ignition_frame_id>${tf_prefix_ext}${camera_name}_depth_optical_frame</ignition_frame_id>
 
           <camera name="${tf_prefix_ext}camera">
             <horizontal_fov>${60.0/180.0*pi}</horizontal_fov>