From c336d4f7eadd7738fd8c401c69d82b632425664b Mon Sep 17 00:00:00 2001
From: hello-binit <64861565+hello-binit@users.noreply.github.com>
Date: Sat, 31 Jul 2021 00:59:09 -0400
Subject: [PATCH] Support non-GPU lidar and swappable worlds in Stretch Gazebo

---
 stretch_gazebo/launch/gazebo.launch           |   5 +-
 stretch_gazebo/urdf/stretch_gazebo.urdf.xacro | 106 ++++++++++++------
 2 files changed, 77 insertions(+), 34 deletions(-)

diff --git a/stretch_gazebo/launch/gazebo.launch b/stretch_gazebo/launch/gazebo.launch
index ddf6a02..2239c08 100644
--- a/stretch_gazebo/launch/gazebo.launch
+++ b/stretch_gazebo/launch/gazebo.launch
@@ -7,8 +7,11 @@
   <arg name="debug" default="false"/>
   <arg name="rviz" default="false"/>
   <arg name="model" default="$(find stretch_gazebo)/urdf/stretch_gazebo.urdf.xacro"/>
+  <arg name="gpu_lidar" default="false"/>
+  <arg name="world" default="worlds/empty.world"/>
 
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(arg world)" />
     <arg name="debug" value="$(arg debug)" />
     <arg name="gui" value="$(arg gui)" />
     <arg name="paused" value="$(arg paused)"/>
@@ -17,7 +20,7 @@
     <arg name="verbose" value="true"/>
   </include>
 
-  <param name="robot_description" command="$(find xacro)/xacro $(arg model)" />
+  <param name="robot_description" command="$(find xacro)/xacro $(arg model) gpu_lidar:=$(arg gpu_lidar)" />
 
   <!-- 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 a939a88..fcec6ee 100644
--- a/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro
+++ b/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro
@@ -1,5 +1,7 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch_description">
+  <xacro:arg name="gpu_lidar" default="false" />
+
   <xacro:include filename="$(find stretch_description)/urdf/stretch_main.xacro" />
   <xacro:include filename="$(find stretch_description)/urdf/stretch_aruco.xacro" />
   <xacro:include filename="$(find stretch_description)/urdf/stretch_d435i.xacro" />
@@ -237,39 +239,77 @@
       <material>Gazebo/Green</material>
   </gazebo>
 
-  <!-- LIDAR -->
-  <gazebo reference="laser">
-    <material>Gazebo/Black</material>
-    <sensor type="gpu_ray" name="laser_sensor">
-      <pose>0 0 0 0 0 0</pose>
-      <visualize>false</visualize>
-      <update_rate>5.5</update_rate>
-      <ray>
-        <scan>
-          <horizontal>
-            <samples>2000</samples>
-            <resolution>1</resolution>
-            <min_angle>${-M_PI}</min_angle>
-            <max_angle>${M_PI}</max_angle>
-          </horizontal>
-        </scan>
-        <range>
-          <min>0.08</min>
-          <max>12.0</max>
-          <resolution>0.01</resolution>
-        </range>
-        <noise>
-          <type>gaussian</type>
-          <mean>0.0</mean>
-          <stddev>0.001</stddev>
-        </noise>
-      </ray>
-      <plugin name="gazebo_ros_lidar_controller" filename="libgazebo_ros_gpu_laser.so">
-        <topicName>scan</topicName>
-        <frameName>laser</frameName>
-      </plugin>
-    </sensor>
-  </gazebo>
+  <!-- Non GPU LIDAR -->
+  <xacro:unless value="$(arg gpu_lidar)">
+    <gazebo reference="laser">
+      <material>Gazebo/Black</material>
+      <sensor type="ray" name="laser_sensor">
+        <pose>0 0 0 0 0 0</pose>
+        <visualize>true</visualize>
+        <update_rate>5.5</update_rate>
+        <ray>
+          <scan>
+            <horizontal>
+              <samples>2000</samples>
+              <resolution>1</resolution>
+              <min_angle>${-M_PI}</min_angle>
+              <max_angle>${M_PI}</max_angle>
+            </horizontal>
+          </scan>
+          <range>
+            <min>0.08</min>
+            <max>12.0</max>
+            <resolution>0.01</resolution>
+          </range>
+          <noise>
+            <type>gaussian</type>
+            <mean>0.0</mean>
+            <stddev>0.001</stddev>
+          </noise>
+        </ray>
+        <plugin name="gazebo_ros_lidar_controller" filename="libgazebo_ros_laser.so">
+          <topicName>scan</topicName>
+          <frameName>laser</frameName>
+        </plugin>
+      </sensor>
+    </gazebo>
+  </xacro:unless>
+
+  <!-- GPU LIDAR -->
+  <xacro:if value="$(arg gpu_lidar)">
+    <gazebo reference="laser">
+      <material>Gazebo/Black</material>
+      <sensor type="gpu_ray" name="laser_sensor">
+        <pose>0 0 0 0 0 0</pose>
+        <visualize>true</visualize>
+        <update_rate>5.5</update_rate>
+        <ray>
+          <scan>
+            <horizontal>
+              <samples>2000</samples>
+              <resolution>1</resolution>
+              <min_angle>${-M_PI}</min_angle>
+              <max_angle>${M_PI}</max_angle>
+            </horizontal>
+          </scan>
+          <range>
+            <min>0.08</min>
+            <max>12.0</max>
+            <resolution>0.01</resolution>
+          </range>
+          <noise>
+            <type>gaussian</type>
+            <mean>0.0</mean>
+            <stddev>0.001</stddev>
+          </noise>
+        </ray>
+        <plugin name="gazebo_ros_lidar_controller" filename="libgazebo_ros_gpu_laser.so">
+          <topicName>scan</topicName>
+          <frameName>laser</frameName>
+        </plugin>
+      </sensor>
+    </gazebo>
+  </xacro:if>
 
   <!-- Base IMU -->
   <gazebo reference="base_link">