From f03dd5c88030fb62769e98d89bf5fb3ee6a480ee Mon Sep 17 00:00:00 2001
From: Naoya Yamaguchi <708yamaguchi@gmail.com>
Date: Fri, 10 Jul 2020 13:06:12 +0900
Subject: [PATCH] [fetch_gazebo] Add option to launch teleop node

---
 fetch_gazebo/launch/include/fetch.launch.xml | 13 +++++++++----
 fetch_gazebo/launch/playground.launch        |  9 ++++++++-
 2 files changed, 17 insertions(+), 5 deletions(-)

diff --git a/fetch_gazebo/launch/include/fetch.launch.xml b/fetch_gazebo/launch/include/fetch.launch.xml
index f8f683f..b63dfdb 100644
--- a/fetch_gazebo/launch/include/fetch.launch.xml
+++ b/fetch_gazebo/launch/include/fetch.launch.xml
@@ -4,6 +4,9 @@
   <arg name="y" default="0.0" />
   <arg name="z" default="0.0" />
   <arg name="yaw" default="0.0" />
+  <arg name="launch_teleop" default="true" />
+  <arg name="joy_device" default="/dev/fetch_joy" />
+  <arg name="ps4" default="false" />
 
   <!-- Setup controllers -->
   <rosparam file="$(find fetch_gazebo)/config/default_controllers.yaml" command="load" />
@@ -31,9 +34,11 @@
     <param name="lazy" type="bool" value="True"/>
   </node>
 
-  <!-- Start a mux between application and teleop -->
-  <node pkg="topic_tools" type="mux" name="cmd_vel_mux" respawn="true" args="base_controller/command /cmd_vel /teleop/cmd_vel">
-    <remap from="mux" to="cmd_vel_mux" />
-  </node>
+  <!-- Teleop -->
+  <include if="$(arg launch_teleop)" file="$(find fetch_bringup)/launch/include/teleop.launch.xml">
+    <arg name="joy_device" value="$(arg joy_device)"/>
+    <arg name="ps4" value="$(arg ps4)"/>
+  </include>
+  <param name="joy/deadzone" value="0.05"/>
 
 </launch>
diff --git a/fetch_gazebo/launch/playground.launch b/fetch_gazebo/launch/playground.launch
index 97ce796..2f75ba0 100644
--- a/fetch_gazebo/launch/playground.launch
+++ b/fetch_gazebo/launch/playground.launch
@@ -6,6 +6,9 @@
   <arg name="debug" default="false"/>
   <arg name="gui" default="true"/>
   <arg name="headless" default="false"/>
+  <arg name="launch_teleop" default="true"/>
+  <arg name="joy_device" default="/dev/fetch_joy"/>
+  <arg name="ps4" default="false"/>
 
   <!-- Start Gazebo with a blank world -->
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
@@ -18,6 +21,10 @@
   </include>
 
   <!-- Oh, you wanted a robot? -->
-  <include file="$(find fetch_gazebo)/launch/include/$(arg robot).launch.xml" />
+  <include file="$(find fetch_gazebo)/launch/include/$(arg robot).launch.xml">
+    <arg name="launch_teleop" value="$(arg launch_teleop)"/>
+    <arg name="joy_device" value="$(arg joy_device)"/>
+    <arg name="ps4" value="$(arg ps4)"/>
+  </include>
 
 </launch>
\ No newline at end of file