From 3bf1dd8f539333df6cfae438547a330f9aac0f3c Mon Sep 17 00:00:00 2001
From: Binit Shah <bshah@hello-robot.com>
Date: Mon, 13 Sep 2021 14:04:35 -0700
Subject: [PATCH] Revert Stretch Navigation and Gazebo files

---
 stretch_gazebo/launch/gazebo.launch           |  2 +-
 stretch_gazebo/urdf/stretch_gazebo.urdf.xacro |  4 +--
 stretch_navigation/launch/mapping.launch      | 25 +++++++++----------
 stretch_navigation/launch/navigation.launch   | 17 ++++++-------
 4 files changed, 23 insertions(+), 25 deletions(-)

diff --git a/stretch_gazebo/launch/gazebo.launch b/stretch_gazebo/launch/gazebo.launch
index 3213794..6b75bcd 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/willowgarage.world"/>
+  <arg name="world" default="worlds/empty.world"/>
 
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
     <arg name="world_name" value="$(arg world)" />
diff --git a/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro b/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro
index c095343..1e516a3 100644
--- a/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro
+++ b/stretch_gazebo/urdf/stretch_gazebo.urdf.xacro
@@ -258,7 +258,7 @@
             </horizontal>
           </scan>
           <range>
-            <min>0.2</min>
+            <min>0.15</min>
             <max>12.0</max>
             <resolution>0.01</resolution>
           </range>
@@ -294,7 +294,7 @@
             </horizontal>
           </scan>
           <range>
-            <min>0.2</min>
+            <min>0.15</min>
             <max>12.0</max>
             <resolution>0.01</resolution>
           </range>
diff --git a/stretch_navigation/launch/mapping.launch b/stretch_navigation/launch/mapping.launch
index e85bebb..71542cb 100644
--- a/stretch_navigation/launch/mapping.launch
+++ b/stretch_navigation/launch/mapping.launch
@@ -6,23 +6,22 @@
   <!-- STRETCH DRIVER -->
   <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="true"/>
   <param name="/stretch_driver/mode" type="string" value="navigation" />
-  <!-- <include file="$(find stretch_core)/launch/stretch_driver.launch" pass_all_args="true"/> -->
+  <include file="$(find stretch_core)/launch/stretch_driver.launch" pass_all_args="true"/>
 
-  <!-- <include file="$(find stretch_core)/launch/rplidar.launch" /> -->
+  <!-- LASER RANGE FINDER -->
+  <include file="$(find stretch_core)/launch/rplidar.launch" />
 
-  <node name="teleop_twist_keyboard" pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" output="screen" >
-    <param name="speed" type="double" value="0.04" />
-    <param name="turn" type="double" value="0.1" />
-    <remap from="/cmd_vel" to="/stretch_diff_drive_controller/cmd_vel" />
-  </node>
-
-  <node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find stretch_navigation)/rviz/octomap_mapper.rviz" />
-
-  <node pkg="gmapping" type="slam_gmapping" name="gmapping_record_map" output="screen" />
-  <include file="$(find stretch_navigation)/launch/octomap_mapper.launch">
-    <arg name="pointcloud_topic" value="/camera/depth/color/points"/>
+  <!-- TELEOP -->
+  <include file="$(find stretch_core)/launch/teleop_twist.launch">
+    <arg name="teleop_type" value="$(arg teleop_type)" />
+    <arg name="linear" value="0.04" />
+    <arg name="angular" value="0.1" />
+    <arg name="twist_topic" value="/stretch/cmd_vel" />
   </include>
 
+  <!-- MAPPING -->
+  <node pkg="gmapping" type="slam_gmapping" name="gmapping_record_map" output="log" />
+
   <!-- VISUALIZE -->
   <node name="rviz" pkg="rviz" type="rviz" output="log" args="-d $(find stretch_navigation)/rviz/mapping.rviz" if="$(arg rviz)" />
 
diff --git a/stretch_navigation/launch/navigation.launch b/stretch_navigation/launch/navigation.launch
index 986358f..1aa359a 100644
--- a/stretch_navigation/launch/navigation.launch
+++ b/stretch_navigation/launch/navigation.launch
@@ -1,20 +1,18 @@
 <launch>
 
-  <arg name="map_yaml" />
-  <arg name="octomap_location" />
+  <arg name="map_yaml"                doc="filepath to previously captured map (required)" />
+  <arg name="rviz"     default="true" doc="whether to show Rviz" />
 
   <!-- STRETCH DRIVER -->
   <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="true"/>
   <param name="/stretch_driver/mode" type="string" value="navigation" />
-  <!-- <include file="$(find stretch_core)/launch/stretch_driver.launch" pass_all_args="true"/> -->
+  <include file="$(find stretch_core)/launch/stretch_driver.launch" pass_all_args="true"/>
 
-  <!-- <include file="$(find stretch_core)/launch/rplidar.launch" /> -->
+  <!-- LASER RANGE FINDER -->
+  <include file="$(find stretch_core)/launch/rplidar.launch" />
 
   <!-- MAP SERVER -->
   <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_yaml)" />
-  <include file="$(find stretch_navigation)/launch/octomap_server.launch">
-    <arg name="map" value="$(arg octomap_location)"/>
-  </include>
 
   <!-- LOCALIZATION -->
   <include file="$(find amcl)/examples/amcl_diff.launch" />
@@ -26,9 +24,10 @@
     <rosparam file="$(find stretch_navigation)/config/local_costmap_params.yaml" command="load" />
     <rosparam file="$(find stretch_navigation)/config/global_costmap_params_withmap.yaml" command="load" />
     <rosparam file="$(find stretch_navigation)/config/base_local_planner_params.yaml" command="load" />
-    <remap from="/cmd_vel" to="/stretch_diff_drive_controller/cmd_vel" />
+    <remap from="/cmd_vel" to="/stretch/cmd_vel" />
   </node>
 
-  <node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find stretch_navigation)/rviz/navigation.rviz" />
+  <!-- VISUALIZE -->
+  <node name="rviz" pkg="rviz" type="rviz" output="log" args="-d $(find stretch_navigation)/rviz/navigation.rviz" if="$(arg rviz)" />
 
 </launch>