def task_finished(self, message): # Navigate to the GPSR meeting point self.finished = rospy.get_time() - self.start_time > (60 * self.time_limit - 45) and self.tasks_done >= 1 if not self.skip and not self.finished: self.robot.speech.speak("Moving to the meeting point.", block=False) nwc = NavigateToWaypoint(robot=self.robot, waypoint_designator=EntityByIdDesignator(robot=self.robot, id=self.knowledge.starting_pose), radius=0.3) nwc.execute() # Report to the user and ask for a new task # Report to the user self.robot.head.look_at_standing_person() self._say_to_user(message) self._say_to_user("I performed {} {} so far, still going strong!".format(self.tasks_done, "tasks" if self.tasks_done == 1 else "tasks")) self.timeout_count = 0 self._start_wait_for_command(self.knowledge.grammar, self.knowledge.grammar_target) else: rospy.logwarn("Not going to meeting point, challenge has param skip:=true set") if self.tasks_done >= self.tasks_to_be_done and not self.skip: nwc = NavigateToWaypoint(robot=self.robot, waypoint_designator=EntityByIdDesignator(robot=self.robot, id=self.knowledge.exit_waypoint), radius=0.3) self.robot.speech.speak("I'm done now. Thank you very much, and goodbye!", block=True) nwc.execute() return
def task_finished(self, message): # Navigate to the GPSR meeting point self.finished = rospy.get_time() - self.start_time > ( 60 * self.time_limit - 45) and self.tasks_done >= 1 if not self.skip and not self.finished: self.robot.speech.speak("Moving to the meeting point.", block=False) nwc = NavigateToWaypoint(robot=self.robot, waypoint_designator=EntityByIdDesignator( robot=self.robot, id=self.knowledge.starting_pose), radius=0.3) nwc.execute() # Report to the user and ask for a new task # Report to the user self.robot.head.look_at_standing_person() self._say_to_user(message) self._say_to_user( "I performed {} {} so far, still going strong!".format( self.tasks_done, "tasks" if self.tasks_done == 1 else "tasks")) self.timeout_count = 0 self._start_wait_for_command(self.knowledge.grammar, self.knowledge.grammar_target) else: rospy.logwarn( "Not going to meeting point, challenge has param skip:=true set" ) if (self.tasks_done >= self.tasks_to_be_done or self.finished) and not self.skip: nwc = NavigateToWaypoint(robot=self.robot, waypoint_designator=EntityByIdDesignator( robot=self.robot, id=self.knowledge.exit_waypoint), radius=0.3) self.robot.speech.speak( "I'm done now. Thank you very much, and goodbye!", block=True) nwc.execute() return
def setup_statemachine(robot): state_machine = StateMachine(outcomes=['done']) furniture_designator = ds.VariableDesignator(resolve_type=Entity) entity_designator = ds.VariableDesignator(resolve_type=Entity) arm_designator = ds.UnoccupiedArmDesignator(robot, {}) with state_machine: # Intro StateMachine.add('START_CHALLENGE_ROBUST', StartChallengeRobust(robot, STARTING_POINT), transitions={'Done': 'SAY_START', 'Aborted': 'done', 'Failed': 'SAY_START'}) # Say we're gonna start StateMachine.add('SAY_START', Say(robot, "Hand me that it is!", block=False), transitions={'spoken': 'NAVIGATE_TO_START'}) # Drive to the start location StateMachine.add('NAVIGATE_TO_START', NavigateToWaypoint(robot, ds.EdEntityDesignator(robot, id=HOME_LOCATION)), transitions={'arrived': 'GET_FURNITURE_FROM_OPERATOR_POSE', 'unreachable': 'NAVIGATE_TO_START', # ToDo: other fallback 'goal_not_defined': 'done'}) # I'm not even going to fill this in # The pre-work StateMachine.add('GET_FURNITURE_FROM_OPERATOR_POSE', GetFurnitureFromOperatorPose(robot, furniture_designator.writeable), transitions={'done': 'INSPECT_FURNITURE'}) # Go to the furniture object that was pointing to see what's there StateMachine.add('INSPECT_FURNITURE', InspectFurniture(robot, furniture_designator, entity_designator.writeable), transitions={"succeeded": "IDENTIFY_OBJECT", "failed": "NAVIGATE_TO_START"}) # If no entities, try again # Point at the object StateMachine.add('IDENTIFY_OBJECT', IdentifyObject(robot, entity_designator, arm_designator), transitions={'done': 'NAVIGATE_TO_START', # Just keep on going 'failed': 'NAVIGATE_TO_START'}) # Just keep on going return state_machine
def main(): rospy.init_node("gpsr") 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) test = rospy.get_param('~test_mode', False) rospy.loginfo("[GPSR] Parameters:") rospy.loginfo("[GPSR] robot_name = {}".format(robot_name)) if skip: rospy.loginfo("[GPSR] skip = {}".format(skip)) if no_of_tasks: rospy.loginfo("[GPSR] number_of_tasks = {}".format(no_of_tasks)) if restart: rospy.loginfo("[GPSR] running a restart") if test: rospy.loginfo("[GPSR] running in test mode") # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - if robot_name == 'amigo': from robot_skills.amigo import Amigo as Robot elif robot_name == 'sergio': from robot_skills.sergio import Sergio as Robot else: raise ValueError('unknown robot') robot = Robot() action_client = ActionClient(robot.robot_name) knowledge = load_knowledge('challenge_gpsr') 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() while True: # Navigate to the GPSR meeting point if not skip and not finished: 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) while True: while True and not test: try: robot.hmi.query(description="", grammar="T -> %s" % robot_name, target="T") except hmi.TimeoutException: continue else: break 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) break except hmi.TimeoutException: robot.speech.speak( random.sample(knowledge.not_understood_sentences, 1)[0]) continue if not test: # 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.leftArm.reset() robot.leftArm.send_gripper_goal('close', 0.0) robot.rightArm.reset() robot.rightArm.send_gripper_goal('close', 0.0) robot.torso.reset() if task_result.succeeded: # Keep track of the number of performed tasks no_of_tasks_performed += 1 if no_of_tasks_performed >= no_of_tasks: finished = True # If we succeeded, we can say something optimistic after reporting to the operator if no_of_tasks_performed == 1: task_word = "task" else: task_word = "tasks" report += " I performed {} {} so far, still going strong!".format( no_of_tasks_performed, task_word) if rospy.get_time() - start_time > 60 * 15: finished = True if finished and not skip: nwc = NavigateToWaypoint(robot=robot, waypoint_designator=EntityByIdDesignator( robot=robot, id=knowledge.exit_waypoint), radius=0.3) nwc.execute() robot.speech.speak("Thank you very much, and goodbye!", block=True) break
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() for arm in robot.arms.itervalues(): arm.reset() arm.send_gripper_goal('close', 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()
def __init__(self, robot): """ Constructor :param robot: robot object """ smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"]) with self: self.spec_des = ds.Designator(knowledge.location_grammar) self.answer_des = ds.VariableDesignator(resolve_type=HMIResult) self.entity_des = EntityFromHmiResults(robot, self.answer_des) self._location_hmi_attempt = 0 self._max_hmi_attempts = 3 # ToDo: parameterize? @smach.cb_interface(outcomes=["reset"]) def _reset_location_hmi_attempt(userdata=None): """ Resets the location hmi attempt so that each operator gets three attempts """ self._location_hmi_attempt = 0 return "reset" smach.StateMachine.add("RESET_HMI_ATTEMPT", smach.CBState(_reset_location_hmi_attempt), transitions={"reset": "ANNOUNCE_ITEM"}) if WAIT_MODE == WaitMode.SPEECH: smach.StateMachine.add( "ANNOUNCE_ITEM", Say(robot, "Hello, my name is {}. Please call me by my name. " "Talk loudly into my microphone and wait for the ping". format(robot.robot_name), block=True), transitions={"spoken": "WAIT_TO_BE_CALLED"}) smach.StateMachine.add( "WAIT_TO_BE_CALLED", HearOptions(robot, ["{}".format(robot.robot_name)], timeout=10), transitions={ "{}".format(robot.robot_name): "INSTRUCT", "no_result": "ANNOUNCE_ITEM" }) elif WAIT_MODE == WaitMode.VISUAL: smach.StateMachine.add( "ANNOUNCE_ITEM", Say(robot, "Hello, my name is {}. Please step in front of me.". format(robot.robot_name), block=True), transitions={"spoken": "WAIT_TO_BE_CALLED"}) smach.StateMachine.add("WAIT_TO_BE_CALLED", WaitForPersonInFront( robot, attempts=10, sleep_interval=1.0), transitions={ "success": "INSTRUCT", "failed": "SAY_NOT_DETECTED" }) smach.StateMachine.add( "SAY_NOT_DETECTED", Say(robot, "I did not see you but will try to continue anyway.". format(robot.robot_name), block=True), transitions={"spoken": "INSTRUCT"}) smach.StateMachine.add( "INSTRUCT", Say(robot, [ "Please tell me where you would like to go. " "Talk loudly into my microphone and wait for the ping" ], block=True), transitions={"spoken": "LISTEN_FOR_LOCATION"}) smach.StateMachine.add("LISTEN_FOR_LOCATION", HearOptionsExtra(robot, self.spec_des, self.answer_des.writeable, 6), transitions={ "heard": "ASK_CONFIRMATION", "no_result": "HANDLE_FAILED_HMI" }) smach.StateMachine.add( "ASK_CONFIRMATION", Say(robot, [ "I hear that you would like to go to the {place}, is this correct?" ], place=ds.AttrDesignator(self.entity_des, "id", resolve_type=str)), transitions={"spoken": "CONFIRM_LOCATION"}) smach.StateMachine.add("CONFIRM_LOCATION", HearOptions(robot=robot, options=["yes", "no"]), transitions={ "yes": "INSTRUCT_FOR_WAIT", "no": "INSTRUCT", "no_result": "HANDLE_FAILED_HMI" }) @smach.cb_interface(outcomes=["retry", "fallback", "failed"]) def _handle_failed_hmi(userdata=None): """ Handle failed HMI queries so we can try up to x times """ self._location_hmi_attempt += 1 # Increment if self._location_hmi_attempt == self._max_hmi_attempts: rospy.logwarn( "HMI failed for the {} time, returning 'failed'". format(self._max_hmi_attempts)) if not BACKUP_SCENARIOS: rospy.logwarn( "No fallback scenario's available anymore") return "failed" backup = BACKUP_SCENARIOS.pop(0) robot.speech.speak("I am sorry but I did not hear you", mood="sad", block=False) robot.speech.speak(backup.sentence, block=False) self.answer_des.writeable.write( HMIResult("", backup.entity_id)) return "fallback" rospy.loginfo( "HMI failed for the {} time out of {}, retrying".format( self._location_hmi_attempt, self._max_hmi_attempts)) return "retry" smach.StateMachine.add("HANDLE_FAILED_HMI", smach.CBState(_handle_failed_hmi), transitions={ "retry": "INSTRUCT", "fallback": "INSTRUCT_FOR_WAIT", "failed": "failed" }) smach.StateMachine.add( "INSTRUCT_FOR_WAIT", Say(robot, [ "Let me think how to get to the {entity_id}", "I will now determine the best route to the {entity_id}" ], entity_id=ds.AttrDesignator(self.entity_des, "id", resolve_type=str)), transitions={"spoken": "GIVE_DIRECTIONS"}) smach.StateMachine.add("GIVE_DIRECTIONS", GiveDirections(robot, self.entity_des), transitions={ "succeeded": "INSTRUCT_FOLLOW", "failed": "failed" }) smach.StateMachine.add("INSTRUCT_FOLLOW", Say(robot, ["Please follow me"], block=True), transitions={"spoken": "GUIDE_OPERATOR"}) smach.StateMachine.add("GUIDE_OPERATOR", GuideToRoomOrObject(robot, self.entity_des), transitions={ "arrived": "SUCCESS", "unreachable": "SAY_CANNOT_REACH", "goal_not_defined": "SAY_CANNOT_REACH", "lost_operator": "SAY_LOST_OPERATOR", "preempted": "failed" }) smach.StateMachine.add( "SUCCESS", Say(robot, ["We have arrived"], block=True), transitions={"spoken": "RETURN_TO_INFORMATION_POINT"}) smach.StateMachine.add( "SAY_CANNOT_REACH", Say(robot, ["I am sorry but I cannot reach the destination."], block=True), transitions={"spoken": "RETURN_TO_INFORMATION_POINT"}) smach.StateMachine.add( "SAY_LOST_OPERATOR", Say(robot, ["Oops I have lost you completely."], block=True), transitions={"spoken": "RETURN_TO_INFORMATION_POINT"}) smach.StateMachine.add("RETURN_TO_INFORMATION_POINT", NavigateToWaypoint( robot, ds.EntityByIdDesignator( robot, INFORMATION_POINT_ID)), transitions={ "arrived": "succeeded", "unreachable": "failed", "goal_not_defined": "failed" })
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() for arm in robot.arms.itervalues(): arm.reset() arm.send_gripper_goal('close', 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()
def main(): rospy.init_node("ee_gpsr") parser = argparse.ArgumentParser() parser.add_argument('robot', help='Robot name') parser.add_argument('--once', action='store_true', help='Turn on infinite loop') parser.add_argument('--skip', action='store_true', help='Skip enter/exit') parser.add_argument('sentence', nargs='*', help='Optional sentence') args = parser.parse_args() rospy.loginfo('args: %s', args) mock_sentence = " ".join( [word for word in args.sentence if word[0] != '_']) # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - if args.robot == 'amigo': from robot_skills.amigo import Amigo as Robot elif args.robot == 'sergio': from robot_skills.sergio import Sergio as Robot else: raise ValueError('unknown robot') robot = Robot() # Sleep for 1 second to make sure everything is connected time.sleep(1) command_center = CommandCenter(robot) challenge_knowledge = load_knowledge('challenge_eegpsr') command_center.set_grammar( os.path.dirname(sys.argv[0]) + "/grammar.fcfg", challenge_knowledge) # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - # Start if not args.skip: # Wait for door, enter arena s = StartChallengeRobust(robot, challenge_knowledge.initial_pose) s.execute() # Move to the start location nwc = NavigateToWaypoint(robot, EntityByIdDesignator( robot, id=challenge_knowledge.starting_pose), radius=0.3) nwc.execute() # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - sentence = " ".join([word for word in args.sentence if word[0] != '_']) first = True if sentence: semantics = command_center.parse_command(sentence) if not semantics: rospy.logerr("Cannot parse \"{}\"".format(sentence)) return command_center.execute_command(semantics) else: while True: if first: # First sentence robot says sentences = [ "Hello there! Welcome to the double E GPSR. You can give me an order, but wait for the ping." ] else: # Sentence robot says after completing a task sentences = [ "Hello there, you look lovely! I'm here to take a new order, but wait for the ping!" ] # These sentences are for when the first try fails # (Robot says "Do you want me to ...?", you say "No", then robot says sentence below) sentences += [ "I'm so sorry! Can you please speak louder and slower? And wait for the ping!", "Again, I am deeply sorry. Bad robot! Please try again, but wait for the ping!", "You and I have communication issues. Speak up! Tell me what you want, but wait for the ping" ] hey_robot_wait_forever(robot) res = command_center.request_command(ask_confirmation=True, ask_missing_info=False, timeout=600, sentences=sentences) if not res: continue first = False (sentence, semantics) = res if not command_center.execute_command(semantics): robot.speech.speak("I am truly sorry, let's try this again") if args.once: break if not args.skip: nwc = NavigateToWaypoint( robot, EntityByIdDesignator(robot, id=challenge_knowledge.starting_pose), radius=0.3) nwc.execute()
def main(): rospy.init_node("ee_gpsr") parser = argparse.ArgumentParser() parser.add_argument('robot', help='Robot name') parser.add_argument('--once', action='store_true', help='Turn on infinite loop') parser.add_argument('--skip', action='store_true', help='Skip enter/exit') parser.add_argument('sentence', nargs='*', help='Optional sentence') args = parser.parse_args() rospy.loginfo('args: %s', args) mock_sentence = " ".join([word for word in args.sentence if word[0] != '_']) # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - if args.robot == 'amigo': from robot_skills.amigo import Amigo as Robot elif args.robot == 'sergio': from robot_skills.sergio import Sergio as Robot else: raise ValueError('unknown robot') robot = Robot() # Sleep for 1 second to make sure everything is connected time.sleep(1) command_center = CommandCenter(robot) challenge_knowledge = load_knowledge('challenge_eegpsr') command_center.set_grammar(os.path.dirname(sys.argv[0]) + "/grammar.fcfg", challenge_knowledge) # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - # Start if not args.skip: # Wait for door, enter arena s = StartChallengeRobust(robot, challenge_knowledge.initial_pose) s.execute() # Move to the start location nwc = NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=challenge_knowledge.starting_pose), radius = 0.3) nwc.execute() # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - sentence = " ".join([word for word in args.sentence if word[0] != '_']) first = True if sentence: semantics = command_center.parse_command(sentence) if not semantics: rospy.logerr("Cannot parse \"{}\"".format(sentence)) return command_center.execute_command(semantics) else: while True: if first: # First sentence robot says sentences = ["Hello there! Welcome to the double E GPSR. You can give me an order, but wait for the ping."] else: # Sentence robot says after completing a task sentences = ["Hello there, you look lovely! I'm here to take a new order, but wait for the ping!"] # These sentences are for when the first try fails # (Robot says "Do you want me to ...?", you say "No", then robot says sentence below) sentences += [ "I'm so sorry! Can you please speak louder and slower? And wait for the ping!", "Again, I am deeply sorry. Bad robot! Please try again, but wait for the ping!", "You and I have communication issues. Speak up! Tell me what you want, but wait for the ping" ] hey_robot_wait_forever(robot) res = command_center.request_command(ask_confirmation=True, ask_missing_info=False, timeout=600, sentences=sentences) if not res: continue first = False (sentence, semantics) = res if not command_center.execute_command(semantics): robot.speech.speak("I am truly sorry, let's try this again") if args.once: break if not args.skip: nwc = NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=challenge_knowledge.starting_pose), radius = 0.3) nwc.execute()
robot = Robot() if len(sys.argv) > 2: room = sys.argv[2] else: room = "kitchen" rospy.sleep(2) # wait for tf cache to be filled #nav_state = NavigateToPose(robot, 1, 0, 0) #nav_state.execute() #nav_state = NavigateToObserve(robot, ds.EntityByIdDesignator(robot=robot, id="dinnertable")) #nav_state.execute() # nav_state = NavigateToSymbolic(robot, { # ds.EntityByIdDesignator(robot, id=room):"in"}, # ds.EntityByIdDesignator(robot, id=room)) # nav_state.execute() #nav_state = NavigateToSymbolic(robot, { # ds.EntityByIdDesignator(robot, id="couch"):"in_front_of"}, # ds.EntityByIdDesignator(robot, id="couch")) #nav_state.execute() nav_state = NavigateToWaypoint(robot=robot, waypoint_designator=ds.EntityByIdDesignator( robot=robot, id="hsr_demo_waypoint"), radius=0.025) nav_state.execute()
def single_item(robot, results_writer, cls, support, waypoint, inspect_from_area=None, non_strict_class=False, search_area='on_top_of'): """ Benchmark grasping for a single item. Outputs a record dictionary :param robot: an instance of Robot :param results_writer: a csv.DictWriter to which the output record is written :param cls: class/type of item to grab :param support: ID of the entity supporting the item-to-grab :param waypoint: From where should the robot start the grasp :param inspect_from_area: Which area of the support-entity should the robot be in to start the inspection :param non_strict_class: If set to True, the robot is not strict about the type of item it grabs, eg. it continues grasping with another type of object :param search_area: which area of the support-entity to search/inspect for an item of the given class :return: a dict with the benchmark result """ grasp_cls = ds.Designator(cls, name='grasp_cls') support_entity = ds.EdEntityDesignator(robot, id=support, name='support_entity') entity_ids = ds.VariableDesignator([], resolve_type=[ClassificationResult], name='entity_ids') waypoint_des = ds.EdEntityDesignator(robot, id=waypoint, name='waypoint') arm = ds.LockingDesignator( ds.UnoccupiedArmDesignator(robot, name='unoccupied-arm', arm_properties={ "required_trajectories": ["prepare_grasp"], "required_goals": ["carrying_pose"], "required_gripper_types": [arms.GripperTypes.GRASPING] })) arm.lock() record = { 'robot': robot.robot_name, 'start_waypoint': waypoint, 'expected_class': cls, 'id': None, 'timestamp': datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S') } try: say_announce = Say( robot, sentence="Please put a {cls} {search_area} the {support}".format( cls=cls, support=support, search_area=search_area)) say_announce.execute() nav_to_start = NavigateToWaypoint(robot, waypoint_designator=waypoint_des, look_at_designator=support_entity) assert nav_to_start.execute( ) == 'arrived', "I did not arrive at the start" inspect = Inspect(robot=robot, entityDes=support_entity, objectIDsDes=entity_ids, navigation_area=inspect_from_area, searchArea=search_area) record['inspect_start'] = time.time() assert inspect.execute( ) == 'done', "I could not inspect the support entity" record['inspect_end'] = time.time() record['inspect_duration'] = '{:3.3f}'.format(record['inspect_end'] - record['inspect_start']) inspection_result = entity_ids.resolve( ) # type: List[ClassificationResult] if inspection_result: if grasp_cls.resolve() in ANY_OPTIONS or non_strict_class: rospy.loginfo("Any item will match") matching_results = inspection_result else: matching_results = [ result for result in inspection_result if result.type == grasp_cls.resolve() ] rospy.loginfo("Found {} items of class {}".format( len(matching_results), grasp_cls.resolve())) if matching_results: if len(matching_results) > 1: rospy.logwarn( "There are multiple items OK to grab, will select the last one" ) record['observed_class'] = ' '.join( [result.type for result in matching_results]) else: record['observed_class'] = matching_results[-1].type selected_entity_id = matching_results[-1].id rospy.loginfo("Selected entity {} for grasping".format( selected_entity_id)) grasp_entity = ds.EdEntityDesignator(robot, id=selected_entity_id) record['id'] = selected_entity_id[:6] entity = grasp_entity.resolve() # type: Entity if entity: vector_stamped = entity.pose.extractVectorStamped( ) # type: VectorStamped record['x'] = '{:.3f}'.format(vector_stamped.vector.x()) record['y'] = '{:.3f}'.format(vector_stamped.vector.y()) record['z'] = '{:.3f}'.format(vector_stamped.vector.z()) grab_state = Grab(robot, grasp_entity, arm) record['grab_start'] = time.time() assert grab_state.execute() == 'done', "I couldn't grasp" record['grab_end'] = time.time() record['grab_duration'] = '{:3.3f}'.format( record['grab_end'] - record['grab_start']) assert nav_to_start.execute( ) == 'arrived', "I could not navigate back to the start" rospy.logwarn("Robot will turn around to drop the {}".format( grasp_cls.resolve())) force_drive = ForceDrive(robot, 0, 0, 1, 3.14) # rotate 180 degs in pi seconds force_drive.execute() say_drop = Say( robot, sentence="I'm going to drop the item, please hold it!") say_drop.execute() drop_it = SetGripper(robot, arm_designator=arm, grab_entity_designator=grasp_entity) drop_it.execute() force_drive_back = ForceDrive( robot, 0, 0, -1, 3.14) # rotate -180 degs in pi seconds force_drive_back.execute() nav_to_start.execute() else: raise AssertionError("No {} found".format(grasp_cls.resolve())) else: rospy.logerr("No entities found at all :-(") except AssertionError as assertion_err: say_fail = Say(robot, sentence=assertion_err.message + ", sorry") say_fail.execute() finally: results_writer.writerow(record) return record
def main(): rospy.init_node("gpsr") 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) test = rospy.get_param('~test_mode', False) eegpsr = rospy.get_param('~eegpsr', False) time_limit = rospy.get_param('~time_limit', 0) if no_of_tasks == 0: no_of_tasks = 999 if time_limit == 0: time_limit = 999 rospy.loginfo("[GPSR] Parameters:") rospy.loginfo("[GPSR] robot_name = {}".format(robot_name)) if skip: rospy.loginfo("[GPSR] skip = {}".format(skip)) if no_of_tasks: rospy.loginfo("[GPSR] number_of_tasks = {}".format(no_of_tasks)) if restart: rospy.loginfo("[GPSR] running a restart") if test: rospy.loginfo("[GPSR] running in test mode") rospy.loginfo("[GPSR] time_limit = {}".format(time_limit)) # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 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() if eegpsr: knowledge = load_knowledge('challenge_eegpsr') else: knowledge = load_knowledge('challenge_gpsr') conversation_engine = ConversationEngineWithHmi(robot, knowledge.grammar, knowledge.grammar_target, knowledge) conversation_engine.test = test conversation_engine.skip = skip conversation_engine.tasks_to_be_done = no_of_tasks conversation_engine.time_limit = time_limit if not skip and not restart: # Wait for door, enter arena s = StartChallengeRobust(robot, knowledge.initial_pose) s.execute() 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() # 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) conversation_engine._start_wait_for_command(knowledge.grammar, knowledge.grammar_target) rospy.spin()
# ROS import rospy # TU/e Robotics from robot_skills import get_robot_from_argv # Robot smach states from robot_smach_states.navigation import NavigateRobust, NavigateToWaypoint import robot_smach_states.util.designators as ds if __name__ == "__main__": # Initialize rosnode rospy.init_node("test_robust_navigation") # Construct robot object robot = get_robot_from_argv(index=1) # Construct statemachine sm = NavigateRobust( robot, options=[ NavigateToWaypoint(robot, ds.EntityByIdDesignator(robot, id="foo")), NavigateToWaypoint(robot, ds.EntityByIdDesignator(robot, id="bar")), NavigateToWaypoint(robot, ds.EntityByIdDesignator(robot, id="initial_pose")) ], wait_time=1.0) sm.execute()
def setup_statemachine(robot): sm = smach.StateMachine(outcomes=['Done', 'Aborted']) with sm: # Start challenge via StartChallengeRobust smach.StateMachine.add("START_CHALLENGE_ROBUST", StartChallengeRobust(robot, STARTING_POINT), transitions={ "Done": "GO_TO_INTERMEDIATE_WAYPOINT", "Aborted": "GO_TO_INTERMEDIATE_WAYPOINT", "Failed": "GO_TO_INTERMEDIATE_WAYPOINT" }) # There is no transition to Failed in StartChallengeRobust (28 May) smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT', NavigateToWaypoint( robot, EntityByIdDesignator(robot, id=INTERMEDIATE_1), radius=0.5), transitions={ 'arrived': 'ASK_CONTINUE', 'unreachable': 'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1', 'goal_not_defined': 'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1' }) smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1', NavigateToWaypoint( robot, EntityByIdDesignator(robot, id=INTERMEDIATE_2), radius=0.5), transitions={ 'arrived': 'ASK_CONTINUE', 'unreachable': 'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2', 'goal_not_defined': 'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2' }) smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2', NavigateToWaypoint( robot, EntityByIdDesignator(robot, id=INTERMEDIATE_3), radius=0.5), transitions={ 'arrived': 'ASK_CONTINUE', 'unreachable': 'ASK_CONTINUE', 'goal_not_defined': 'ASK_CONTINUE' }) smach.StateMachine.add("ASK_CONTINUE", AskContinue(robot, 30), transitions={ 'continue': 'SAY_CONTINUEING', 'no_response': 'SAY_CONTINUEING' }) smach.StateMachine.add( 'SAY_CONTINUEING', Say(robot, [ "I heard continue, so I will move to the exit now. See you guys later!" ], block=False), transitions={'spoken': 'GO_TO_EXIT'}) # Amigo goes to the exit (waypoint stated in knowledge base) smach.StateMachine.add('GO_TO_EXIT', NavigateToWaypoint(robot, EntityByIdDesignator( robot, id=EXIT_1), radius=0.7), transitions={ 'arrived': 'AT_END', 'unreachable': 'GO_TO_EXIT_2', 'goal_not_defined': 'GO_TO_EXIT_2' }) smach.StateMachine.add('GO_TO_EXIT_2', NavigateToWaypoint(robot, EntityByIdDesignator( robot, id=EXIT_2), radius=0.5), transitions={ 'arrived': 'AT_END', 'unreachable': 'GO_TO_EXIT_3', 'goal_not_defined': 'GO_TO_EXIT_3' }) smach.StateMachine.add('GO_TO_EXIT_3', NavigateToWaypoint(robot, EntityByIdDesignator( robot, id=EXIT_3), radius=0.5), transitions={ 'arrived': 'AT_END', 'unreachable': 'RESET_ED_TARGET', 'goal_not_defined': 'AT_END' }) smach.StateMachine.add('RESET_ED_TARGET', ResetED(robot), transitions={'done': 'GO_TO_EXIT'}) # Finally amigo will stop and says 'goodbye' to show that he's done. smach.StateMachine.add('AT_END', Say(robot, "Goodbye"), transitions={'spoken': 'Done'}) analyse_designators(sm, "rips") return sm
def setup_statemachine(robot): sm = smach.StateMachine(outcomes=['done', 'failed', 'aborted']) with sm: smach.StateMachine.add('START_CHALLENGE_ROBUST', StartChallengeRobust(robot, STARTING_POINT), transitions={ 'Done': 'GO_TO_SEARCH_POSE', 'Aborted': 'aborted', 'Failed': 'GO_TO_SEARCH_POSE' }) smach.StateMachine.add('SAY_START', Say(robot, "Finding your mates, here we go!", block=False), transitions={'spoken': 'GO_TO_SEARCH_POSE'}) smach.StateMachine.add('GO_TO_SEARCH_POSE', NavigateToWaypoint(robot, ds.EntityByIdDesignator( robot, id=SEARCH_POINT), radius=0.375), transitions={ 'arrived': 'RISE_FOR_THE_PEOPLE', 'goal_not_defined': 'failed', 'unreachable': 'WAIT_SEARCH_POSE' }) smach.StateMachine.add('WAIT_SEARCH_POSE', WaitTime(robot, 5), transitions={ 'preempted': 'aborted', 'waited': 'GO_TO_SEARCH_POSE' }) # noinspection PyUnusedLocal @smach.cb_interface(outcomes=["done"]) def _rise_for_the_people(userdata=None): """ Resets the location hmi attempt so that each operator gets three attempts """ robot.arms.values()[0]._send_joint_trajectory( [[0.70, -1.9, 0.0, -1.57, 0.0]]) robot.speech.speak( "Hi there. My Name is Hero. I'm looking for the mates of my operator", block=False) return "done" smach.StateMachine.add("RISE_FOR_THE_PEOPLE", smach.CBState(_rise_for_the_people), transitions={"done": "LOCATE_PEOPLE"}) # locate all four people smach.StateMachine.add('LOCATE_PEOPLE', LocatePeople(robot, room_id=ROOM_ID), transitions={ 'done': 'RESET_FOR_DRIVING', }) # noinspection PyUnusedLocal @smach.cb_interface(outcomes=["done"]) def _reset_for_driving(userdata=None): """ Resets the location hmi attempt so that each operator gets three attempts """ robot.speech.speak("Thank you for your attention", block=False) robot.arms.values()[0]._send_joint_trajectory([ [0.01, -1.9, 0.0, -1.57, 0.0], # Inspect with q0 low [0.01, 0.0, -1.57, -1.57, 0.0] ]) # Reset return "done" smach.StateMachine.add("RESET_FOR_DRIVING", smach.CBState(_reset_for_driving), transitions={"done": "GO_BACK_TO_OPERATOR"}) # drive back to the operator to describe the mates smach.StateMachine.add('GO_BACK_TO_OPERATOR', NavigateToWaypoint( robot, ds.EntityByIdDesignator(robot, id=OPERATOR_POINT), radius=0.7, look_at_designator=ds.EntityByIdDesignator( robot, id=OPERATOR_POINT)), transitions={ 'arrived': 'REPORT_PEOPLE', 'goal_not_defined': 'REPORT_PEOPLE', 'unreachable': 'WAIT_GO_BACK' }) smach.StateMachine.add('WAIT_GO_BACK', WaitTime(robot, 5), transitions={ 'preempted': 'aborted', 'waited': 'GO_BACK_TO_OPERATOR' }) # check how to uniquely define them # ToDo: make this more interesting smach.StateMachine.add('REPORT_PEOPLE', ReportPeople(robot), transitions={'done': 'done'}) return sm
def main(): rospy.init_node("gpsr") 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) test = rospy.get_param('~test_mode', False) eegpsr = rospy.get_param('~eegpsr', False) time_limit = rospy.get_param('~time_limit', 0) if no_of_tasks == 0: no_of_tasks = 999 if time_limit == 0: time_limit = 999 rospy.loginfo("[GPSR] Parameters:") rospy.loginfo("[GPSR] robot_name = {}".format(robot_name)) if skip: rospy.loginfo("[GPSR] skip = {}".format(skip)) if no_of_tasks: rospy.loginfo("[GPSR] number_of_tasks = {}".format(no_of_tasks)) if restart: rospy.loginfo("[GPSR] running a restart") if test: rospy.loginfo("[GPSR] running in test mode") rospy.loginfo("[GPSR] time_limit = {}".format(time_limit)) # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - robot = get_robot(robot_name) if eegpsr: knowledge = load_knowledge('challenge_eegpsr') else: knowledge = load_knowledge('challenge_gpsr') conversation_engine = ConversationEngineWithHmi(robot, knowledge.grammar, knowledge.grammar_target, knowledge) conversation_engine.test = test conversation_engine.skip = skip conversation_engine.tasks_to_be_done = no_of_tasks conversation_engine.time_limit = time_limit if not skip and not restart: # Wait for door, enter arena s = StartChallengeRobust(robot, knowledge.initial_pose) s.execute() 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() # 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) conversation_engine._start_wait_for_command(knowledge.grammar, knowledge.grammar_target) rospy.spin()