From 255e5c581c3654375f35ec63b1da4ade19dc0280 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 3 May 2020 12:29:24 +0200 Subject: [PATCH 01/14] First version of joy_teleop --- launch/joy_teleop.launch | 12 ++++++++++++ parameters/tools/teleop.yaml | 14 ++++++++++++++ 2 files changed, 26 insertions(+) create mode 100644 launch/joy_teleop.launch create mode 100644 parameters/tools/teleop.yaml diff --git a/launch/joy_teleop.launch b/launch/joy_teleop.launch new file mode 100644 index 00000000..3ed71797 --- /dev/null +++ b/launch/joy_teleop.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/parameters/tools/teleop.yaml b/parameters/tools/teleop.yaml new file mode 100644 index 00000000..cb6c9a37 --- /dev/null +++ b/parameters/tools/teleop.yaml @@ -0,0 +1,14 @@ +teleop: + move: + type: topic + message_type: geometry_msgs/Twist + topic_name: base/references + axis_mappings: + - + axis: 1 + target: linear.x + scale: 1.0 + - + axis: 2 + target: angular.z + scale: 1.0 From 72f345c38ee4c0c0f5158852ee06774d0f52835a Mon Sep 17 00:00:00 2001 From: Josja Geijsberts Date: Sat, 5 Jun 2021 11:43:00 +0200 Subject: [PATCH 02/14] Added mapping and removed old setup. --- launch/tools/joy_teleop.launch | 14 +++++++ parameters/tools/teleop.yaml | 14 ------- scripts/joystick_controls_node | 72 ++++++++++++++++++++++++++++++++++ 3 files changed, 86 insertions(+), 14 deletions(-) create mode 100644 launch/tools/joy_teleop.launch delete mode 100644 parameters/tools/teleop.yaml create mode 100755 scripts/joystick_controls_node diff --git a/launch/tools/joy_teleop.launch b/launch/tools/joy_teleop.launch new file mode 100644 index 00000000..7493335d --- /dev/null +++ b/launch/tools/joy_teleop.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + \ No newline at end of file diff --git a/parameters/tools/teleop.yaml b/parameters/tools/teleop.yaml deleted file mode 100644 index cb6c9a37..00000000 --- a/parameters/tools/teleop.yaml +++ /dev/null @@ -1,14 +0,0 @@ -teleop: - move: - type: topic - message_type: geometry_msgs/Twist - topic_name: base/references - axis_mappings: - - - axis: 1 - target: linear.x - scale: 1.0 - - - axis: 2 - target: angular.z - scale: 1.0 diff --git a/scripts/joystick_controls_node b/scripts/joystick_controls_node new file mode 100755 index 00000000..1b56f30f --- /dev/null +++ b/scripts/joystick_controls_node @@ -0,0 +1,72 @@ +#! /usr/bin/env python + +import sys + +import rospy +import rosnode +import actionlib +from sensor_msgs.msg import Joy +from geometry_msgs.msg import Twist +from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryActionGoal +from tue_manipulation_msgs.msg import GripperCommandAction, GripperCommandActionGoal + + +class JoystickControls(object): + 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_mode = 4 + # self.map = self.controller_mappings[self.controller_mode] + self.controller_sub = rospy.Subscriber("joy", Joy, self.map_joystick, 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.end_effector_client = actionlib.SimpleActionClient("body/joint_trajectory_action", FollowJointTrajectoryAction) + # self.gripper_client = actionlib.SimpleActionClient("gripper/action", GripperCommandAction) + # self.end_effector_client.wait_for_server() + # self.gripper_client.wait_for_server() + + def _send_controls(self, mapping): + # 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']: + # close/open gripper + print("Clicked A") + if mapping['X']: + # reset? + if mapping['Y']: + # move to current fixed pose + + 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._send_controls(controller_mappings[self.controller_mode]) + + +if __name__ == "__main__": + rospy.init_node("joystick_controls_node") + ConversionNode = JoystickControls() + sys.exit(rospy.spin()) From c0b02cf81bc15f483c4d71519f5a7220f9b1b6dd Mon Sep 17 00:00:00 2001 From: Josja Geijsberts Date: Sat, 5 Jun 2021 15:37:25 +0200 Subject: [PATCH 03/14] Add additional command options for the controller and ensure continuous publishing of command velocities --- launch/joy_teleop.launch | 12 -------- scripts/joystick_controls_node | 52 +++++++++++++++++++++++++++------- 2 files changed, 42 insertions(+), 22 deletions(-) delete mode 100644 launch/joy_teleop.launch diff --git a/launch/joy_teleop.launch b/launch/joy_teleop.launch deleted file mode 100644 index 3ed71797..00000000 --- a/launch/joy_teleop.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/scripts/joystick_controls_node b/scripts/joystick_controls_node index 1b56f30f..f899760e 100755 --- a/scripts/joystick_controls_node +++ b/scripts/joystick_controls_node @@ -7,9 +7,11 @@ 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 class JoystickControls(object): def __init__(self): @@ -19,29 +21,52 @@ class JoystickControls(object): 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.end_effector_client = actionlib.SimpleActionClient("body/joint_trajectory_action", FollowJointTrajectoryAction) - # self.gripper_client = actionlib.SimpleActionClient("gripper/action", GripperCommandAction) - # self.end_effector_client.wait_for_server() - # self.gripper_client.wait_for_server() + 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 + + # send controls at constant rate + r = rospy.Rate(10) # 10hz + while not rospy.is_shutdown(): + self.publish_controls() + r.sleep() def _send_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']: - # close/open gripper - print("Clicked 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']: - # reset? + self.robot.reset() if mapping['Y']: - # move to current fixed pose + 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'] + if abs(mapping['left_stick_horizontal']) > 0.1: # angular base movement + self.base_command.angular.z = 0.6 * mapping['left_stick_horizontal'] + + if abs(mapping['right_stick_horizontal']) < 0.1 and abs(mapping['right_stick_vertical']) < 0.1 and abs(mapping['left_stick_horizontal']) < 0.1: + self.base_command = Twist() + + def publish_controls(self): + 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], @@ -65,6 +90,13 @@ class JoystickControls(object): } self._send_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") From 952d10f58ca2a110c1000e831bce953539b0ff8d Mon Sep 17 00:00:00 2001 From: Josja Geijsberts Date: Sat, 5 Jun 2021 16:33:38 +0200 Subject: [PATCH 04/14] Added speaking button. --- scripts/joystick_controls_node | 2 ++ 1 file changed, 2 insertions(+) diff --git a/scripts/joystick_controls_node b/scripts/joystick_controls_node index f899760e..16bb30d8 100755 --- a/scripts/joystick_controls_node +++ b/scripts/joystick_controls_node @@ -59,6 +59,8 @@ class JoystickControls(object): self.base_command.linear.y = 0.4 * mapping['right_stick_horizontal'] if abs(mapping['left_stick_horizontal']) > 0.1: # angular base movement self.base_command.angular.z = 0.6 * mapping['left_stick_horizontal'] + if mapping['right_bumper']: + self.robot.speech.speak("Beep. Boop. Beep boop.") if abs(mapping['right_stick_horizontal']) < 0.1 and abs(mapping['right_stick_vertical']) < 0.1 and abs(mapping['left_stick_horizontal']) < 0.1: self.base_command = Twist() From 81051a243f9836b4373869301506bec781c66e9e Mon Sep 17 00:00:00 2001 From: Josja Geijsberts Date: Tue, 15 Jun 2021 21:37:11 +0200 Subject: [PATCH 05/14] Small cleanup --- launch/hero_bringup.launch | 3 +++ launch/tools/joy_teleop.launch | 2 -- scripts/joystick_controls_node | 5 ----- 3 files changed, 3 insertions(+), 7 deletions(-) diff --git a/launch/hero_bringup.launch b/launch/hero_bringup.launch index 7d8d362e..93ad1bc9 100644 --- a/launch/hero_bringup.launch +++ b/launch/hero_bringup.launch @@ -206,6 +206,9 @@ + + + diff --git a/launch/tools/joy_teleop.launch b/launch/tools/joy_teleop.launch index 7493335d..7cc51e4e 100644 --- a/launch/tools/joy_teleop.launch +++ b/launch/tools/joy_teleop.launch @@ -4,8 +4,6 @@ - - diff --git a/scripts/joystick_controls_node b/scripts/joystick_controls_node index 16bb30d8..44ba8f41 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 @@ -19,11 +16,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']) From 96631384766eb8a3ce2e9a9295c7fc816f5a6d6e Mon Sep 17 00:00:00 2001 From: Josja Geijsberts Date: Tue, 15 Jun 2021 21:44:00 +0200 Subject: [PATCH 06/14] Removed redundant namespace. --- launch/tools/joy_teleop.launch | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/launch/tools/joy_teleop.launch b/launch/tools/joy_teleop.launch index 7cc51e4e..51aac73c 100644 --- a/launch/tools/joy_teleop.launch +++ b/launch/tools/joy_teleop.launch @@ -1,12 +1,8 @@ - - - - - - - + + + \ No newline at end of file From 9914fdc28611d85c36032795c72bee33080b7338 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Sun, 3 May 2020 12:29:24 +0200 Subject: [PATCH 07/14] First version of joy_teleop --- launch/joy_teleop.launch | 12 ++++++++++++ parameters/tools/teleop.yaml | 14 ++++++++++++++ 2 files changed, 26 insertions(+) create mode 100644 launch/joy_teleop.launch create mode 100644 parameters/tools/teleop.yaml diff --git a/launch/joy_teleop.launch b/launch/joy_teleop.launch new file mode 100644 index 00000000..45def863 --- /dev/null +++ b/launch/joy_teleop.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/parameters/tools/teleop.yaml b/parameters/tools/teleop.yaml new file mode 100644 index 00000000..cb6c9a37 --- /dev/null +++ b/parameters/tools/teleop.yaml @@ -0,0 +1,14 @@ +teleop: + move: + type: topic + message_type: geometry_msgs/Twist + topic_name: base/references + axis_mappings: + - + axis: 1 + target: linear.x + scale: 1.0 + - + axis: 2 + target: angular.z + scale: 1.0 From cf4904bfe017ecc8b37ed4a22a3a3f261311cc31 Mon Sep 17 00:00:00 2001 From: Josja Geijsberts Date: Sat, 5 Jun 2021 11:43:00 +0200 Subject: [PATCH 08/14] Added mapping and removed old setup. --- launch/joy_teleop.launch | 12 ---- launch/tools/joy_teleop.launch | 14 +++++ parameters/tools/teleop.yaml | 14 ----- scripts/joystick_controls_node | 105 +++++++++++++++++++++++++++++++++ 4 files changed, 119 insertions(+), 26 deletions(-) delete mode 100644 launch/joy_teleop.launch create mode 100644 launch/tools/joy_teleop.launch delete mode 100644 parameters/tools/teleop.yaml create mode 100755 scripts/joystick_controls_node diff --git a/launch/joy_teleop.launch b/launch/joy_teleop.launch deleted file mode 100644 index 45def863..00000000 --- a/launch/joy_teleop.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/launch/tools/joy_teleop.launch b/launch/tools/joy_teleop.launch new file mode 100644 index 00000000..12770e24 --- /dev/null +++ b/launch/tools/joy_teleop.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/parameters/tools/teleop.yaml b/parameters/tools/teleop.yaml deleted file mode 100644 index cb6c9a37..00000000 --- a/parameters/tools/teleop.yaml +++ /dev/null @@ -1,14 +0,0 @@ -teleop: - move: - type: topic - message_type: geometry_msgs/Twist - topic_name: base/references - axis_mappings: - - - axis: 1 - target: linear.x - scale: 1.0 - - - axis: 2 - target: angular.z - scale: 1.0 diff --git a/scripts/joystick_controls_node b/scripts/joystick_controls_node new file mode 100755 index 00000000..5b1c15dd --- /dev/null +++ b/scripts/joystick_controls_node @@ -0,0 +1,105 @@ +#! /usr/bin/env python + +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 + + +class JoystickControls(object): + 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_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']) + self.gripper_state = None + + # set all out-msgs + self.base_command = Twist() + # self.head_command = None + + # send controls at constant rate + r = rospy.Rate(10) # 10hz + while not rospy.is_shutdown(): + self.publish_controls() + r.sleep() + + def _send_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'] + if abs(mapping['left_stick_horizontal']) > 0.1: # angular base movement + self.base_command.angular.z = 0.6 * mapping['left_stick_horizontal'] + + if abs(mapping['right_stick_horizontal']) < 0.1 and abs(mapping['right_stick_vertical']) < 0.1 and abs(mapping['left_stick_horizontal']) < 0.1: + self.base_command = Twist() + + def publish_controls(self): + 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._send_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()) From a9cf6e97fcad02e972c0fb853e3e40ea3705599f Mon Sep 17 00:00:00 2001 From: Josja Geijsberts Date: Sat, 5 Jun 2021 16:33:38 +0200 Subject: [PATCH 09/14] Added speaking button. --- scripts/joystick_controls_node | 2 ++ 1 file changed, 2 insertions(+) diff --git a/scripts/joystick_controls_node b/scripts/joystick_controls_node index 5b1c15dd..f798ca6e 100755 --- a/scripts/joystick_controls_node +++ b/scripts/joystick_controls_node @@ -60,6 +60,8 @@ class JoystickControls(object): self.base_command.linear.y = 0.4 * mapping['right_stick_horizontal'] if abs(mapping['left_stick_horizontal']) > 0.1: # angular base movement self.base_command.angular.z = 0.6 * mapping['left_stick_horizontal'] + if mapping['right_bumper']: + self.robot.speech.speak("Beep. Boop. Beep boop.") if abs(mapping['right_stick_horizontal']) < 0.1 and abs(mapping['right_stick_vertical']) < 0.1 and abs(mapping['left_stick_horizontal']) < 0.1: self.base_command = Twist() From 03d1be593a380eda28a6017b227cf6dda1765447 Mon Sep 17 00:00:00 2001 From: Josja Geijsberts Date: Tue, 15 Jun 2021 21:37:11 +0200 Subject: [PATCH 10/14] 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 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 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], From 0c380f58c62885b35e7d8df7e731ce07dc917a63 Mon Sep 17 00:00:00 2001 From: Josja Geijsberts Date: Sat, 7 May 2022 15:52:21 +0200 Subject: [PATCH 11/14] started cleanup, not tested properly --- launch/tools/joy_teleop.launch | 9 ++++-- scripts/joystick_controls_node | 58 ++++++++++++++++------------------ 2 files changed, 34 insertions(+), 33 deletions(-) diff --git a/launch/tools/joy_teleop.launch b/launch/tools/joy_teleop.launch index 286954a5..5a3a13e2 100644 --- a/launch/tools/joy_teleop.launch +++ b/launch/tools/joy_teleop.launch @@ -1,8 +1,13 @@ + - + - + + + + + diff --git a/scripts/joystick_controls_node b/scripts/joystick_controls_node index af7e3513..9c3d683d 100755 --- a/scripts/joystick_controls_node +++ b/scripts/joystick_controls_node @@ -12,11 +12,33 @@ 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_mode = 4 + 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) @@ -29,11 +51,8 @@ class JoystickControls(object): self.base_command = Twist() # self.head_command = None - # send controls at constant rate - r = rospy.Rate(10) # 10hz - while not rospy.is_shutdown(): - self.publish_controls() - r.sleep() + # send controls at 10 Hz + rospy.Timer(rospy.Duration(0.1), self.publish_controls()) def _send_controls(self, mapping): self.base_command = Twist() @@ -58,34 +77,11 @@ class JoystickControls(object): if mapping['right_bumper']: self.robot.speech.speak("Beep. Boop. Beep boop.") - if abs(mapping['right_stick_horizontal']) < 0.1 and abs(mapping['right_stick_vertical']) < 0.1 and abs(mapping['left_stick_horizontal']) < 0.1: - self.base_command = Twist() - - def publish_controls(self): + def publish_controls(self, event=None): 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._send_controls(controller_mappings[self.controller_mode]) def get_gripper_state(self, state_msg): From 1fdb3cb8793f246546cdc768ec0af1bd79728f48 Mon Sep 17 00:00:00 2001 From: Josja Geijsberts Date: Tue, 31 May 2022 21:19:04 +0200 Subject: [PATCH 12/14] fix: only send base_commands if the user of the joystick wants to --- scripts/joystick_controls_node | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/scripts/joystick_controls_node b/scripts/joystick_controls_node index 9c3d683d..dbd46129 100755 --- a/scripts/joystick_controls_node +++ b/scripts/joystick_controls_node @@ -50,11 +50,12 @@ class JoystickControls(object): # 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()) + rospy.Timer(rospy.Duration(0.1), self.publish_controls) - def _send_controls(self, mapping): + 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']: @@ -72,13 +73,18 @@ class JoystickControls(object): 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): - self.base_pub.publish(self.base_command) + if self.non_zero_vel_received: + self.base_pub.publish(self.base_command) def map_joystick(self, joy_msg): From 0799f6edf8226f2a228b59d6fae4a3673cb0b8f9 Mon Sep 17 00:00:00 2001 From: Josja Geijsberts Date: Tue, 31 May 2022 21:20:35 +0200 Subject: [PATCH 13/14] typo: use changed function name when calling --- scripts/joystick_controls_node | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/joystick_controls_node b/scripts/joystick_controls_node index dbd46129..127f7080 100755 --- a/scripts/joystick_controls_node +++ b/scripts/joystick_controls_node @@ -88,7 +88,7 @@ class JoystickControls(object): def map_joystick(self, joy_msg): - self._send_controls(controller_mappings[self.controller_mode]) + self._translate_controls(controller_mappings[self.controller_mode]) def get_gripper_state(self, state_msg): if state_msg.name[20] == 'hand_motor_joint': From ce0ca67e7b25cceb9703b4b5eb27d045c8f3e196 Mon Sep 17 00:00:00 2001 From: Josja Geijsberts Date: Tue, 21 Jun 2022 22:12:04 +0200 Subject: [PATCH 14/14] working version, tested on the robot, still requires cleaning --- scripts/joystick_controls_node | 64 ++++++++++++++++++++++------------ 1 file changed, 42 insertions(+), 22 deletions(-) diff --git a/scripts/joystick_controls_node b/scripts/joystick_controls_node index 127f7080..34560a93 100755 --- a/scripts/joystick_controls_node +++ b/scripts/joystick_controls_node @@ -13,27 +13,27 @@ 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] - } - } + # 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 @@ -87,7 +87,27 @@ class JoystickControls(object): 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):