From f282e89dcaad1ae7c0e2ffcfe8120ff225c1e32d Mon Sep 17 00:00:00 2001
From: Vatan Aksoy Tezer <vatan@picknik.ai>
Date: Fri, 24 Sep 2021 17:22:05 +0300
Subject: [PATCH 1/2] Make realsense parametric

---
 stretch_gazebo/launch/gazebo.launch           |   4 +-
 stretch_gazebo/urdf/stretch_gazebo.urdf.xacro | 251 +++++++++---------
 2 files changed, 129 insertions(+), 126 deletions(-)

diff --git a/stretch_gazebo/launch/gazebo.launch b/stretch_gazebo/launch/gazebo.launch
index 6b75bcd5..cbb78fe6 100644
--- a/stretch_gazebo/launch/gazebo.launch
+++ b/stretch_gazebo/launch/gazebo.launch
@@ -9,7 +9,7 @@
   <arg name="model" default="$(find stretch_gazebo)/urdf/stretch_gazebo.urdf.xacro"/>
   <arg name="gpu_lidar" default="false"/>
   <arg name="visualize_lidar" default="false"/>
-  <arg name="world" default="worlds/empty.world"/>
+  <arg name="realsense" default="false"/>
 
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
     <arg name="world_name" value="$(arg world)" />
@@ -21,7 +21,7 @@
     <arg name="verbose" value="true"/>
   </include>
 
-  <param name="robot_description" command="$(find xacro)/xacro $(arg model) gpu_lidar:=$(arg gpu_lidar) visualize_lidar:=$(arg visualize_lidar)" />
+  <param name="robot_description" command="$(find xacro)/xacro $(arg model) gpu_lidar:=$(arg gpu_lidar) visualize_lidar:=$(arg visualize_lidar) realsense:=$(arg realsense)" />
 
   <!-- push robot_description to factory and spawn robot in gazebo -->
   <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model"
diff --git a/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro b/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro
index 1e516a35..4e6167c5 100644
--- a/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro
+++ b/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro
@@ -2,6 +2,7 @@
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_description">
   <xacro:arg name="gpu_lidar" default="false" />
   <xacro:arg name="visualize_lidar" default="false" />
+  <xacro:arg name="realsense" default="false" />
 
   <xacro:include filename="$(find stretch_description)/urdf/stretch_main.xacro" />
   <xacro:include filename="$(find stretch_description)/urdf/stretch_aruco.xacro" />
@@ -70,130 +71,132 @@
   <!-- Sensors -->
 
   <!-- Realsense D435i -->
-  <gazebo reference="camera_color_frame">
-    <sensor name="color" type="camera">
-      <pose frame="">0 0 0 0 0 0</pose>
-      <camera name="__default__">
-        <horizontal_fov>1.5009831567151233</horizontal_fov>
-        <image>
-          <width>640</width>
-          <height>480</height>
-          <format>RGB_INT8</format>
-        </image>
-        <clip>
-          <near>0.1</near>
-          <far>100</far>
-        </clip>
-        <noise>
-          <type>gaussian</type>
-          <mean>0.0</mean>
-          <stddev>0.007</stddev>
-        </noise>
-      </camera>
-      <always_on>1</always_on>
-      <update_rate>30</update_rate>
-      <visualize>0</visualize>
-    </sensor>
-  </gazebo>
-  <gazebo reference="camera_infra1_frame">
-    <sensor name="ired1" type="camera">
-      <pose frame="">0 0 0 0 0 0</pose>
-      <camera name="__default__">
-        <horizontal_fov>1.5009831567151233</horizontal_fov>
-        <image>
-          <width>640</width>
-          <height>480</height>
-          <format>L_INT8</format>
-        </image>
-        <clip>
-          <near>0.1</near>
-          <far>100</far>
-        </clip>
-        <noise>
-          <type>gaussian</type>
-          <mean>0.0</mean>
-          <stddev>0.007</stddev>
-        </noise>
-      </camera>
-      <always_on>1</always_on>
-      <update_rate>30</update_rate>
-      <visualize>0</visualize>
-    </sensor>
-  </gazebo>
-  <gazebo reference="camera_infra2_frame">
-    <sensor name="ired2" type="camera">
-      <pose frame="">0 0 0 0 0 0</pose>
-      <camera name="__default__">
-        <horizontal_fov>1.5009831567151233</horizontal_fov>
-        <image>
-          <width>640</width>
-          <height>480</height>
-          <format>L_INT8</format>
-        </image>
-        <clip>
-          <near>0.1</near>
-          <far>100</far>
-        </clip>
-        <noise>
-          <type>gaussian</type>
-          <mean>0.0</mean>
-          <stddev>0.007</stddev>
-        </noise>
-      </camera>
-      <always_on>1</always_on>
-      <update_rate>30</update_rate>
-      <visualize>0</visualize>
-    </sensor>
-  </gazebo>
-  <gazebo reference="camera_depth_frame">
-    <sensor name="depth" type="depth">
-      <pose frame="">0 0 0 0 0 0</pose>
-      <camera name="__default__">
-        <horizontal_fov>1.5009831567151233</horizontal_fov>
-        <image>
-          <width>640</width>
-          <height>480</height>
-        </image>
-        <clip>
-          <near>0.1</near>
-          <far>100</far>
-        </clip>
-        <noise>
-          <type>gaussian</type>
-          <mean>0.0</mean>
-          <stddev>0.007</stddev>
-        </noise>
-      </camera>
-      <always_on>1</always_on>
-      <update_rate>30</update_rate>
-      <visualize>0</visualize>
-    </sensor>
-  </gazebo>
-  <gazebo>
-      <plugin name="camera" filename="librealsense_gazebo_plugin.so">
-        <depthUpdateRate>30</depthUpdateRate>
-        <colorUpdateRate>30</colorUpdateRate>
-        <infraredUpdateRate>30</infraredUpdateRate>
-        <depthTopicName>depth/image_raw</depthTopicName>
-        <depthCameraInfoTopicName>depth/camera_info</depthCameraInfoTopicName>
-        <colorTopicName>color/image_raw</colorTopicName>
-        <colorCameraInfoTopicName>color/camera_info</colorCameraInfoTopicName>
-        <infrared1TopicName>infrared/image_raw</infrared1TopicName>
-        <infrared1CameraInfoTopicName>infrared/camera_info</infrared1CameraInfoTopicName>
-        <infrared2TopicName>infrared2/image_raw</infrared2TopicName>
-        <infrared2CameraInfoTopicName>infrared2/camera_info</infrared2CameraInfoTopicName>
-        <colorOpticalframeName>camera_color_optical_frame</colorOpticalframeName>
-        <depthOpticalframeName>camera_depth_optical_frame</depthOpticalframeName>
-        <infrared1OpticalframeName>camera_left_ir_optical_frame</infrared1OpticalframeName>
-        <infrared2OpticalframeName>camera_right_ir_optical_frame</infrared2OpticalframeName>
-        <rangeMinDepth>0.1</rangeMinDepth>
-        <rangeMaxDepth>10</rangeMaxDepth>
-        <pointCloud>1</pointCloud>
-        <pointCloudTopicName>depth/color/points</pointCloudTopicName>
-        <pointCloudCutoff>0.15</pointCloudCutoff>
-        <pointCloudCutoffMax>10</pointCloudCutoffMax>
-      </plugin>
-  </gazebo>
+  <xacro:if value="$(arg realsense)">
+    <gazebo reference="camera_color_frame">
+      <sensor name="color" type="camera">
+        <pose frame="">0 0 0 0 0 0</pose>
+        <camera name="__default__">
+          <horizontal_fov>1.5009831567151233</horizontal_fov>
+          <image>
+            <width>640</width>
+            <height>480</height>
+            <format>RGB_INT8</format>
+          </image>
+          <clip>
+            <near>0.1</near>
+            <far>100</far>
+          </clip>
+          <noise>
+            <type>gaussian</type>
+            <mean>0.0</mean>
+            <stddev>0.007</stddev>
+          </noise>
+        </camera>
+        <always_on>1</always_on>
+        <update_rate>30</update_rate>
+        <visualize>0</visualize>
+      </sensor>
+    </gazebo>
+    <gazebo reference="camera_infra1_frame">
+      <sensor name="ired1" type="camera">
+        <pose frame="">0 0 0 0 0 0</pose>
+        <camera name="__default__">
+          <horizontal_fov>1.5009831567151233</horizontal_fov>
+          <image>
+            <width>640</width>
+            <height>480</height>
+            <format>L_INT8</format>
+          </image>
+          <clip>
+            <near>0.1</near>
+            <far>100</far>
+          </clip>
+          <noise>
+            <type>gaussian</type>
+            <mean>0.0</mean>
+            <stddev>0.007</stddev>
+          </noise>
+        </camera>
+        <always_on>1</always_on>
+        <update_rate>30</update_rate>
+        <visualize>0</visualize>
+      </sensor>
+    </gazebo>
+    <gazebo reference="camera_infra2_frame">
+      <sensor name="ired2" type="camera">
+        <pose frame="">0 0 0 0 0 0</pose>
+        <camera name="__default__">
+          <horizontal_fov>1.5009831567151233</horizontal_fov>
+          <image>
+            <width>640</width>
+            <height>480</height>
+            <format>L_INT8</format>
+          </image>
+          <clip>
+            <near>0.1</near>
+            <far>100</far>
+          </clip>
+          <noise>
+            <type>gaussian</type>
+            <mean>0.0</mean>
+            <stddev>0.007</stddev>
+          </noise>
+        </camera>
+        <always_on>1</always_on>
+        <update_rate>30</update_rate>
+        <visualize>0</visualize>
+      </sensor>
+    </gazebo>
+    <gazebo reference="camera_depth_frame">
+      <sensor name="depth" type="depth">
+        <pose frame="">0 0 0 0 0 0</pose>
+        <camera name="__default__">
+          <horizontal_fov>1.5009831567151233</horizontal_fov>
+          <image>
+            <width>640</width>
+            <height>480</height>
+          </image>
+          <clip>
+            <near>0.1</near>
+            <far>100</far>
+          </clip>
+          <noise>
+            <type>gaussian</type>
+            <mean>0.0</mean>
+            <stddev>0.007</stddev>
+          </noise>
+        </camera>
+        <always_on>1</always_on>
+        <update_rate>30</update_rate>
+        <visualize>0</visualize>
+      </sensor>
+    </gazebo>
+    <gazebo>
+        <plugin name="camera" filename="librealsense_gazebo_plugin.so">
+          <depthUpdateRate>30</depthUpdateRate>
+          <colorUpdateRate>30</colorUpdateRate>
+          <infraredUpdateRate>30</infraredUpdateRate>
+          <depthTopicName>depth/image_raw</depthTopicName>
+          <depthCameraInfoTopicName>depth/camera_info</depthCameraInfoTopicName>
+          <colorTopicName>color/image_raw</colorTopicName>
+          <colorCameraInfoTopicName>color/camera_info</colorCameraInfoTopicName>
+          <infrared1TopicName>infrared/image_raw</infrared1TopicName>
+          <infrared1CameraInfoTopicName>infrared/camera_info</infrared1CameraInfoTopicName>
+          <infrared2TopicName>infrared2/image_raw</infrared2TopicName>
+          <infrared2CameraInfoTopicName>infrared2/camera_info</infrared2CameraInfoTopicName>
+          <colorOpticalframeName>camera_color_optical_frame</colorOpticalframeName>
+          <depthOpticalframeName>camera_depth_optical_frame</depthOpticalframeName>
+          <infrared1OpticalframeName>camera_left_ir_optical_frame</infrared1OpticalframeName>
+          <infrared2OpticalframeName>camera_right_ir_optical_frame</infrared2OpticalframeName>
+          <rangeMinDepth>0.1</rangeMinDepth>
+          <rangeMaxDepth>10</rangeMaxDepth>
+          <pointCloud>1</pointCloud>
+          <pointCloudTopicName>depth/color/points</pointCloudTopicName>
+          <pointCloudCutoff>0.15</pointCloudCutoff>
+          <pointCloudCutoffMax>10</pointCloudCutoffMax>
+        </plugin>
+    </gazebo>
+  </xacro:if>
 
   <gazebo reference="camera_gyro_frame">
     <gravity>true</gravity>

From b428b06eeac8640bc8bcb99a0ddc6268a2205faa Mon Sep 17 00:00:00 2001
From: Vatan Aksoy Tezer <vatan@picknik.ai>
Date: Fri, 24 Sep 2021 17:34:53 +0300
Subject: [PATCH 2/2] Correct topic name in rviz config

---
 stretch_gazebo/config/sim.rviz | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/stretch_gazebo/config/sim.rviz b/stretch_gazebo/config/sim.rviz
index e23aa3dc..975d3520 100644
--- a/stretch_gazebo/config/sim.rviz
+++ b/stretch_gazebo/config/sim.rviz
@@ -318,7 +318,7 @@ Visualization Manager:
       Size (Pixels): 3
       Size (m): 0.009999999776482582
       Style: Flat Squares
-      Topic: /realsense/depth/color/points
+      Topic: /camera/depth/color/points
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true