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 @@
+
+
+
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 @@
-
+
-
-
-
-
-
-
-
+
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],