cccccccc
/
stretch_ros
mirror of https://github.com/hello-robot/stretch_ros.git
1
0
Fork 0
Code Issues 0 Projects 0 Releases 0 Wiki Activity
Browse Source

Added keyboard teleop node for Gazebo

pull/90/head
Mohamed Fazil 1 year ago
parent
fc826fc300
commit
b4c0331de4
2 changed files with 344 additions and 0 deletions
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. +44
    -0
      stretch_gazebo/nodes/keyboard.py
  2. +300
    -0
      stretch_gazebo/nodes/keyboard_teleop_gazebo

+ 44
- 0
stretch_gazebo/nodes/keyboard.py View File

@ -0,0 +1,44 @@
#!/usr/bin/env python3
import sys, tty, termios
# This allows Ctrl-C and related keyboard commands to still function.
# It detects escape codes in order to recognize arrow keys. It also
# has buffer flushing to avoid repeated commands. This is
# blocking code.
def getch():
stdin_fd = 0
# "Return a list containing the tty attributes for file descriptor
# fd, as follows: [iflag, oflag, cflag, lflag, ispeed, ospeed, cc]"
# from https://docs.python.org/2/library/termios.html
original_tty_attributes = termios.tcgetattr(stdin_fd)
new_tty_attributes = termios.tcgetattr(stdin_fd)
# Change the lflag (local modes) to turn off canonical mode
new_tty_attributes[3] &= ~termios.ICANON
# Set VMIN = 0 and VTIME > 0 for a timed read, as explained in:
# http://unixwiz.net/techtips/termios-vmin-vtime.html
new_tty_attributes[6][termios.VMIN] = b'\x00'
new_tty_attributes[6][termios.VTIME] = b'\x01'
try:
termios.tcsetattr(stdin_fd, termios.TCSAFLUSH, new_tty_attributes)
ch1 = sys.stdin.read(1)
if ch1 == '\x1b':
# special key pressed
ch2 = sys.stdin.read(1)
ch3 = sys.stdin.read(1)
ch = ch1 + ch2 + ch3
else:
# not a special key
ch = ch1
finally:
termios.tcsetattr(stdin_fd, termios.TCSAFLUSH, original_tty_attributes)
return ch
if __name__ == '__main__':
while True:
c = getch()
print('c')

+ 300
- 0
stretch_gazebo/nodes/keyboard_teleop_gazebo View File

@ -0,0 +1,300 @@
#!/usr/bin/env python
from __future__ import print_function
import math
import keyboard as kb
import argparse as ap
import rospy
from std_srvs.srv import Trigger, TriggerRequest
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
self.mode = 'position' # 'manipulation' #'navigation'
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']}
if self.mode == 'manipulation':
# 4 or left arrow
if c == '4' or c == '\x1b[D':
command = {'joint': 'joint_mobile_base_translation', 'delta': self.get_deltas()['translate']}
# 6 or right arrow
if c == '6' or c == '\x1b[C':
command = {'joint': 'joint_mobile_base_translation', 'delta': -self.get_deltas()['translate']}
elif self.mode == 'position':
# 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']}
elif self.mode == 'navigation':
rospy.loginfo('ERROR: Navigation mode is not currently supported.')
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()
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_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()
self.trajectory_client.send_goal(trajectory_goal)
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 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')
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))
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')

Write Preview
Loading…
Cancel
Save