states.Select_object( robot, person_to_look_at, ignore_predicate, retract_previous=False), transitions={ 'selected': 'RETRACT_ITEM_TO_LOOK_AT', 'no_answers': 'RETRACT_IGNORE' }) #Un-select the object we were looking at smach.StateMachine.add( 'RETRACT_ITEM_TO_LOOK_AT', states.Retract_facts( robot, Compound(person_to_look_at_predicate, "Item")), transitions={'retracted': 'WAIT_FOR_POSSIBLE_DETECTION'}) #When we're done, and can't find matches to the WM query, retract all ignores we introduced so the WM is normal again smach.StateMachine.add('RETRACT_IGNORE', states.Retract_facts( robot, Compound(ignore_predicate, "Item")), transitions={'retracted': 'Done'}) if __name__ == "__main__": rospy.init_node("executive_look_at_person") from robot_smach_states.util.startup import startup startup(IterateLookAtPerson)
analyse_designators(sm, "restaurant") return sm def test_delivery(robot): from robot_skills.util.kdl_conversions import kdlFrameStampedFromXYZRPY robot.ed.update_entity(id="one", kdlFrameStamped=kdlFrameStampedFromXYZRPY(x=1.0, y=0, frame_id="/map"), type="waypoint") robot.ed.update_entity(id="two", kdlFrameStamped=kdlFrameStampedFromXYZRPY(x=-1.2, y=0.0, frame_id="/map"), type="waypoint") robot.ed.update_entity(id="three", kdlFrameStamped=kdlFrameStampedFromXYZRPY(x=1.950, y=1.551, frame_id="/map"), type="waypoint") global ORDERS ORDERS = {"beverage":{"name":"coke", "location":"one"}, "combo":{"name":"pringles and chocolate", "location":"two"}} deliver = smach.StateMachine(outcomes=['done', 'aborted']) with deliver: smach.StateMachine.add('DELIVER_BEVERAGE', DeliverOrderWithBasket(robot, "beverage"), transitions={'succeeded':'NAVIGATE_TO_KITCHEN', 'failed':'NAVIGATE_TO_KITCHEN'}) smach.StateMachine.add('NAVIGATE_TO_KITCHEN', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="kitchen"), radius = WAYPOINT_RADIUS), transitions={'arrived': 'DELIVER_COMBO', 'unreachable':'DELIVER_COMBO', 'goal_not_defined':'DELIVER_COMBO'}) smach.StateMachine.add('DELIVER_COMBO', DeliverOrderWithBasket(robot, "combo"), transitions={'succeeded':'done', 'failed':'aborted'}) deliver.execute(None) ############################## initializing program ###################### if __name__ == '__main__': rospy.init_node('restaurant_exec') startup(setup_statemachine, challenge_name="restaurant", argv=[ e for e in sys.argv if e != "--custom" ]) print "If you want to save the learned locations, do '$ rosparam dump $(rospack find challenge_restaurant)/param/locations.yaml /restaurant_locations/'"
vs = self._robot.head.project_roi(roi=d.roi) except Exception as e: rospy.logwarn("ROI Projection failed: {}".format(e)) continue detections.append((d, vs)) # Sort the detectiosn detections = sorted(detections, key=lambda det: det[1].vector.Norm()) return detections[0][1].vector.Norm() < threshold, detections[0][1] def setup_state_machine(robot): sm = smach.StateMachine(outcomes=['Done', 'Aborted']) pointing_des = Designator() with sm: smach.StateMachine.add('DETECT_POINTING', PointingDetector(robot, pointing_des, ""), transitions={ 'succeeded': 'Done', 'failed': 'Aborted' }) return sm if __name__ == "__main__": rospy.init_node('state_machine') startup(setup_state_machine)
SearchPeopleContainer(robot, personNameDes), transitions={ 'container_success':'END_CHALLENGE', 'container_failed': 'END_CHALLENGE'}) smach.StateMachine.add( 'END_CHALLENGE', states.Say(robot,"My work here is done, goodbye!"), transitions={ 'spoken':'Done'}) ############################## MAIN ############################## if __name__ == "__main__": rospy.init_node('test_executive') startup(ChallengeTest, challenge_name="test") ''' Setup: - insert 2 persons in "computer room" - insert person in living room, in sight - insert 3 drinks in dinning table - insert object behind the kitchen table Behaviours to test: - navigate to waypoint - navigate to object - avoid obstacle - detect/recognize objects
# Part II: Clean the table smach.StateMachine.add('RETURN_TO_START_2', states.NavigateToPose(robot=robot, x=start_x, y=start_y, rz=start_rz, radius=0.3), transitions={'arrived': 'FETCH_COMMAND_II', 'unreachable': 'FETCH_COMMAND_II', 'goal_not_defined': 'FETCH_COMMAND_II'}) smach.StateMachine.add('FETCH_COMMAND_II', # Hear "clear up the table" HearFetchCommand(robot, 15.0, "clear"), transitions={'done': 'CLEAR_UP'}) smach.StateMachine.add('CLEAR_UP', # Clear the table ClearManipulateMachine(robot=robot, grasp_furniture_id=knowledge.place_furniture_id, place_furniture_id1=knowledge.grasp_furniture_id1, place_furniture_id2=knowledge.grasp_furniture_id2), transitions={'succeeded': 'END_CHALLENGE', 'failed': 'END_CHALLENGE'}) # End smach.StateMachine.add('END_CHALLENGE', states.Say(robot, "I am done here"), transitions={'spoken': 'Done'}) ds.analyse_designators(self, "set_a_table") if __name__ == "__main__": rospy.init_node('set_a_table_exec') startup(ChallengeSetATable, challenge_name="challenge_set_a_table")
smach.StateMachine.add( 'DETECT', Detect(robot), transitions={'succeeded': 'END_CHALLENGE', 'failed': 'DETECT'}) smach.StateMachine.add('END_CHALLENGE', states.Say(robot, "My work here is done, goodbye!"), transitions={'spoken': 'Done'}) ds.analyse_designators(self, "person_recognition") if __name__ == "__main__": rospy.init_node('person_recognition_exec') startup(ChallengePersonRecognition, challenge_name="person_recognition") cmd = 'convert `ls -t /tmp/faces/*_annotated.jpeg | head -1` /home/amigo/usb/tech_united_%s.pdf' % datetime.datetime.now().strftime("%Y-%m-%d-%H-%M") rospy.loginfo("Executing bash command: %s" % cmd) os.system(cmd) rospy.loginfo("Listing files on USB:") os.system("ls -lah /home/amigo/usb") rospy.loginfo("If this went wrong for a reason, please execute the command on amigo1 to create a pdf and copy to USB manually") cmd = 'convert `ls -t /tmp/faces/*_annotated.jpeg | head -1` /home/amigo/tech_united_%s.pdf' % datetime.datetime.now().strftime("%Y-%m-%d-%H-%M") rospy.loginfo("Executing bash command: %s" % cmd) os.system(cmd)
smach.StateMachine.__init__(self, outcomes=['Done','Aborted']) self.userdata.crowd_data = { "males": 1, "men": 2, "females": 3, "women": 4, "children": 5, "boys": 6, "girls": 7, "adults": 8, "elders": 9, "crowd_size": 10 } with self: smach.StateMachine.add('INITIALIZE', Initialize(robot), transitions={'initialized': 'BLUFF_GAME', 'abort': 'Aborted'}) smach.StateMachine.add('BLUFF_GAME', HearTurnAndAnswerQuestions(robot, num_questions=3), transitions={'done': 'Done'}, remapping={'crowd_data':'crowd_data'}) if __name__ == "__main__": rospy.init_node('speech_person_recognition_exec') startup(TestBluffGame, challenge_name="challenge_spr")
return "done" def setup_statemachine(robot): sm = smach.StateMachine(outcomes=['done', 'aborted']) robot.ed.reset() with sm: smach.StateMachine.add('WAIT_SAY', WaitSay(robot), transitions={'done': 'STORE_WAYPOINT'}) smach.StateMachine.add('STORE_WAYPOINT', StoreWaypoint(robot), 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') startup(setup_statemachine, challenge_name="automatic_side_detection")
"Another destination reached", "I have arrived", "I have arrived at my goal", "I have arrived at my target", "I have arrived at my destination", "I am at my goal", "I am at my target", "I am at my destination", "Here I am",]), transitions={ 'spoken':'SELECT_ACTION'}) smach.StateMachine.add("SAY_UNREACHABLE", states.Say(robot, [ "I can't find a way to my goal, better try something else", "This goal is unreachable, I better find somewhere else to go", "I am having a hard time getting there so I will look for a new target"]), transitions={ 'spoken':'SELECT_ACTION'}) smach.StateMachine.add("SAY_DONE", states.Say(robot, [ "That's all folks", "I'll stay here for a while", "Goodbye"]), transitions={ 'spoken':'Done'}) def requestedLocationcallback(self, msg): self.requested_location = msg.data self.robot.speech.speak("I got a request to go to location %"%self.requested_location) rospy.loginfo("Requested location is %s"%self.requested_location) if __name__ == "__main__": rospy.init_node('random_nav_exec') startup(RandomNav)
'failed': 'DETECT' }) smach.StateMachine.add('END_CHALLENGE', states.Say( robot, "My work here is done, goodbye!"), transitions={'spoken': 'Done'}) ds.analyse_designators(self, "person_recognition") if __name__ == "__main__": rospy.init_node('person_recognition_exec') startup(ChallengePersonRecognition, challenge_name="person_recognition") cmd = 'convert `ls -t /tmp/faces/*_annotated.jpeg | head -1` /home/amigo/usb/tech_united_%s.pdf' % datetime.datetime.now( ).strftime("%Y-%m-%d-%H-%M") rospy.loginfo("Executing bash command: %s" % cmd) os.system(cmd) rospy.loginfo("Listing files on USB:") os.system("ls -lah /home/amigo/usb") rospy.loginfo( "If this went wrong for a reason, please execute the command on amigo1 to create a pdf and copy to USB manually" ) cmd = 'convert `ls -t /tmp/faces/*_annotated.jpeg | head -1` /home/amigo/tech_united_%s.pdf' % datetime.datetime.now( ).strftime("%Y-%m-%d-%H-%M")
import rospy import smach import robot_smach_states as states from robot_smach_states.util.startup import startup from challenge_restaurant import AskOrder, HearWhichTable, StoreWaypoint def setup_statemachine(robot): sm = smach.StateMachine(outcomes=['done']) with sm: smach.StateMachine.add('WAYPOINT_SAY', states.Say(robot, "Say waypoint"), transitions={ 'spoken' :'WAYPOINT_ASK'}) smach.StateMachine.add('WAYPOINT_ASK', StoreWaypoint(robot), transitions={ 'done' :'TABLE_SAY', 'continue' : 'TABLE_SAY'}) smach.StateMachine.add('TABLE_SAY', states.Say(robot, "Say table"), transitions={ 'spoken' :'TABLE_ASK'}) smach.StateMachine.add('TABLE_ASK', HearWhichTable(robot), transitions={ 'one' :'ORDER_SAY', 'two' :'ORDER_SAY', 'three' :'ORDER_SAY', 'no_result' :'ORDER_SAY',}) smach.StateMachine.add('ORDER_SAY', states.Say(robot, "Say order"), transitions={ 'spoken' :'ORDER_ASK'}) smach.StateMachine.add('ORDER_ASK', AskOrder(robot, "one"), transitions={ 'next_order' :'WAYPOINT_SAY', 'orders_done' : 'WAYPOINT_SAY'}) return sm ############################## initializing program ###################### if __name__ == '__main__': rospy.init_node('restaurant_test_recognition') startup(setup_statemachine, challenge_name="restaurant_test_recognition")
}) smach.StateMachine.add("RESET_LEFT", states.ArmToJointPos( robot, robot.leftArm, [0, 0, 0, 0, 0, 0, 0]), transitions={ "done": "WAIT_FOR_PERSON", "failed": "WAIT_FOR_PERSON" }) smach.StateMachine.add("WAIT_FOR_PERSON", states.Wait_queried_perception( self.robot, ["face_recognition"], self.query_detect_face, timeout=60), transitions={ "query_true": "TALK", "timed_out": "Failed", "preempted": "Aborted" }) smach.StateMachine.add("TALK", states.Say(robot, ["Boooo!"]), transitions={'spoken': 'Done'}) if __name__ == "__main__": rospy.init_node("challenge_robo_zoo_boo") startup(Boo)
# Riddle Game smach.StateMachine.add('RIDDLE_GAME', riddle_game.HearAndAnswerQuestions(robot, num_questions=5), transitions={'done':'TRANSITION'}) # Transition: smach.StateMachine.add("TRANSITION", Say(robot, "Now lets play the blind mans bluff game"), transitions={"spoken": "BLUFF_GAME"}) # Bluff Games: smach.StateMachine.add('BLUFF_GAME', bluff_game.HearTurnAndAnswerQuestions(robot, num_questions=5, num_operators=5), transitions={'done': 'END_CHALLENGE'}) # End smach.StateMachine.add('END_CHALLENGE', Say(robot, "My work here is done, goodbye!"), transitions={'spoken': 'Done'}) ds.analyse_designators(self, "person_recognition") if __name__ == "__main__": rospy.init_node('speech_person_recognition_exec') startup(ChallengeSpeechPersonRecognition, challenge_name="challenge_spr")
return "succeeded" # Testing def setup_statemachine(robot): entity = ds.EntityByIdDesignator(robot, id='hallway_couch') sm = smach.StateMachine(outcomes=['Done', 'Aborted']) with sm: smach.StateMachine.add('TEST', LookAtEntity(robot, entity), transitions={'succeeded': 'Done'}) return sm if __name__ == "__main__": import doctest doctest.testmod() if len(sys.argv) > 1: robot_name = sys.argv[1] else: print "Please provide robot name as argument." exit(1) rospy.init_node('manipulation_exec') from robot_smach_states.util.startup import startup startup(setup_statemachine, robot_name=robot_name)
from robot_smach_states.util.startup import startup from robot_smach_states.util.designators import EntityByIdDesignator from empty_shelf_designator import EmptyShelfDesignator challenge_knowledge = load_knowledge('challenge_storing_groceries') USE_SLAM = True # Indicates whether or not to use SLAM for localization if USE_SLAM: CABINET = challenge_knowledge.cabinet_slam else: CABINET = challenge_knowledge.cabinet_amcl PLACE_SHELF = challenge_knowledge.place_shelf def setup_statemachine(robot): cabinet = EntityByIdDesignator(robot, id="bookcase", name="pick_shelf") ds = EmptyShelfDesignator(robot, cabinet, name="placement", area=PLACE_SHELF) for i in range(5): ds.resolve() import ipdb;ipdb.set_trace() if __name__ == '__main__': rospy.init_node('test_empty_shelf_designator') startup(setup_statemachine)
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - # TEST CONTAINER # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - # container for this stage testContainer = smach.StateMachine(outcomes = ['container_success', 'container_failed']) with testContainer: smach.StateMachine.add('TEST_SPEACH', states.Say(robot,"Testing speach"), transitions={'spoken':'container_success'}) # add container to state machine smach.StateMachine.add( 'TEST_CONTAINER', testContainer, transitions={ 'container_success':'END_CHALLENGE', 'container_failed': 'END_CHALLENGE'}) smach.StateMachine.add('END_CHALLENGE', states.Say(robot,"My work here is done, goodbye!"), transitions={'spoken':'Done'}) ############################## PYTHON ENTRY POINT ############################## if __name__ == "__main__": rospy.init_node('person_recognition_exec') startup(ChallengeTest)
else: print "but we reached that goal at some point, so we can safely update navigation" self._update_navigation() else: print "We never replanned so far, so we can safely update navigation" self._update_navigation() # else: # print "Updating navigation" # self._update_navigation() else: self._update_navigation() print "Updating navigation." rospy.sleep(self._period) # Loop at 2Hz def setup_statemachine(robot): sm = smach.StateMachine(outcomes=['Done', 'Aborted']) with sm: smach.StateMachine.add('TEST', FollowOperator(robot), transitions={"stopped":"TEST",'lost_operator':"TEST", "no_operator":"TEST"}) return sm if __name__ == "__main__": if len(sys.argv) > 1: robot_name = sys.argv[1] else: print "Please provide robot name as argument." exit(1) rospy.init_node('test_follow_operator') startup(setup_statemachine, robot_name=robot_name)
bluff_game.HearQuestionRepeat(robot), transitions={'answered': 'BLUFF_GAME_INF', 'not_answered': 'BLUFF_GAME_INF'}) # 6 smach.StateMachine.add('BLUFF_GAME_INF', bluff_game.HearQuestion(robot), transitions={'answered': 'BLUFF_GAME_INF', 'not_answered': 'BLUFF_GAME_INF_ASK_REPEAT'}) smach.StateMachine.add("BLUFF_GAME_INF_ASK_REPEAT", Say(robot, "Could you please repeat your question?"), transitions={"spoken": "BLUFF_GAME_INF_REPEAT"}) smach.StateMachine.add('BLUFF_GAME_INF_REPEAT', bluff_game.HearQuestionRepeat(robot), transitions={'answered' :'BLUFF_GAME_INF', 'not_answered': 'BLUFF_GAME_INF'}) # End smach.StateMachine.add('END_CHALLENGE', Say(robot, "My work here is done, goodbye!"), transitions={'spoken': 'Done'}) ds.analyse_designators(self, "person_recognition") if __name__ == "__main__": rospy.init_node('speech_person_recognition_exec') startup(ChallengeSpeechPersonRecognition, challenge_name="challenge_spr")
smach.StateMachine.__init__(self, outcomes=['Done','Aborted']) self.userdata.crowd_data = { "males": 3, "men": 2, "females": 5, "women": 3, "children": 3, "boys": 1, "girls": 2, "adults": 5, "elders": 1, "crowd_size": 8 } with self: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={'initialized': 'RIDDLE_GAME', 'abort': 'Aborted'}) smach.StateMachine.add("RIDDLE_GAME", HearAndAnswerQuestions(robot, num_questions=3), transitions={'done': 'Done'}, remapping={'crowd_data':'crowd_data'}) if __name__ == "__main__": rospy.init_node('speech_person_recognition_exec') startup(TestRiddleGame, challenge_name="challenge_spr")
} # Standalone testing ----------------------------------------------------------------- class TestDetectCrowd(smach.StateMachine): def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted']) with self: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized': 'DETECT_CROWD', 'abort': 'Aborted' }) smach.StateMachine.add("DETECT_CROWD", DetectCrowd(robot), transitions={ 'succeeded': 'Done', 'failed': 'Aborted' }) if __name__ == "__main__": rospy.init_node('speech_person_recognition_exec') startup(TestDetectCrowd, challenge_name="challenge_spr")
with sm: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized':'STORE_ROBOCUP_ARENA', 'abort':'aborted'}) smach.StateMachine.add('STORE_ROBOCUP_ARENA', StoreRobocupArena(robot), transitions={ 'done':'HEAD_STRAIGHT'}) smach.StateMachine.add('HEAD_STRAIGHT', HeadStraight(robot), transitions={ 'done':'SAY_INTRO'}) smach.StateMachine.add('SAY_INTRO', states.Say(robot, "Hi, Guide me out of the arena please.", look_at_standing_person=True), transitions={ 'spoken' :'FOLLOW_INITIAL'}) smach.StateMachine.add('FOLLOW_INITIAL', states.FollowOperator(robot, operator_timeout=30,replan=True), transitions={ 'stopped':'WAIT_FOR_OPERATOR_COMMAND', 'lost_operator':'WAIT_FOR_OPERATOR_COMMAND', 'no_operator':'FOLLOW_INITIAL'}) smach.StateMachine.add('FOLLOW', states.FollowOperator(robot, operator_timeout=30, ask_follow=False, learn_face=False, replan=True), transitions={ 'stopped':'WAIT_FOR_OPERATOR_COMMAND', 'lost_operator':'SAY_GUIDE', 'no_operator':'SAY_GUIDE'}) smach.StateMachine.add('WAIT_FOR_OPERATOR_COMMAND', WaitForOperatorCommand(robot), transitions={ 'follow':'FOLLOW', 'command':'SAY_GUIDE' }) smach.StateMachine.add('SAY_GUIDE', states.Say(robot, "I will guide you back to the robocup arena!", look_at_standing_person=True), transitions={ 'spoken' :'GUIDE_TO_ROBOCUP_ARENA'}) smach.StateMachine.add('GUIDE_TO_ROBOCUP_ARENA', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="robocup_arena"), radius = knowledge.back_radius), transitions={'arrived': 'SAY_BACK', 'unreachable':'GUIDE_TO_ROBOCUP_ARENA_BACKUP', 'goal_not_defined':'GUIDE_TO_ROBOCUP_ARENA_BACKUP'}) smach.StateMachine.add('GUIDE_TO_ROBOCUP_ARENA_BACKUP', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="robocup_arena"), radius = knowledge.back_radius+0.1), transitions={'arrived': 'SAY_BACK', 'unreachable':'GUIDE_TO_ROBOCUP_ARENA', 'goal_not_defined':'GUIDE_TO_ROBOCUP_ARENA'}) smach.StateMachine.add('SAY_BACK', states.Say(robot, "We are back in the robocup arena!", look_at_standing_person=True), transitions={ 'spoken' :'done'}) return sm ############################## initializing program ###################### if __name__ == '__main__': rospy.init_node('challenge_following_and_guiding') startup(setup_statemachine, challenge_name="challenge_following_and_guiding")
# Part II: Clean the table smach.StateMachine.add('RETURN_TO_START_2', states.NavigateToPose(robot=robot, x=start_x, y=start_y, rz=start_rz, radius=0.3), transitions={'arrived': 'FETCH_COMMAND_II', 'unreachable': 'FETCH_COMMAND_II', 'goal_not_defined': 'FETCH_COMMAND_II'}) smach.StateMachine.add('FETCH_COMMAND_II', # Hear "clear up the table" HearFetchCommand(robot, 15.0, "clear"), transitions={'done': 'CLEAR_UP'}) smach.StateMachine.add('CLEAR_UP', # Clear the table ClearManipulateMachine(robot=robot, grasp_furniture_id=knowledge.table, place_furniture_id1=knowledge.cupboard, place_furniture_id3=knowledge.cupboard), transitions={'succeeded': 'END_CHALLENGE', 'failed': 'END_CHALLENGE'}) # End smach.StateMachine.add('END_CHALLENGE', states.Say(robot, "I am done here"), transitions={'spoken': 'Done'}) ds.analyse_designators(self, "set_a_table") if __name__ == "__main__": rospy.init_node('set_a_table_exec') startup(ChallengeSetATable, challenge_name="challenge_set_a_table")
# smach.StateMachine.add( "PICKUP_LINES", # Pickup(robot), # transitions={"Done":"RESET_ALL"}) smach.StateMachine.add( "R2D2", R2D2(robot), transitions={"Done":"RESET_ALL"}) smach.StateMachine.add( "TOETER", Toeter(robot), transitions={"Done":"RESET_ALL"}) smach.StateMachine.add( "MACARENA", Macarena(robot), transitions={"Done":"RESET_ALL"}) smach.StateMachine.add( "GANGNAM", GangNamStyle(robot), transitions={"Done":"RESET_ALL"}) smach.StateMachine.add( "HOOFD_SCHOUDERS_KNIE_TEEN", HoofdSchouderKnieTeen(robot), transitions={"Done":"RESET_ALL"}) if __name__ == "__main__": rospy.init_node("challenge_robo_zoo_exec") rospy.loginfo("QR transitions: {0}".format(qr_transitions)) startup(RoboZooSimple)
]), transitions={'spoken': 'SELECT_ACTION'}) smach.StateMachine.add( "SAY_UNREACHABLE", states.Say(robot, [ "I can't find a way to my goal, better try something else", "This goal is unreachable, I better find somewhere else to go", "I am having a hard time getting there so I will look for a new target" ]), transitions={'spoken': 'SELECT_ACTION'}) smach.StateMachine.add( "SAY_DONE", states.Say(robot, [ "That's all folks", "I'll stay here for a while", "Goodbye" ]), transitions={'spoken': 'Done'}) def requestedLocationcallback(self, msg): self.requested_location = msg.data self.robot.speech.speak("I got a request to go to location %" % self.requested_location) rospy.loginfo("Requested location is %s" % self.requested_location) if __name__ == "__main__": rospy.init_node('random_nav_exec') startup(RandomNav)
states.NavigateToWaypoint( robot, EntityByIdDesignator(robot, id="starting_point"), radius=knowledge.back_radius, speak=False), transitions={ 'arrived': 'done', 'unreachable': 'WAIT_FOR_KEY_PRESS', 'goal_not_defined': 'aborted' }) smach.StateMachine.add('SAY_OEPS', states.Say(robot, "Oeps!", look_at_standing_person=True, language='nl', voice='marjolijn', block=False), transitions={'spoken': 'WAIT_FOR_KEY_PRESS'}) return sm ############################## initializing program ###################### if __name__ == '__main__': rospy.init_node('challenge_following_and_guiding') startup(setup_statemachine, challenge_name="challenge_following_and_guiding")
class TestFetchCommand(smach.StateMachine): def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted']) with self: smach.StateMachine.add('INITIALIZE', Initialize(robot), transitions={ 'initialized': 'FETCH_COMMAND', 'abort': 'Aborted' }) smach.StateMachine.add( 'FETCH_COMMAND', # Hear "set the table" HearFetchCommand(robot, 15.0), transitions={'heard': 'END_CHALLENGE'}) # End smach.StateMachine.add('END_CHALLENGE', Say(robot, "I am finally free!"), transitions={'spoken': 'Done'}) ds.analyse_designators(self, "set_a_table") if __name__ == "__main__": rospy.init_node('set_a_table_exec') startup(TestFetchCommand, challenge_name="challenge_set_a_table")
"raising_right": num_rarm, "pointing_left": num_lpointing, "pointing_right": num_rpointing, "laying": num_laying, "sitting": num_sitting } # Standalone testing ----------------------------------------------------------------- class TestDetectCrowd(smach.StateMachine): def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['Done','Aborted']) with self: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={'initialized': 'DETECT_CROWD', 'abort': 'Aborted'}) smach.StateMachine.add("DETECT_CROWD", DetectCrowd(robot), transitions={'succeeded': 'Done', 'failed': 'Aborted'}) if __name__ == "__main__": rospy.init_node('speech_person_recognition_exec') startup(TestDetectCrowd, challenge_name="challenge_spr")
"women": 3, "children": 3, "boys": 1, "girls": 2, "adults": 5, "elders": 1, "crowd_size": 8 } with self: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={'initialized': 'RIDDLE_GAME', 'abort': 'Aborted'}) smach.StateMachine.add("RIDDLE_GAME", HearAndAnswerQuestions( robot, grammar=knowledge.grammar, knowledge=common_knowledge, num_questions=3, ), transitions={'done': 'Done'}, remapping={'crowd_data':'crowd_data'}) if __name__ == "__main__": rospy.init_node('speech_person_recognition_exec') startup(TestRiddleGame, challenge_name="challenge_spr")
self.livingroom_waypoint = ds.EntityByIdDesignator( robot, id=challenge_knowledge.waypoint_livingroom['id']) self.operator_designator = ds.VariableDesignator( resolve_type=Entity) self.guest1_entity_des = ds.VariableDesignator( resolve_type=Entity, name='guest1_entity') self.guest1_name_des = ds.VariableDesignator('guest 1', name='guest1_name') self.guest1_drink_des = ds.VariableDesignator( resolve_type=HMIResult, name='guest1_drink') self.guest1_drinkname_des = ds.FieldOfHMIResult( self.guest1_drink_des, semantics_field='drink', name='guest1_drinkname') with self: smach.StateMachine.add('LEARN_GUEST', LearnGuest(robot, self.door_waypoint, self.guest1_entity_des, self.guest1_name_des, self.guest1_drink_des), transitions={ 'succeeded': 'succeeded', 'failed': 'failed', 'aborted': 'aborted' }) startup(Test)
'container_failed': 'END_CHALLENGE' }) smach.StateMachine.add('END_CHALLENGE', states.Say( robot, "My work here is done, goodbye!"), transitions={'spoken': 'Done'}) ############################## MAIN ############################## if __name__ == "__main__": rospy.init_node('test_executive') startup(ChallengeTest, challenge_name="test") ''' Setup: - insert 2 persons in "computer room" - insert person in living room, in sight - insert 3 drinks in dinning table - insert object behind the kitchen table Behaviours to test: - navigate to waypoint - navigate to object - avoid obstacle - detect/recognize objects - detect/recognize people - speach
testContainer = smach.StateMachine( outcomes=['container_success', 'container_failed']) with testContainer: smach.StateMachine.add( 'TEST_SPEACH', states.Say(robot, "Testing speach"), transitions={'spoken': 'container_success'}) # add container to state machine smach.StateMachine.add('TEST_CONTAINER', testContainer, transitions={ 'container_success': 'END_CHALLENGE', 'container_failed': 'END_CHALLENGE' }) smach.StateMachine.add('END_CHALLENGE', states.Say( robot, "My work here is done, goodbye!"), transitions={'spoken': 'Done'}) ############################## PYTHON ENTRY POINT ############################## if __name__ == "__main__": rospy.init_node('template_executive') startup(ChallengeTemplate, challenge_name="template")
'abort': 'Aborted' }) smach.StateMachine.add('BLUFF_GAME_1', HearQuestion(robot), transitions={ 'answered': 'Done', 'not_answered': 'BLUFF_GAME_1_ASK_REPEAT' }, remapping={'crowd_data': 'crowd_data'}) smach.StateMachine.add( "BLUFF_GAME_1_ASK_REPEAT", Say(robot, "Could you please repeat your question?"), transitions={"spoken": "BLUFF_GAME_1_REPEAT"}) smach.StateMachine.add('BLUFF_GAME_1_REPEAT', HearQuestionRepeat(robot), transitions={ 'answered': 'Done', 'not_answered': 'Done' }, remapping={'crowd_data': 'crowd_data'}) if __name__ == "__main__": rospy.init_node('speech_person_recognition_exec') startup(TestBluffGame, challenge_name="challenge_spr")
}) 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 if __name__ == '__main__': rospy.init_node('rips_exec') startup(setup_statemachine, challenge_name="rips")