From d55ef54c77839580ce77994095c2798373d82798 Mon Sep 17 00:00:00 2001
From: Josja Geijsberts <j.geijsberts@student.tue.nl>
Date: Tue, 15 Jun 2021 21:37:11 +0200
Subject: [PATCH] Small cleanup

---
 launch/hero_bringup.launch     |  3 +++
 launch/tools/joy_teleop.launch | 10 ++--------
 scripts/joystick_controls_node |  7 +------
 3 files changed, 6 insertions(+), 14 deletions(-)

diff --git a/launch/hero_bringup.launch b/launch/hero_bringup.launch
index db78aa7a..bdd68f5c 100644
--- a/launch/hero_bringup.launch
+++ b/launch/hero_bringup.launch
@@ -209,6 +209,9 @@
             <remap from="odom" to="base/measurements" />
         </node>
 
+        <!-- Joystick controller -->
+        <include file="$(find hero_bringup)/launch/tools/joy_teleop.launch" />
+
     </group> <!-- End of TU/e namespace group -->
 
 </launch>
diff --git a/launch/tools/joy_teleop.launch b/launch/tools/joy_teleop.launch
index 12770e24..286954a5 100644
--- a/launch/tools/joy_teleop.launch
+++ b/launch/tools/joy_teleop.launch
@@ -1,14 +1,8 @@
 <?xml version="1.0"?>
 <launch>
 
-    <arg name="namespace" default="/hero" />
+    <node name="joy_teleop" pkg="joy" type="joy_node" />
 
-    <group ns="$(arg namespace)">
-        <!-- <rosparam command="load" file="$(find hero_bringup)/parameters/tools/teleop.yaml" /> -->
-
-        <node name="joy_teleop" pkg="joy" type="joy_node" />
-
-        <node name="joy_controls" pkg="hero_bringup" type="joystick_controls_node" />
-    </group>
+    <node name="joy_controls" pkg="hero_bringup" type="joystick_controls_node" />
 
 </launch>
diff --git a/scripts/joystick_controls_node b/scripts/joystick_controls_node
index f798ca6e..af7e3513 100755
--- a/scripts/joystick_controls_node
+++ b/scripts/joystick_controls_node
@@ -4,12 +4,9 @@ import sys
 
 import rospy
 import rosnode
-import actionlib
 from sensor_msgs.msg import Joy
 from geometry_msgs.msg import Twist
 from sensor_msgs.msg import JointState
-from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryActionGoal
-from tue_manipulation_msgs.msg import GripperCommandAction, GripperCommandActionGoal
 from robot_skills.get_robot import get_robot
 from robot_skills.arm import arms
 
@@ -20,11 +17,9 @@ class JoystickControls(object):
         Subscribes to the Joy messages sent via the controller and the joy node and then transforms these to the desired behavior
         """
         self.controller_mode = 4
-        # self.map = self.controller_mappings[self.controller_mode]
         self.controller_sub = rospy.Subscriber("joy", Joy, self.map_joystick, queue_size=1)
         self.gripper_state_sub = rospy.Subscriber("joint_states", JointState, self.get_gripper_state, queue_size=1)
 
-        # Publish to kill all state_machines, to open/close the gripper, to move the base, loaded trajectory/pose, move the end effector
         self.base_pub = rospy.Publisher("base/references", Twist, queue_size=1)
         self.robot = get_robot('hero')
         self.robot_arm = self.robot.get_arm(required_gripper_types=[arms.GripperTypes.GRASPING], required_goals=['show_gripper'])
@@ -67,7 +62,7 @@ class JoystickControls(object):
             self.base_command = Twist()
 
     def publish_controls(self):
-            self.base_pub.publish(self.base_command)
+        self.base_pub.publish(self.base_command)
 
     def map_joystick(self, joy_msg):
         controller_mappings = {4: {'A': joy_msg.buttons[0],