diff --git a/stretch_gazebo/nodes/keyboard.py b/stretch_gazebo/nodes/keyboard.py new file mode 100644 index 0000000..a9e7a8d --- /dev/null +++ b/stretch_gazebo/nodes/keyboard.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python + +from __future__ import print_function + +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') + diff --git a/stretch_gazebo/nodes/keyboard_teleop_gazebo b/stretch_gazebo/nodes/keyboard_teleop_gazebo new file mode 100755 index 0000000..ee11326 --- /dev/null +++ b/stretch_gazebo/nodes/keyboard_teleop_gazebo @@ -0,0 +1,511 @@ +#!/usr/bin/env python +from __future__ import print_function + +import math +from ntpath import join +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 numpy as np +# import hello_helpers.hello_misc as hm +import actionlib + + +class GetKeyboardCommands: + # Note : Currently Planning services are not compatible with Stretch Gazebo Version + def __init__(self, mapping_on, hello_world_on, open_drawer_on, clean_surface_on, grasp_object_on, + deliver_object_on): + self.mapping_on = mapping_on + self.hello_world_on = hello_world_on + self.open_drawer_on = open_drawer_on + self.clean_surface_on = clean_surface_on + self.grasp_object_on = grasp_object_on + self.deliver_object_on = deliver_object_on + + self.step_size = 'medium' + # self.persistent_command_count = 0 + # self.prev_persistent_c = None + # self.persistent_start_s = time.time() + # self.max_persistent_delay_s = 1.0 + 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() + # rospy.loginfo('c =', c) + + #################################################### + ## MOSTLY MAPPING RELATED CAPABILITIES + ## (There are non-mapping outliers.) + #################################################### + + # Sequential performs a fixed number of autonomus mapping iterations + if (c == '!') and self.mapping_on: + number_iterations = 4 + for n in range(number_iterations): + # Trigger a 3D scan with the D435i + trigger_request = TriggerRequest() + trigger_result = node.trigger_head_scan_service(trigger_request) + rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + + # Trigger driving the robot to the estimated next best place to scan + trigger_request = TriggerRequest() + trigger_result = node.trigger_drive_to_scan_service(trigger_request) + rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + + # Trigger localizing the robot to a new pose anywhere on the current map + if ((c == '+') or (c == '=')) and self.mapping_on: + trigger_request = TriggerRequest() + trigger_result = node.trigger_global_localization_service(trigger_request) + rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + + # Trigger localizing the robot to a new pose that is near its current pose on the map + if ((c == '-') or (c == '_')) and self.mapping_on: + trigger_request = TriggerRequest() + trigger_result = node.trigger_local_localization_service(trigger_request) + rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + + # Trigger driving the robot to the estimated next best place to perform a 3D scan + if ((c == '\\') or (c == '|')) and self.mapping_on: + trigger_request = TriggerRequest() + trigger_result = node.trigger_drive_to_scan_service(trigger_request) + rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + + # Trigger performing a 3D scan using the D435i + if (c == ' ') and self.mapping_on: + trigger_request = TriggerRequest() + trigger_result = node.trigger_head_scan_service(trigger_request) + rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + + # Trigger rotating the mobile base to align with the nearest 3D cliff detected visually + if ((c == '[') or (c == '{')) and self.mapping_on: + trigger_request = TriggerRequest() + trigger_result = node.trigger_align_with_nearest_cliff_service(trigger_request) + rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + + # DEPRECATED: Trigger extend arm until contact + if ((c == ']') or (c == '}')) and self.mapping_on: + trigger_request = TriggerRequest() + trigger_result = node.trigger_reach_until_contact_service(trigger_request) + rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + + # DEPRECATED: Trigger lower arm until contact + if ((c == ':') or (c == ';')) and self.mapping_on: + trigger_request = TriggerRequest() + trigger_result = node.trigger_lower_until_contact_service(trigger_request) + rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + + #################################################### + ## OTHER CAPABILITIES + #################################################### + + # Trigger Hello World whiteboard writing demo + if ((c == '`') or (c == '~')) and self.hello_world_on: + trigger_request = TriggerRequest() + trigger_result = node.trigger_write_hello_service(trigger_request) + rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + + # Trigger open drawer demo with downward hook motion + if ((c == 'z') or (c == 'Z')) and self.open_drawer_on: + trigger_request = TriggerRequest() + trigger_result = node.trigger_open_drawer_down_service(trigger_request) + rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + + # Trigger open drawer demo with upward hook motion + if ((c == '.') or (c == '>')) and self.open_drawer_on: + trigger_request = TriggerRequest() + trigger_result = node.trigger_open_drawer_up_service(trigger_request) + rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + + # Trigger clean surface demo + if ((c == '/') or (c == '?')) and self.clean_surface_on: + trigger_request = TriggerRequest() + trigger_result = node.trigger_clean_surface_service(trigger_request) + rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + + # Trigger grasp object demo + if ((c == '\'') or (c == '\"')) and self.grasp_object_on: + trigger_request = TriggerRequest() + trigger_result = node.trigger_grasp_object_service(trigger_request) + rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + + # Trigger deliver object demo + if ((c == 'y') or (c == 'Y')) and self.deliver_object_on: + trigger_request = TriggerRequest() + trigger_result = node.trigger_deliver_object_service(trigger_request) + rospy.loginfo('trigger_result = {0}'.format(trigger_result)) + + #################################################### + ## 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, mapping_on=False, hello_world_on=False, open_drawer_on=False, clean_surface_on=False, + grasp_object_on=False, deliver_object_on=False): + # hm.HelloNode.__init__(self) + self.keys = GetKeyboardCommands(mapping_on, hello_world_on, open_drawer_on, clean_surface_on, grasp_object_on, + deliver_object_on) + self.rate = 10.0 + self.joint_state = None + self.mapping_on = mapping_on + self.hello_world_on = hello_world_on + self.open_drawer_on = open_drawer_on + self.clean_surface_on = clean_surface_on + self.grasp_object_on = grasp_object_on + self.deliver_object_on = deliver_object_on + 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_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') + # hm.HelloNode.main(self, 'keyboard_teleop', 'keyboard_teleop', wait_for_first_pointcloud=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)) + + self.cmd_vel_pub = rospy.Publisher('/stretch_diff_drive_controller/cmd_vel', Twist, queue_size=10) + + if self.mapping_on: + rospy.loginfo('Node ' + self.node_name + ' waiting to connect to /funmap/trigger_head_scan.') + + rospy.wait_for_service('/funmap/trigger_head_scan') + rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_head_scan.') + self.trigger_head_scan_service = rospy.ServiceProxy('/funmap/trigger_head_scan', Trigger) + + rospy.wait_for_service('/funmap/trigger_drive_to_scan') + rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_drive_to_scan.') + self.trigger_drive_to_scan_service = rospy.ServiceProxy('/funmap/trigger_drive_to_scan', Trigger) + + rospy.wait_for_service('/funmap/trigger_global_localization') + rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_global_localization.') + self.trigger_global_localization_service = rospy.ServiceProxy('/funmap/trigger_global_localization', + Trigger) + + rospy.wait_for_service('/funmap/trigger_local_localization') + rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_local_localization.') + self.trigger_local_localization_service = rospy.ServiceProxy('/funmap/trigger_local_localization', Trigger) + + rospy.wait_for_service('/funmap/trigger_align_with_nearest_cliff') + rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_align_with_nearest_cliff.') + self.trigger_align_with_nearest_cliff_service = rospy.ServiceProxy( + '/funmap/trigger_align_with_nearest_cliff', Trigger) + + rospy.wait_for_service('/funmap/trigger_reach_until_contact') + rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_reach_until_contact.') + self.trigger_reach_until_contact_service = rospy.ServiceProxy('/funmap/trigger_reach_until_contact', + Trigger) + + rospy.wait_for_service('/funmap/trigger_lower_until_contact') + rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_lower_until_contact.') + self.trigger_lower_until_contact_service = rospy.ServiceProxy('/funmap/trigger_lower_until_contact', + Trigger) + + if self.hello_world_on: + rospy.wait_for_service('/hello_world/trigger_write_hello') + rospy.loginfo('Node ' + self.node_name + ' connected to /hello_world/trigger_write_hello.') + self.trigger_write_hello_service = rospy.ServiceProxy('/hello_world/trigger_write_hello', Trigger) + + if self.open_drawer_on: + rospy.wait_for_service('/open_drawer/trigger_open_drawer_down') + rospy.loginfo('Node ' + self.node_name + ' connected to /open_drawer/trigger_open_drawer_down.') + self.trigger_open_drawer_down_service = rospy.ServiceProxy('/open_drawer/trigger_open_drawer_down', Trigger) + + rospy.wait_for_service('/open_drawer/trigger_open_drawer_up') + rospy.loginfo('Node ' + self.node_name + ' connected to /open_drawer/trigger_open_drawer_up.') + self.trigger_open_drawer_up_service = rospy.ServiceProxy('/open_drawer/trigger_open_drawer_up', Trigger) + + if self.clean_surface_on: + rospy.wait_for_service('/clean_surface/trigger_clean_surface') + rospy.loginfo('Node ' + self.node_name + ' connected to /clean_surface/trigger_clean_surface.') + self.trigger_clean_surface_service = rospy.ServiceProxy('/clean_surface/trigger_clean_surface', Trigger) + + if self.grasp_object_on: + rospy.wait_for_service('/grasp_object/trigger_grasp_object') + rospy.loginfo('Node ' + self.node_name + ' connected to /grasp_object/trigger_grasp_object.') + self.trigger_grasp_object_service = rospy.ServiceProxy('/grasp_object/trigger_grasp_object', Trigger) + + if self.deliver_object_on: + rospy.wait_for_service('/deliver_object/trigger_deliver_object') + rospy.loginfo('Node ' + self.node_name + ' connected to /deliver_object/trigger_deliver_object.') + self.trigger_deliver_object_service = rospy.ServiceProxy('/deliver_object/trigger_deliver_object', Trigger) + + 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: + parser = ap.ArgumentParser(description='Keyboard teleoperation for stretch.') + parser.add_argument('--mapping_on', action='store_true', + help='Turn on mapping control. For example, the space bar will trigger a head scan. This requires that the mapping node be run (funmap).') + parser.add_argument('--hello_world_on', action='store_true', + help='Enable Hello World writing trigger, which requires connection to the appropriate hello_world service.') + parser.add_argument('--open_drawer_on', action='store_true', + help='Enable Open Drawer trigger, which requires connection to the appropriate open_drawer service.') + parser.add_argument('--clean_surface_on', action='store_true', + help='Enable Clean Surface trigger, which requires connection to the appropriate clean_surface service.') + parser.add_argument('--grasp_object_on', action='store_true', + help='Enable Grasp Object trigger, which requires connection to the appropriate grasp_object service.') + parser.add_argument('--deliver_object_on', action='store_true', + help='Enable Deliver Object trigger, which requires connection to the appropriate deliver_object service.') + + args, unknown = parser.parse_known_args() + mapping_on = args.mapping_on + hello_world_on = args.hello_world_on + open_drawer_on = args.open_drawer_on + clean_surface_on = args.clean_surface_on + grasp_object_on = args.grasp_object_on + deliver_object_on = args.deliver_object_on + + node = KeyboardTeleopNode(mapping_on, hello_world_on, open_drawer_on, clean_surface_on, grasp_object_on, + deliver_object_on) + node.main() + except KeyboardInterrupt: + rospy.loginfo('interrupt received, so shutting down')