From d3f9e464c4df45dba41a3e83249b6e58845dcd30 Mon Sep 17 00:00:00 2001
From: Binit Shah <bshah@hello-robot.com>
Date: Fri, 3 Sep 2021 15:46:44 -0700
Subject: [PATCH] Add Joystick teleop to Stretch Navigation

---
 stretch_core/launch/teleop_twist.launch       | 33 +++++++++++++++++++
 stretch_core/package.xml                      |  3 ++
 stretch_navigation/launch/mapping.launch      | 23 ++++++++-----
 .../launch/mapping_gazebo.launch              | 28 ++++++++--------
 stretch_navigation/launch/navigation.launch   |  4 +--
 .../launch/navigation_gazebo.launch           | 10 +++---
 6 files changed, 73 insertions(+), 28 deletions(-)
 create mode 100644 stretch_core/launch/teleop_twist.launch

diff --git a/stretch_core/launch/teleop_twist.launch b/stretch_core/launch/teleop_twist.launch
new file mode 100644
index 0000000..80602cd
--- /dev/null
+++ b/stretch_core/launch/teleop_twist.launch
@@ -0,0 +1,33 @@
+<launch>
+
+  <arg name="teleop_type"   default="keyboard"         doc="how to teleop ('keyboard', 'joystick', or 'none')" />
+  <arg name="linear"        default="0.04"             doc="linear speed (m/s)" />
+  <arg name="angular"       default="0.1"              doc="angular speed (rad/s)" />
+  <arg name="twist_topic"   default="/stretch/cmd_vel" doc="topic to command Twist messages" />
+  <arg name="joystick_port" default="/dev/input/js0"   doc="joystick USB device name" />
+
+  <!-- KEYBOARD TELEOP -->
+  <group if="$(eval teleop_type == 'keyboard')">
+    <node name="teleop_twist_keyboard" pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" output="screen" >
+      <param name="speed" type="double" value="$(arg linear)" />
+      <param name="turn" type="double" value="$(arg angular)" />
+      <remap from="/cmd_vel" to="$(arg twist_topic)" />
+    </node>
+  </group>
+
+  <!-- JOYSTICK TELEOP -->
+  <group if="$(eval teleop_type == 'joystick')">
+    <node name="joy" pkg="joy" type="joy_node" output="screen">
+        <param name="dev" value="$(arg joystick_port)" />
+        <param name="autorepeat_rate" value="20" />
+        <param name="deadzone" value="0.05" />
+    </node>
+    <node name="teleop_twist_joy" pkg="teleop_twist_joy" type="teleop_node">
+        <param name="enable_button" type="int" value="-1" /> <!-- value="0" to enable deadman button -->
+        <param name="scale_linear" type="double" value="$(arg linear)" />
+        <param name="scale_angular" type="double" value="$(arg angular)" />
+        <remap from="/cmd_vel" to="$(arg twist_topic)" />
+    </node>
+  </group>
+
+</launch>
diff --git a/stretch_core/package.xml b/stretch_core/package.xml
index f152327..9678954 100644
--- a/stretch_core/package.xml
+++ b/stretch_core/package.xml
@@ -83,6 +83,9 @@
   <exec_depend>std_srvs</exec_depend>
   <exec_depend>tf</exec_depend>
   <exec_depend>tf2</exec_depend>
+  <exec_depend>joy</exec_depend>
+  <exec_depend>teleop_twist_joy</exec_depend>
+  <exec_depend>teleop_twist_keyboard</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->
diff --git a/stretch_navigation/launch/mapping.launch b/stretch_navigation/launch/mapping.launch
index 4fce65b..71542cb 100644
--- a/stretch_navigation/launch/mapping.launch
+++ b/stretch_navigation/launch/mapping.launch
@@ -1,21 +1,28 @@
 <launch>
 
-  <arg name="rviz" default="true"/>
+  <arg name="rviz"        default="true"     doc="whether to show Rviz" />
+  <arg name="teleop_type" default="keyboard" doc="how to teleop ('keyboard', 'joystick', or 'none')" />
 
+  <!-- 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"/>
 
+  <!-- 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/cmd_vel" />
-  </node>
-
-  <node name="rviz" pkg="rviz" type="rviz" output="log" args="-d $(find stretch_navigation)/rviz/mapping.rviz" if="$(arg rviz)" />
+  <!-- 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)" />
+
 </launch>
diff --git a/stretch_navigation/launch/mapping_gazebo.launch b/stretch_navigation/launch/mapping_gazebo.launch
index cf7e39b..d2ce28b 100644
--- a/stretch_navigation/launch/mapping_gazebo.launch
+++ b/stretch_navigation/launch/mapping_gazebo.launch
@@ -1,9 +1,10 @@
 <launch>
 
-  <arg name="rviz" default="true"/>
-  <arg name="gazebo_gpu_lidar" default="false"/>
-  <arg name="gazebo_visualize_lidar" default="false"/>
-  <arg name="gazebo_world" default="worlds/empty.world"/>
+  <arg name="rviz"                   default="true"                      doc="whether to show Rviz" />
+  <arg name="teleop_type"            default="keyboard"                  doc="how to teleop ('keyboard', 'joystick', or 'none')" />
+  <arg name="gazebo_world"           default="worlds/willowgarage.world" doc="the environment within which Stretch is loaded in Gazebo" />
+  <arg name="gazebo_gpu_lidar"       default="false"                     doc="whether to compute lidar with hardware acceleration (requires GPU)" />
+  <arg name="gazebo_visualize_lidar" default="false"                     doc="whether to visualize planar lidar within Gazebo" />
 
   <!-- GAZEBO SIMULATION -->
   <include file="$(find stretch_gazebo)/launch/gazebo.launch">
@@ -13,16 +14,17 @@
   </include>
 
   <!-- TELEOP -->
-  <node name="teleop_twist_keyboard" pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" output="screen" >
-    <param name="speed" type="double" value="1.0" />
-    <param name="turn" type="double" value="2.0" />
-    <remap from="/cmd_vel" to="/stretch_diff_drive_controller/cmd_vel" />
-  </node>
-
-  <!-- RVIZ -->
-  <node name="rviz" pkg="rviz" type="rviz" output="log" args="-d $(find stretch_navigation)/rviz/mapping.rviz" if="$(arg rviz)" />
+  <include file="$(find stretch_core)/launch/teleop_twist.launch">
+    <arg name="teleop_type" value="$(arg teleop_type)" />
+    <arg name="linear" value="1.0" />
+    <arg name="angular" value="2.0" />
+    <arg name="twist_topic" value="/stretch_diff_drive_controller/cmd_vel" />
+  </include>
 
-  <!-- GMAPPING -->
+  <!-- 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)" />
+
 </launch>
diff --git a/stretch_navigation/launch/navigation.launch b/stretch_navigation/launch/navigation.launch
index 2a359b5..2423f3f 100644
--- a/stretch_navigation/launch/navigation.launch
+++ b/stretch_navigation/launch/navigation.launch
@@ -1,7 +1,7 @@
 <launch>
 
-  <arg name="map_yaml" />
-  <arg name="rviz" default="true"/>
+  <arg name="map_yaml"                doc="filepath to previously captured map (required)" />
+  <arg name="rviz"     default="true" doc="whether to show Rviz" />
 
   <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="true"/>
   <param name="/stretch_driver/mode" type="string" value="navigation" />
diff --git a/stretch_navigation/launch/navigation_gazebo.launch b/stretch_navigation/launch/navigation_gazebo.launch
index d5cc933..dce81c5 100644
--- a/stretch_navigation/launch/navigation_gazebo.launch
+++ b/stretch_navigation/launch/navigation_gazebo.launch
@@ -1,10 +1,10 @@
 <launch>
 
-  <arg name="map_yaml" />
-  <arg name="rviz" default="true"/>
-  <arg name="gazebo_gpu_lidar" default="false"/>
-  <arg name="gazebo_visualize_lidar" default="false"/>
-  <arg name="gazebo_world" default="worlds/empty.world"/>
+  <arg name="map_yaml"                                                   doc="filepath to previously captured map (required)" />
+  <arg name="rviz"                   default="true"                      doc="whether to show Rviz" />
+  <arg name="gazebo_world"           default="worlds/willowgarage.world" doc="the environment within which Stretch is loaded in Gazebo" />
+  <arg name="gazebo_gpu_lidar"       default="false"                     doc="whether to compute lidar with hardware acceleration (requires GPU)" />
+  <arg name="gazebo_visualize_lidar" default="false"                     doc="whether to visualize planar lidar within Gazebo" />
 
   <!-- GAZEBO SIMULATION -->
   <include file="$(find stretch_gazebo)/launch/gazebo.launch">