diff --git a/launch/hero_bringup.launch b/launch/hero_bringup.launch index f011819c..220e1cbb 100644 --- a/launch/hero_bringup.launch +++ b/launch/hero_bringup.launch @@ -213,6 +213,9 @@ + + + diff --git a/launch/tools/joy_teleop.launch b/launch/tools/joy_teleop.launch new file mode 100644 index 00000000..5a3a13e2 --- /dev/null +++ b/launch/tools/joy_teleop.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/scripts/joystick_controls_node b/scripts/joystick_controls_node new file mode 100755 index 00000000..34560a93 --- /dev/null +++ b/scripts/joystick_controls_node @@ -0,0 +1,124 @@ +#! /usr/bin/env python + +import sys + +import rospy +import rosnode +from sensor_msgs.msg import Joy +from geometry_msgs.msg import Twist +from sensor_msgs.msg import JointState +from robot_skills.get_robot import get_robot +from robot_skills.arm import arms + + +class JoystickControls(object): + controller_mode = 4 # the used game controller has 4 modes (shown with a light at the back), the default is 4 + # controller_mappings = {4: {'A': joy_msg.buttons[0], + # 'B': joy_msg.buttons[1], + # 'X': joy_msg.buttons[2], + # 'Y': joy_msg.buttons[3], + # 'left_bumper': joy_msg.buttons[4], + # 'right_bumper': joy_msg.buttons[5], + # 'select': joy_msg.buttons[6], + # 'start': joy_msg.buttons[7], + # 'home': joy_msg.buttons[8], + # 'left_stick_pressed': joy_msg.buttons[9], + # 'right_stick_pressed': joy_msg.buttons[10], + # 'left_stick_horizontal': joy_msg.axes[0], + # 'left_stick_vertical': joy_msg.axes[1], + # 'left_trigger': joy_msg.axes[2], + # 'right_stick_horizontal': joy_msg.axes[3], + # 'right_stick_vertical': joy_msg.axes[4], + # 'right_trigger': joy_msg.axes[5], + # 'directional_pad_horizontal': joy_msg.axes[6], + # 'directional_pad_vertical': joy_msg.axes[7] + # } + # } + def __init__(self): + """ + Subscribes to the Joy messages sent via the controller and the joy node and then transforms these to the desired behavior + """ + + 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) + + 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']) + self.gripper_state = None + + # set all out-msgs + self.base_command = Twist() + # self.head_command = None + self.non_zero_vel_received = False + + # send controls at 10 Hz + rospy.Timer(rospy.Duration(0.1), self.publish_controls) + + def _translate_controls(self, mapping): + self.base_command = Twist() + # First check if any state machines need killing since this likely takes longest + if mapping['B']: + rospy.loginfo("Killing existing statemachine.") + rosnode.kill_nodes("/state_machine") # kill whatever statemachine (=challenge/free_mode) is currently running + if mapping['A']: + if self.gripper_state == "open": + self.robot_arm.gripper.send_goal('close') + elif self.gripper_state == "closed": + self.robot_arm.gripper.send_goal('open') + if mapping['X']: + self.robot.reset() + if mapping['Y']: + self.robot_arm.send_joint_goal('show_gripper') + if abs(mapping['right_stick_horizontal']) > 0.1 or abs(mapping['right_stick_vertical']) > 0.1: # linear base movement + self.base_command.linear.x = 0.4 * mapping['right_stick_vertical'] + self.base_command.linear.y = 0.4 * mapping['right_stick_horizontal'] + self.non_zero_vel_received = True + if abs(mapping['left_stick_horizontal']) > 0.1: # angular base movement + self.base_command.angular.z = 0.6 * mapping['left_stick_horizontal'] + self.non_zero_vel_received = True + if mapping['right_bumper']: + self.robot.speech.speak("Beep. Boop. Beep boop.") + if self.base_command.linear.x == 0.0 and self.base_command.linear.y == 0.0 and self.base_command.angular.z == 0.0: + self.non_zero_vel_received = False + + def publish_controls(self, event=None): + if self.non_zero_vel_received: + self.base_pub.publish(self.base_command) + + def map_joystick(self, joy_msg): + controller_mappings = {4: {'A': joy_msg.buttons[0], + 'B': joy_msg.buttons[1], + 'X': joy_msg.buttons[2], + 'Y': joy_msg.buttons[3], + 'left_bumper': joy_msg.buttons[4], + 'right_bumper': joy_msg.buttons[5], + 'select': joy_msg.buttons[6], + 'start': joy_msg.buttons[7], + 'home': joy_msg.buttons[8], + 'left_stick_pressed': joy_msg.buttons[9], + 'right_stick_pressed': joy_msg.buttons[10], + 'left_stick_horizontal': joy_msg.axes[0], + 'left_stick_vertical': joy_msg.axes[1], + 'left_trigger': joy_msg.axes[2], + 'right_stick_horizontal': joy_msg.axes[3], + 'right_stick_vertical': joy_msg.axes[4], + 'right_trigger': joy_msg.axes[5], + 'directional_pad_horizontal': joy_msg.axes[6], + 'directional_pad_vertical': joy_msg.axes[7] + } + } + self._translate_controls(controller_mappings[self.controller_mode]) + + def get_gripper_state(self, state_msg): + if state_msg.name[20] == 'hand_motor_joint': + if state_msg.position[20] > 0.6: + self.gripper_state = 'open' + else: + self.gripper_state = 'closed' + + +if __name__ == "__main__": + rospy.init_node("joystick_controls_node") + ConversionNode = JoystickControls() + sys.exit(rospy.spin())