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")
