Added realsense gazebo urdf in d435, d455 and l515

pal-robotics-forks · Jan 22, 2024 · 75477ea · 75477ea
1 parent 3580b30
commit 75477ea
Show file tree

Hide file tree

Showing 4 changed files with 22 additions and 6 deletions.
diff --git a/realsense2_description/package.xml b/realsense2_description/package.xml
@@ -17,6 +17,7 @@
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>realsense2_camera_msgs</depend>
+  <depend>realsense_simulation</depend>
 
   <exec_depend>launch_ros</exec_depend>
   <exec_depend>xacro</exec_depend>

diff --git a/realsense2_description/urdf/_d435.urdf.xacro b/realsense2_description/urdf/_d435.urdf.xacro
@@ -25,8 +25,9 @@ aluminum peripherial evaluation case.
   <!-- Includes -->
   <xacro:include filename="$(find realsense2_description)/urdf/_materials.urdf.xacro" />
   <xacro:include filename="$(find realsense2_description)/urdf/_usb_plug.urdf.xacro" />
-
-  <xacro:macro name="sensor_d435" params="parent *origin name:=camera use_nominal_extrinsics:=false add_plug:=false use_mesh:=true">
+  <xacro:include filename="$(find realsense_description)/urdf/_d435.gazebo.xacro" />
+
+  <xacro:macro name="sensor_d435" params="parent *origin name:=camera use_nominal_extrinsics:=false add_plug:=false use_mesh:=true topics_ns:=camera publish_pointcloud:=true">
     <xacro:property name="M_PI" value="3.1415926535897931" />
 
     <!-- The following values are approximate, and the camera node
@@ -168,5 +169,9 @@ aluminum peripherial evaluation case.
         <origin xyz="${d435_cam_mount_from_center_offset - 0.02095} ${-d435_cam_depth_py - 0.0353} 0" rpy="0 0 0"/>
       </xacro:usb_plug>
     </xacro:if>
+
+    <!-- Realsense Gazebo Plugin -->
+    <xacro:gazebo_d435 camera_name="${name}" reference_link="${name}_link" topics_ns="${topics_ns}" depth_optical_frame="${name}_depth_optical_frame" color_optical_frame="${name}_color_optical_frame" infrared1_optical_frame="${name}_infra1_ir_optical_frame" infrared2_optical_frame="${name}_infra2_ir_optical_frame" publish_pointcloud="${publish_pointcloud}"/>
+
   </xacro:macro>
-</robot>
+</robot>
diff --git a/realsense2_description/urdf/_d455.urdf.xacro b/realsense2_description/urdf/_d455.urdf.xacro
@@ -25,8 +25,9 @@ aluminum peripherial evaluation case.
   <!-- Includes -->
   <xacro:include filename="$(find realsense2_description)/urdf/_materials.urdf.xacro" />
   <xacro:include filename="$(find realsense2_description)/urdf/_usb_plug.urdf.xacro" />
+  <xacro:include filename="$(find realsense_description)/urdf/_d455.gazebo.xacro" />
 
-  <xacro:macro name="sensor_d455" params="parent *origin name:=camera use_nominal_extrinsics:=false">
+  <xacro:macro name="sensor_d455" params="parent *origin name:=camera use_nominal_extrinsics:=false topics_ns:=camera publish_pointcloud:=true" >
     <xacro:arg name="add_plug" default="false" />
     <xacro:property name="M_PI" value="3.1415926535897931" />
 
@@ -206,5 +207,9 @@ aluminum peripherial evaluation case.
         <origin xyz="${-0.00406-d455_cam_depth_px} ${-0.03701-d455_cam_depth_py} ${-d455_cam_depth_pz}" rpy="${M_PI/2} 0 0"/>
       </xacro:usb_plug>
     </xacro:if>
+
+    <!-- Realsense Gazebo Plugin -->
+    <xacro:gazebo_d455 camera_name="${name}" reference_link="${name}_link" topics_ns="${topics_ns}" depth_optical_frame="${name}_depth_optical_frame" color_optical_frame="${name}_color_optical_frame" infrared1_optical_frame="${name}_infra1_ir_optical_frame" infrared2_optical_frame="${name}_infra2_ir_optical_frame" publish_pointcloud="${publish_pointcloud}"/>
+
   </xacro:macro>
 </robot>
diff --git a/realsense2_description/urdf/_l515.urdf.xacro b/realsense2_description/urdf/_l515.urdf.xacro
@@ -25,8 +25,9 @@ aluminum peripherial evaluation case.
   <!-- Includes -->
   <xacro:include filename="$(find realsense2_description)/urdf/_materials.urdf.xacro" />
   <xacro:include filename="$(find realsense2_description)/urdf/_usb_plug.urdf.xacro" /> <!-- this specific plug does not apply to L515 -->
-
-  <xacro:macro name="sensor_l515" params="parent *origin name:=camera use_nominal_extrinsics:=false add_plug:=false use_mesh:=true">
+  <xacro:include filename="$(find realsense_simulation)/urdf/_l515.gazebo.xacro" />
+
+  <xacro:macro name="sensor_l515" params="parent *origin name:=camera use_nominal_extrinsics:=false add_plug:=false use_mesh:=true topics_ns:=camera">
     <xacro:property name="M_PI" value="3.1415926535897931" />
 
     <!-- The following values are approximate, and the camera node
@@ -209,5 +210,9 @@ aluminum peripherial evaluation case.
         <origin xyz="${l515_cam_mount_from_glass_offset-0.0148} -0.018 0.018" rpy="${M_PI/4} 0 0 "/>
       </xacro:usb_plug>
     </xacro:if>
+
+    <!-- Realsense Gazebo Plugin -->
+    <xacro:gazebo_l515 camera_name="${name}" reference_link="${name}_link" topics_ns="${topics_ns}" depth_optical_frame="${name}_depth_optical_frame" color_optical_frame="${name}_color_optical_frame" infrared_optical_frame="${name}_infra_optical_frame"/>
+
   </xacro:macro>
 </robot>