diff --git a/stretch_core/nodes/keyboard_teleop b/stretch_core/nodes/keyboard_teleop index 8354486..fd1eac4 100755 --- a/stretch_core/nodes/keyboard_teleop +++ b/stretch_core/nodes/keyboard_teleop @@ -89,54 +89,95 @@ class GetKeyboardCommands: 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_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 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)) + #################################################### + + + # 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)