def main(): keyboard_cmd_listener = rospy.Subscriber("key_commands", String, callback_keyboard_cmd) speech_listener = rospy.Subscriber("/speech/output", String, callback_speech) rospy.init_node('amigo_demo_executioner') global dictionary if rospy.has_param("/nl"): dictionary['nl'] = rospy.get_param("/nl") else: dictionary['nl'] = sentences if rospy.has_param("/en"): dictionary['en'] = rospy.get_param("/en") if rospy.has_param("/language"): global language language = rospy.get_param("/language") print mesg print_keys() global robot #~ if len(sys.argv) > 1: #~ from test_tools.build_amigo import build_amigo #~ robot = build_amigo(fake=['base','arms','perception','head', 'worldmodel']) #~ else: robot = Amigo(dontInclude=[ "reasoner" ]) #dontInclude=['perception'] is needed for grabbing items robot.lights.set_color(0, 0, 1) #import pdb; pdb.set_trace() rospy.spin()
transitions={ 'done': 'RESET', 'continue': 'RESET' }) smach.StateMachine.add('RESET', states.ResetED(robot), transitions={'done': 'WAIT_SAY'}) return sm if __name__ == '__main__': rospy.init_node('automatic_side_detection') from robot_skills.amigo import Amigo robot = Amigo() robot.ed.reset() sm = smach.StateMachine(outcomes=['done', 'aborted']) with sm: smach.StateMachine.add('STORE_WAYPOINT', StoreWaypoint(robot, "kitchen"), transitions={ 'done': 'RESET', 'continue': 'RESET' }) smach.StateMachine.add('RESET', states.ResetED(robot), transitions={'done': 'done'}) # states.startup(setup_statemachine, challenge_name="automatic_side_detection")
def main(): rospy.init_node("demo") random.seed() skip = rospy.get_param('~skip', False) restart = rospy.get_param('~restart', False) robot_name = rospy.get_param('~robot_name') no_of_tasks = rospy.get_param('~number_of_tasks', 0) time_limit = rospy.get_param('~time_limit', 0) if no_of_tasks == 0: no_of_tasks = 999 rospy.loginfo("[DEMO] Parameters:") rospy.loginfo("[DEMO] robot_name = {}".format(robot_name)) if skip: rospy.loginfo("[DEMO] skip = {}".format(skip)) if no_of_tasks: rospy.loginfo("[DEMO] number_of_tasks = {}".format(no_of_tasks)) if restart: rospy.loginfo("[DEMO] running a restart") # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - if robot_name == 'amigo': from robot_skills.amigo import Amigo as Robot elif robot_name == 'sergio': from robot_skills.sergio import Sergio as Robot elif robot_name == 'hero': from robot_skills.hero import Hero as Robot else: raise ValueError('unknown robot') robot = Robot() action_client = ActionClient(robot.robot_name) knowledge = load_knowledge('challenge_demo') no_of_tasks_performed = 0 user_instruction = "What can I do for you?" report = "" # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - # Start if not skip and not restart: # Wait for door, enter arena s = StartChallengeRobust(robot, knowledge.initial_pose) s.execute() # Move to the start location robot.speech.speak("Let's see if my operator has a task for me!", block=False) if restart: robot.speech.speak( "Performing a restart. So sorry about that last time!", block=False) # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - finished = False start_time = rospy.get_time() trigger = WaitForTrigger(robot, ["gpsr"], "/" + robot_name + "/trigger") while True: # Navigate to the GPSR meeting point if not skip: robot.speech.speak("Moving to the meeting point.", block=False) nwc = NavigateToWaypoint(robot=robot, waypoint_designator=EntityByIdDesignator( robot=robot, id=knowledge.starting_pose), radius=0.3) nwc.execute() # Report to the user and ask for a new task # Report to the user robot.head.look_at_standing_person() robot.speech.speak(report, block=True) timeout_count = 0 while True: rospy.loginfo("Waiting for trigger") trigger.execute() base_loc = robot.base.get_location() base_pose = base_loc.frame print base_pose location_id = "starting_point" robot.ed.update_entity(id=location_id, frame_stamped=FrameStamped( base_pose, "/map"), type="waypoint") robot.head.look_at_standing_person() robot.speech.speak(user_instruction, block=True) # Listen for the new task while True: try: sentence, semantics = robot.hmi.query( description="", grammar=knowledge.grammar, target=knowledge.grammar_target) timeout_count = 0 break except hmi.TimeoutException: robot.speech.speak( random.sample(knowledge.not_understood_sentences, 1)[0]) if timeout_count >= 3: robot.hmi.restart_dragonfly() timeout_count = 0 rospy.logwarn("[GPSR] Dragonfly restart") else: timeout_count += 1 rospy.logwarn( "[GPSR] Timeout_count: {}".format(timeout_count)) # check if we have heard this correctly robot.speech.speak('I heard %s, is this correct?' % sentence) try: if 'no' == robot.hmi.query('', 'T -> yes | no', 'T').sentence: robot.speech.speak('Sorry') continue except hmi.TimeoutException: # robot did not hear the confirmation, so lets assume its correct break break # Dump the output json object to a string task_specification = json.dumps(semantics) # Send the task specification to the action server task_result = action_client.send_task(task_specification) print(task_result.missing_field) # # Ask for missing information # while task_result.missing_field: # request_missing_field(knowledge.task_result.missing_field) # task_result = action_client.send_task(task_specification) # Write a report to bring to the operator report = task_result_to_report(task_result) robot.lights.set_color(0, 0, 1) # be sure lights are blue robot.head.look_at_standing_person() robot.reset_all_arms(gripper_timeout=0.0) robot.torso.reset() rospy.loginfo("Driving back to the starting point") nwc = NavigateToWaypoint(robot=robot, waypoint_designator=EntityByIdDesignator( robot=robot, id=location_id), radius=0.3) nwc.execute()
return ActionResult(ActionResult.FAILED, "Amigo: Failed to move arm to reset position") amigo.rightArm.wait_for_motion_done() return ActionResult(ActionResult.SUCCEEDED, "Amigo: Reset arm succeeded") if __name__ == "__main__": side = "left" if len(sys.argv) > 1: if sys.argv[1] == "--side": side = sys.argv[2] """ Test stuff """ rospy.init_node("Test handover motions: Amigo") amigo = Amigo(wait_services=True) amigo_reset_arm(amigo, "left") amigo_reset_arm(amigo, "right") rospy.loginfo("AMIGO is loaded and will move to the pre-handover pose") result = amigo_navigate_to_handover_pose(amigo, side) rospy.loginfo("{0}".format(result.message)) rospy.loginfo("AMIGO will move its arm to the place position") result = amigo_move_arm_to_place_position(amigo, side) raw_input("Press enter when SERGIO has its tray under amigo's gripper") result = amigo_place(amigo, side) rospy.loginfo("{0}".format(result.message))