You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
302 lines
14 KiB
302 lines
14 KiB
#!/usr/bin/env python3
|
|
from __future__ import print_function
|
|
|
|
import math
|
|
import keyboard as kb
|
|
|
|
import rospy
|
|
from sensor_msgs.msg import JointState
|
|
from control_msgs.msg import FollowJointTrajectoryGoal, FollowJointTrajectoryAction
|
|
from trajectory_msgs.msg import JointTrajectoryPoint
|
|
from geometry_msgs.msg import Twist
|
|
import actionlib
|
|
|
|
|
|
# NOTE: FUNMAP services and capabilities won't work with Stretch Gazebo simulation
|
|
|
|
class GetKeyboardCommands:
|
|
def __init__(self):
|
|
self.step_size = 'medium'
|
|
self.rad_per_deg = math.pi / 180.0
|
|
self.small_deg = 3.0
|
|
self.small_rad = self.rad_per_deg * self.small_deg
|
|
self.small_translate = 0.005 # 0.02
|
|
self.medium_deg = 6.0
|
|
self.medium_rad = self.rad_per_deg * self.medium_deg
|
|
self.medium_translate = 0.04
|
|
self.big_deg = 12.0
|
|
self.big_rad = self.rad_per_deg * self.big_deg
|
|
self.big_translate = 0.06
|
|
|
|
def get_deltas(self):
|
|
if self.step_size == 'small':
|
|
deltas = {'rad': self.small_rad, 'translate': self.small_translate}
|
|
if self.step_size == 'medium':
|
|
deltas = {'rad': self.medium_rad, 'translate': self.medium_translate}
|
|
if self.step_size == 'big':
|
|
deltas = {'rad': self.big_rad, 'translate': self.big_translate}
|
|
return deltas
|
|
|
|
def print_commands(self, joint_state, command):
|
|
if command is None:
|
|
return
|
|
|
|
joints = joint_state.name
|
|
|
|
def in_joints(i):
|
|
return len(list(set(i) & set(joints))) > 0
|
|
|
|
print('---------- KEYBOARD TELEOP MENU -----------')
|
|
print(' ')
|
|
if in_joints(['joint_head_tilt']):
|
|
print(' i HEAD UP ')
|
|
if in_joints(['joint_head_pan']):
|
|
print(' j HEAD LEFT l HEAD RIGHT ')
|
|
if in_joints(['joint_head_tilt']):
|
|
print(' , HEAD DOWN ')
|
|
print(' ')
|
|
print(' ')
|
|
print(' 7 BASE ROTATE LEFT 9 BASE ROTATE RIGHT')
|
|
print(' home page-up ')
|
|
print(' ')
|
|
print(' ')
|
|
if in_joints(['joint_lift']):
|
|
print(' 8 LIFT UP ')
|
|
print(' up-arrow ')
|
|
print(' 4 BASE FORWARD 6 BASE BACK ')
|
|
print(' left-arrow right-arrow ')
|
|
if in_joints(['joint_lift']):
|
|
print(' 2 LIFT DOWN ')
|
|
print(' down-arrow ')
|
|
print(' ')
|
|
print(' ')
|
|
if in_joints(['joint_arm_l0', 'joint_arm_l1', 'joint_arm_l2', 'joint_arm_l3']):
|
|
print(' w ARM OUT ')
|
|
if in_joints(['joint_wrist_yaw']):
|
|
print(' a WRIST FORWARD d WRIST BACK ')
|
|
if in_joints(['joint_arm_l0', 'joint_arm_l1', 'joint_arm_l2', 'joint_arm_l3']):
|
|
print(' x ARM IN ')
|
|
if in_joints(['joint_wrist_pitch', 'joint_wrist_roll']):
|
|
print(' ')
|
|
print(' ')
|
|
if in_joints(['joint_wrist_pitch']):
|
|
print(' c PITCH FORWARD v PITCH BACK ')
|
|
if in_joints(['joint_wrist_roll']):
|
|
print(' o ROLL FORWARD p ROLL BACK ')
|
|
print(' ')
|
|
print(' ')
|
|
if in_joints(['joint_gripper_finger_left', 'joint_gripper_finger_right', 'gripper_aperture']):
|
|
print(' 5 GRIPPER CLOSE ')
|
|
print(' 0 GRIPPER OPEN ')
|
|
print(' ')
|
|
print(' step size: b BIG, m MEDIUM, s SMALL ')
|
|
print(' ')
|
|
print(' q QUIT ')
|
|
print(' ')
|
|
print('-------------------------------------------')
|
|
|
|
def get_command(self, node):
|
|
command = None
|
|
|
|
c = kb.getch()
|
|
|
|
####################################################
|
|
## BASIC KEYBOARD TELEOPERATION COMMANDS
|
|
####################################################
|
|
|
|
# 8 or up arrow
|
|
if c == '8' or c == '\x1b[A':
|
|
command = {'joint': 'joint_lift', 'delta': self.get_deltas()['translate']}
|
|
# 2 or down arrow
|
|
if c == '2' or c == '\x1b[B':
|
|
command = {'joint': 'joint_lift', 'delta': -self.get_deltas()['translate']}
|
|
|
|
# 4 or left arrow
|
|
if c == '4' or c == '\x1b[D':
|
|
command = {'joint': 'translate_mobile_base', 'inc': self.get_deltas()['translate']}
|
|
# 6 or right arrow
|
|
if c == '6' or c == '\x1b[C':
|
|
command = {'joint': 'translate_mobile_base', 'inc': -self.get_deltas()['translate']}
|
|
# 1 or end key
|
|
if c == '7' or c == '\x1b[H':
|
|
command = {'joint': 'rotate_mobile_base', 'inc': self.get_deltas()['rad']}
|
|
# 3 or pg down 5~
|
|
if c == '9' or c == '\x1b[5':
|
|
command = {'joint': 'rotate_mobile_base', 'inc': -self.get_deltas()['rad']}
|
|
|
|
if c == 'w' or c == 'W':
|
|
command = {'joint': 'wrist_extension', 'delta': self.get_deltas()['translate']}
|
|
if c == 'x' or c == 'X':
|
|
command = {'joint': 'wrist_extension', 'delta': -self.get_deltas()['translate']}
|
|
if c == 'd' or c == 'D':
|
|
command = {'joint': 'joint_wrist_yaw', 'delta': -self.get_deltas()['rad']}
|
|
if c == 'a' or c == 'A':
|
|
command = {'joint': 'joint_wrist_yaw', 'delta': self.get_deltas()['rad']}
|
|
if c == 'v' or c == 'V':
|
|
command = {'joint': 'joint_wrist_pitch', 'delta': -self.get_deltas()['rad']}
|
|
if c == 'c' or c == 'C':
|
|
command = {'joint': 'joint_wrist_pitch', 'delta': self.get_deltas()['rad']}
|
|
if c == 'p' or c == 'P':
|
|
command = {'joint': 'joint_wrist_roll', 'delta': -self.get_deltas()['rad']}
|
|
if c == 'o' or c == 'O':
|
|
command = {'joint': 'joint_wrist_roll', 'delta': self.get_deltas()['rad']}
|
|
if c == '5' or c == '\x1b[E' or c == 'g' or c == 'G':
|
|
# grasp
|
|
command = {'joint': 'joint_gripper_finger_left', 'delta': -self.get_deltas()['rad']}
|
|
if c == '0' or c == '\x1b[2' or c == 'r' or c == 'R':
|
|
# release
|
|
command = {'joint': 'joint_gripper_finger_left', 'delta': self.get_deltas()['rad']}
|
|
if c == 'i' or c == 'I':
|
|
command = {'joint': 'joint_head_tilt', 'delta': (2.0 * self.get_deltas()['rad'])}
|
|
if c == ',' or c == '<':
|
|
command = {'joint': 'joint_head_tilt', 'delta': -(2.0 * self.get_deltas()['rad'])}
|
|
if c == 'j' or c == 'J':
|
|
command = {'joint': 'joint_head_pan', 'delta': (2.0 * self.get_deltas()['rad'])}
|
|
if c == 'l' or c == 'L':
|
|
command = {'joint': 'joint_head_pan', 'delta': -(2.0 * self.get_deltas()['rad'])}
|
|
if c == 'b' or c == 'B':
|
|
rospy.loginfo('process_keyboard.py: changing to BIG step size')
|
|
self.step_size = 'big'
|
|
if c == 'm' or c == 'M':
|
|
rospy.loginfo('process_keyboard.py: changing to MEDIUM step size')
|
|
self.step_size = 'medium'
|
|
if c == 's' or c == 'S':
|
|
rospy.loginfo('process_keyboard.py: changing to SMALL step size')
|
|
self.step_size = 'small'
|
|
if c == 'q' or c == 'Q':
|
|
rospy.loginfo('keyboard_teleop exiting...')
|
|
rospy.signal_shutdown('Received quit character (q), so exiting')
|
|
|
|
####################################################
|
|
|
|
return command
|
|
|
|
|
|
class KeyboardTeleopNode:
|
|
|
|
def __init__(self):
|
|
|
|
self.keys = GetKeyboardCommands()
|
|
self.rate = 10.0
|
|
self.joint_state = None
|
|
self.twist = Twist()
|
|
self.dex_wrist_control = False
|
|
|
|
def joint_states_callback(self, joint_state):
|
|
self.joint_state = joint_state
|
|
|
|
def send_command(self, command):
|
|
joint_state = self.joint_state
|
|
self.trajectory_client_selector(command)
|
|
if (joint_state is not None) and (command is not None):
|
|
if 'translate_mobile_base' == command['joint'] or 'rotate_mobile_base' == command['joint']:
|
|
self.cmd_vel_pub.publish(self.twist)
|
|
return 0
|
|
point = JointTrajectoryPoint()
|
|
point.time_from_start = rospy.Duration(0.2)
|
|
trajectory_goal = FollowJointTrajectoryGoal()
|
|
trajectory_goal.goal_time_tolerance = rospy.Time(1.0)
|
|
|
|
joint_name = command['joint']
|
|
if joint_name in ['joint_lift', 'joint_wrist_yaw', 'joint_wrist_roll', 'joint_wrist_pitch', 'joint_head_pan', 'joint_head_tilt']:
|
|
trajectory_goal.trajectory.joint_names = [joint_name]
|
|
joint_index = joint_state.name.index(joint_name)
|
|
joint_value = joint_state.position[joint_index]
|
|
delta = command['delta']
|
|
new_value = joint_value + delta
|
|
point.positions = [new_value]
|
|
elif joint_name in ["joint_gripper_finger_left", "wrist_extension"]:
|
|
if joint_name == "joint_gripper_finger_left":
|
|
trajectory_goal.trajectory.joint_names = ['joint_gripper_finger_left', 'joint_gripper_finger_right']
|
|
else:
|
|
trajectory_goal.trajectory.joint_names = ['joint_arm_l0','joint_arm_l1', 'joint_arm_l2', 'joint_arm_l3']
|
|
positions = []
|
|
for j_name in trajectory_goal.trajectory.joint_names:
|
|
joint_index = joint_state.name.index(j_name)
|
|
joint_value = joint_state.position[joint_index]
|
|
delta = command['delta']
|
|
new_value = joint_value + delta/len(trajectory_goal.trajectory.joint_names)
|
|
positions.append(new_value)
|
|
point.positions = positions
|
|
|
|
trajectory_goal.trajectory.points = [point]
|
|
trajectory_goal.trajectory.header.stamp = rospy.Time.now()
|
|
if self.trajectory_client:
|
|
self.trajectory_client.send_goal(trajectory_goal)
|
|
self.trajectory_client.wait_for_result()
|
|
|
|
def trajectory_client_selector(self, command):
|
|
self.trajectory_client = None
|
|
self.twist.linear.x = 0
|
|
self.twist.angular.z = 0
|
|
try:
|
|
joint = command['joint']
|
|
if joint == 'joint_lift' or joint == 'joint_wrist_yaw' or joint == 'wrist_extension':
|
|
self.trajectory_client = self.trajectory_arm_client
|
|
if joint == 'joint_head_pan' or joint == 'joint_head_tilt':
|
|
self.trajectory_client = self.trajectory_head_client
|
|
if joint == 'joint_gripper_finger_right' or joint == 'joint_gripper_finger_left':
|
|
self.trajectory_client = self.trajectory_gripper_client
|
|
if self.dex_wrist_control:
|
|
if joint == 'joint_wrist_roll' or joint == 'joint_wrist_pitch':
|
|
self.trajectory_client = self.trajectory_dex_wrist_client
|
|
if joint == 'translate_mobile_base' or joint == 'rotate_mobile_base':
|
|
if joint == 'translate_mobile_base':
|
|
if 'inc' in command:
|
|
self.twist.linear.x = command['inc']
|
|
else:
|
|
self.twist.linear.x = command['delta']
|
|
else:
|
|
if 'inc' in command:
|
|
self.twist.angular.z = command['inc']
|
|
else:
|
|
self.twist.angular.z = command['delta']
|
|
except TypeError:
|
|
0
|
|
|
|
def main(self):
|
|
rospy.init_node('keyboard_teleop_gazebo')
|
|
try:
|
|
self.dex_wrist_control = rospy.get_param('stretch_gazebo/dex_wrist', default=False)
|
|
except KeyError:
|
|
self.dex_wrist_control = False
|
|
|
|
self.trajectory_gripper_client = actionlib.SimpleActionClient(
|
|
'/stretch_gripper_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
|
|
server_reached = self.trajectory_gripper_client.wait_for_server(timeout=rospy.Duration(60.0))
|
|
|
|
self.trajectory_head_client = actionlib.SimpleActionClient('/stretch_head_controller/follow_joint_trajectory',
|
|
FollowJointTrajectoryAction)
|
|
server_reached = self.trajectory_head_client.wait_for_server(timeout=rospy.Duration(60.0))
|
|
|
|
self.trajectory_arm_client = actionlib.SimpleActionClient('/stretch_arm_controller/follow_joint_trajectory',
|
|
FollowJointTrajectoryAction)
|
|
server_reached = self.trajectory_arm_client.wait_for_server(timeout=rospy.Duration(60.0))
|
|
|
|
if self.dex_wrist_control:
|
|
rospy.loginfo("Dex Wrist Control Activated")
|
|
self.trajectory_dex_wrist_client = actionlib.SimpleActionClient('/stretch_dex_wrist_controller/follow_joint_trajectory',
|
|
FollowJointTrajectoryAction)
|
|
server_reached = self.trajectory_dex_wrist_client.wait_for_server(timeout=rospy.Duration(60.0))
|
|
|
|
self.cmd_vel_pub = rospy.Publisher('/stretch_diff_drive_controller/cmd_vel', Twist, queue_size=10)
|
|
|
|
rospy.Subscriber('/joint_states', JointState, self.joint_states_callback)
|
|
|
|
rate = rospy.Rate(self.rate)
|
|
|
|
command = 1 # set equal to not None, so menu is printed out on first loop
|
|
while not rospy.is_shutdown():
|
|
if self.joint_state is not None:
|
|
self.keys.print_commands(self.joint_state, command)
|
|
command = self.keys.get_command(self)
|
|
self.send_command(command)
|
|
rate.sleep()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
try:
|
|
node = KeyboardTeleopNode()
|
|
node.main()
|
|
except KeyboardInterrupt:
|
|
rospy.loginfo('interrupt received, so shutting down')
|