From 75477eaf9b301c77509e37988a0836e240882c0f Mon Sep 17 00:00:00 2001
From: sergiomoyano <sergio.moyano@pal-robotics.com>
Date: Mon, 22 Jan 2024 12:38:16 +0100
Subject: [PATCH] Added realsense gazebo urdf in d435, d455 and l515

---
 realsense2_description/package.xml           |  1 +
 realsense2_description/urdf/_d435.urdf.xacro | 11 ++++++++---
 realsense2_description/urdf/_d455.urdf.xacro |  7 ++++++-
 realsense2_description/urdf/_l515.urdf.xacro |  9 +++++++--
 4 files changed, 22 insertions(+), 6 deletions(-)

diff --git a/realsense2_description/package.xml b/realsense2_description/package.xml
index fab5bf30bd..dbf718a3a2 100644
--- 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
index 722c6e3281..637781a6dc 100644
--- 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>
\ No newline at end of file
diff --git a/realsense2_description/urdf/_d455.urdf.xacro b/realsense2_description/urdf/_d455.urdf.xacro
index 8804ab880b..dafa9bf3d4 100644
--- 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
index c761ddf734..0f95bcfe25 100644
--- 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>