def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
with self:
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# 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'})
def __init__(self, robot, selected_entity_designator, location_id, segment_area):
smach.StateMachine.__init__(self, outcomes=['done'])
sentences = ["%s, please clean this object %s the %s" % (other_robot_name, segment_area, location_id),
"%s, can you clean the trash %s the %s?" % (other_robot_name, segment_area, location_id),
"Can another robot clean the garbage %s the %s?" % (segment_area, location_id)]
with self:
smach.StateMachine.add('SAY_OTHER_ROBOT_CLEANUP',
robot_smach_states.Say(robot, sentences, block=False),
transitions={"spoken": "NAVIGATE_TO_INITIAL"})
smach.StateMachine.add( "NAVIGATE_TO_INITIAL",
robot_smach_states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=challenge_knowledge.starting_point,), radius=0.05),
transitions={ 'arrived' : 'CONTACT_OTHER_ROBOT',
'unreachable' : 'CONTACT_OTHER_ROBOT',
'goal_not_defined' : 'CONTACT_OTHER_ROBOT'})
smach.StateMachine.add('CONTACT_OTHER_ROBOT',
ContactOtherRobot(robot, selected_entity_designator),
transitions={"done": "WAIT_FOR_TRIGGER", "failed": "SAY_FAILED"})
smach.StateMachine.add('WAIT_FOR_TRIGGER',
robot_smach_states.WaitForTrigger(robot, ["continue", "gpsr"], "/amigo/trigger"),
transitions={"continue": "SAY_DONE", "gpsr": "SAY_FAILED", "preempted" : "SAY_FAILED"})
smach.StateMachine.add('SAY_DONE',
robot_smach_states.Say(robot, ["Thanks for cleaning", "Thank you", "You are the best"], block=True),
transitions={"spoken": "done"})
smach.StateMachine.add('SAY_FAILED',
robot_smach_states.Say(robot, ["Too bad then, we will just leave that trash there"], block=True),
transitions={"spoken": "done"})
def __init__(self, robot, selected_entity_designator, location_id, segment_area):
smach.StateMachine.__init__(self, outcomes=['cleanup', 'no_cleanup'])
sentences = ["Calling my operator. Can you help me with the object %s the %s" % (segment_area, location_id),
"Contacting my Operator. Can you tell me more about the object %s the %s?" % (segment_area, location_id),
"I am asking my operator now about the object %s the %s?" % (segment_area, location_id)]
with self:
smach.StateMachine.add('SAY_CONTACTING_OPERATOR',
robot_smach_states.Say(robot, sentences, block=True),
transitions={"spoken": "OPERATOR_FEEDBACK"})
smach.StateMachine.add('OPERATOR_FEEDBACK',
OperatorFeedback(robot, selected_entity_designator),
transitions={"cleanup": "SAY_CLEANUP", "no_cleanup": "SAY_NO_CLEANUP"})
smach.StateMachine.add('SAY_CLEANUP',
robot_smach_states.Say(robot, ["Ok, I will cleanup the object",
"That's ok!",
"As you wish"], block=True),
transitions={"spoken": "cleanup"})
smach.StateMachine.add('SAY_NO_CLEANUP',
robot_smach_states.Say(robot, ["Ok, I will leave the object here",
"That's ok! I will continue",
"Now I know that this object is not trash"], block=True),
transitions={"spoken": "no_cleanup"})
def __init__(self, robot):
smach.StateMachine.__init__(
self, outcomes=['container_success', 'container_failed'])
with self:
smach.StateMachine.add('SAY_STARTING_TEST',
states.Say(robot,
"Starting navigation test",
block=False),
transitions={'spoken': 'SAY_TEST_SYMBOLIC'})
smach.StateMachine.add('SAY_TEST_SYMBOLIC',
states.Say(robot,
"Testing Navigate To Symbolic",
block=False),
transitions={'spoken': 'NAV_TO_SYMB'})
smach.StateMachine.add(
'NAV_TO_SYMB',
states.NavigateToSymbolic(
robot,
{EntityByIdDesignator(robot, id="living_room"): "in"},
EntityByIdDesignator(robot, id="dinnertable")),
transitions={
'arrived': 'SAY_TEST_WAYPOINT',
'unreachable': 'SAY_FAILED_SYMBOLIC',
'goal_not_defined': 'SAY_FAILED_SYMBOLIC'
})
smach.StateMachine.add('SAY_FAILED_SYMBOLIC',
states.Say(robot,
"Failed Navigate To Symbolic.",
block=True),
transitions={'spoken': 'SAY_TEST_WAYPOINT'})
smach.StateMachine.add('SAY_TEST_WAYPOINT',
states.Say(robot,
"Testing Navigate To Waypoint.",
block=False),
transitions={'spoken': 'NAV_TO_WAYPOINT'})
smach.StateMachine.add(
'NAV_TO_WAYPOINT',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id=challenge_knowledge.wp_test_nav)),
transitions={
'arrived': 'container_success',
'unreachable': 'SAY_FAILED_WAYPOINT',
'goal_not_defined': 'SAY_FAILED_WAYPOINT'
})
smach.StateMachine.add('SAY_FAILED_WAYPOINT',
states.Say(robot,
"Failed reaching the waypoint.",
block=True),
transitions={'spoken': 'container_success'})
def __init__(self, robot, selected_entity_designator, location_id, segment_area):
smach.StateMachine.__init__(self, outcomes=['done','failed'])
place_pose = dropPoseDesignator(robot, 0.6, "drop_pose")
with self:
smach.StateMachine.add("SPEAK", Speak(robot, selected_entity_designator, location_id, segment_area),
transitions={"done": "GRAB"})
smach.StateMachine.add("GRAB",
robot_smach_states.Grab(robot, selected_entity_designator,
UnoccupiedArmDesignator(robot.arms,
robot.rightArm,
name="empty_arm_designator")),
transitions={"done": "SAY_GRAB_SUCCESS", "failed": "SAY_GRAB_FAILED"})
smach.StateMachine.add('SAY_GRAB_SUCCESS',
robot_smach_states.Say(robot, ["Now I am going to toss the item in the trashbin",
"Let's clean up this object",
"Away with this garbage",
"Everything will be cleaned"], block=False),
transitions={"spoken": "CHECK_ARM_FREE"})
smach.StateMachine.add('SAY_GRAB_FAILED',
robot_smach_states.Say(robot, ["I could not grab the item.",
"I failed to grasp the item",
"I cannot reach the item",
"Item grab failed"], block=False),
transitions={"spoken": "failed"})
smach.StateMachine.add('CHECK_ARM_FREE', ArmFree(robot), transitions={"yes": "done", "no": "CHECK_ARM_OCCUPIED"})
smach.StateMachine.add('CHECK_ARM_OCCUPIED', ArmOccupied(robot), transitions={"yes": "PLACE", "no": "done"})
smach.StateMachine.add('PLACE',
robot_smach_states.Place(robot,
selected_entity_designator,
place_pose,
OccupiedArmDesignator(robot.arms,
robot.rightArm,
name="occupied_arm_designator")),
transitions={"done": "SAY_PLACE_SUCCESS", "failed": "SAY_PLACE_FAILED"})
smach.StateMachine.add('SAY_PLACE_SUCCESS',
robot_smach_states.Say(robot, ["Bye bye!",
"Yeah!",
"Successfully disposed the item",
"Another score for AMIGO"], block=False),
transitions={"spoken": "CHECK_ARM_OCCUPIED"})
smach.StateMachine.add('SAY_PLACE_FAILED',
robot_smach_states.Say(robot, ["I could not cleanup the item.",
"I cannot put the item in the trashbin",
"Item cleanup failed"], block=False),
transitions={"spoken": "CHECK_ARM_OCCUPIED"})
def __init__(self, robot):
smach.StateMachine.__init__(
self, outcomes=['container_success', 'container_failed'])
with self:
smach.StateMachine.add('SAY_STARTING_TEST',
states.Say(
robot,
"Starting people recognition test",
block=False),
transitions={'spoken': 'NAV_TO_WAYPOINT'})
smach.StateMachine.add(
'NAV_TO_WAYPOINT',
states.NavigateToWaypoint(
robot,
EdEntityDesignator(robot,
id=challenge_knowledge.wp_test_nav)),
transitions={
'arrived': 'LOOK_AT_OPERATOR',
'unreachable': 'SAY_FAILED_WAYPOINT',
'goal_not_defined': 'SAY_FAILED_WAYPOINT'
})
smach.StateMachine.add('SAY_FAILED_WAYPOINT',
states.Say(robot,
"Failed reaching the waypoint.",
block=True),
transitions={'spoken': 'LOOK_AT_OPERATOR'})
smach.StateMachine.add('LOOK_AT_OPERATOR',
states_interaction.LookAtPersonInFront(
robot, lookDown=False),
transitions={
'succeeded': 'RECOGNIZE_PEOPLE',
'failed': 'RECOGNIZE_PEOPLE'
})
smach.StateMachine.add('RECOGNIZE_PEOPLE',
test_states.RecognizePeople(robot),
transitions={
'succeeded': 'container_success',
'failed': 'SAY_NO_PEOPLE',
'no_people': 'SAY_NO_PEOPLE'
})
smach.StateMachine.add('SAY_NO_PEOPLE',
states.Say(
robot,
"I don see anyone in front of me.",
block=False),
transitions={'spoken': 'LOOK_AT_OPERATOR'})
def __init__(self, robot):
smach.StateMachine.__init__(
self, outcomes=['container_success', 'container_failed'])
with self:
smach.StateMachine.add(
"SAY_WAITING_OPERATOR",
states.Say(robot, [
"I'm waiting for the operator to stand in front of me.",
"Would the operator please come forward.",
"I need an operator, please stand in front of me."
],
block=False),
transitions={'spoken': 'LOOK_AT_OPERATOR'})
smach.StateMachine.add('LOOK_AT_OPERATOR',
states_interaction.LookAtPersonInFront(
robot, lookDown=False),
transitions={
'succeeded': 'WAIT_FOR_OPERATOR',
'failed': 'WAIT_FOR_OPERATOR'
})
smach.StateMachine.add("WAIT_FOR_OPERATOR",
states_interaction.WaitForPersonDetection(
robot, attempts=5, sleep_interval=1),
transitions={
'succeeded': 'container_success',
'failed': 'SAY_FAILED_WAITING'
})
smach.StateMachine.add(
'SAY_FAILED_WAITING',
states.Say(robot, "I don't see anyone.", block=True),
transitions={'spoken': 'WAIT_FOR_OPERATOR_2'})
smach.StateMachine.add("WAIT_FOR_OPERATOR_2",
states_interaction.WaitForPersonDetection(
robot, attempts=5, sleep_interval=1),
transitions={
'succeeded': 'container_success',
'failed': 'container_success'
})
smach.StateMachine.add(
'SAY_FAILED_WAITING_AGAIN',
states.Say(
robot,
"I still couldn't find anyone in front of me. I will continue my task.",
block=True),
transitions={'spoken': 'WAIT_FOR_OPERATOR_2'})
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
self.robot = robot
@smach.cb_interface(outcomes=['done'])
def look_at_standing_person(userdata=None):
robot.head.look_at_standing_person()
return 'done'
with self:
smach.StateMachine.add('LOOK_AT_OPERATOR',
smach.CBState(look_at_standing_person),
transitions={'done': 'SAY_LOOK_AT_ME'})
smach.StateMachine.add(
'SAY_LOOK_AT_ME',
states.Say(
robot,
"Please stand one meter in front of me and look at me while I \
learn to recognize your face.",
block=True),
transitions={'spoken': 'LEARN_PERSON'})
smach.StateMachine.add(
'LEARN_PERSON',
states.LearnPerson(
robot, name_designator=ds.VariableDesignator("operator")),
transitions={
'succeeded_learning': 'SAY_OPERATOR_LEARNED',
'failed_learning': 'SAY_FAILED_LEARNING',
'timeout_learning': 'SAY_FAILED_LEARNING'
})
smach.StateMachine.add(
'SAY_FAILED_LEARNING',
states.Say(
robot,
"I could not learn my operator's face. Let me try again.",
block=True),
transitions={'spoken': 'failed'})
smach.StateMachine.add(
'SAY_OPERATOR_LEARNED',
states.Say(
robot,
"Now i know what you look like. Please go mix with the crowd."
),
transitions={'spoken': 'succeeded'})
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
def __init__(self, robot):
smach.StateMachine.__init__(self,
outcomes=['Done', 'Aborted', 'Failed'])
with self:
smach.StateMachine.add("SAY_HI",
states.Say(robot, "Hi "),
transitions={"spoken": "Done"})
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted'])
runs = ds.Designator([0, 1])
run = ds.VariableDesignator(resolve_type=int)
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'SET_INITIAL_POSE',
'abort': 'aborted'
})
smach.StateMachine.add('SET_INITIAL_POSE',
states.SetInitialPose(
robot,
challenge_knowledge.starting_point),
transitions={
'done': 'HANDLE_GUEST_1',
"preempted": 'aborted',
'error': 'HANDLE_GUEST_1'
})
smach.StateMachine.add('HANDLE_GUEST_1',
HandleSingleGuest(robot, assume_john=True),
transitions={
'succeeded': 'HANDLE_GUEST_2',
'aborted': 'HANDLE_GUEST_2'
})
smach.StateMachine.add('HANDLE_GUEST_2',
HandleSingleGuest(robot, assume_john=False),
transitions={
'succeeded': 'SAY_DONE',
'aborted': 'SAY_DONE'
})
smach.StateMachine.add(
'SAY_DONE',
states.Say(robot,
["That's all folks, my job is done, bye bye!"],
block=False),
transitions={'spoken': 'GO_BACK'})
smach.StateMachine.add(
'GO_BACK',
states.NavigateToWaypoint(
robot,
ds.EntityByIdDesignator(
robot, id=challenge_knowledge.waypoint_door['id']),
challenge_knowledge.waypoint_door['radius']),
transitions={
'arrived': 'succeeded',
'unreachable': 'succeeded',
'goal_not_defined': 'succeeded'
})
def __init__(self, robot, order_type):
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"])
beverage_dest_desig = EdEntityDesignator(robot) #.id is overwritten by instruct_barman
with self:
@smach.cb_interface(outcomes=['spoken'])
def instruct_barman(userdata=None):
try:
order = ORDERS[order_type]
beverage_dest_desig.id = order['location']
robot.speech.speak("Barman, please put a {name} in my basket for table {location}".format(**order))
except KeyError:
rospy.logerr("No beverage in ORDERS")
return 'spoken'
smach.StateMachine.add( 'INSTRUCT_BARMAN',
smach.CBState(instruct_barman),
transitions={'spoken' :'AWAIT_PUT_ORDER_CONFIRMATION'})
smach.StateMachine.add( 'AWAIT_PUT_ORDER_CONFIRMATION',
states.WaitTime(robot, 8),
transitions={ 'waited' :'GOTO_ORDER_DESTINATION_1',
'preempted' :'failed'})
smach.StateMachine.add( 'GOTO_ORDER_DESTINATION_1', states.NavigateToWaypoint(robot, beverage_dest_desig, radius = WAYPOINT_RADIUS),
transitions={ 'arrived' :'SAY_TAKE_ORDER',
'unreachable' :'GOTO_ORDER_DESTINATION_2',
'goal_not_defined' :'GOTO_ORDER_DESTINATION_2'})
smach.StateMachine.add( 'GOTO_ORDER_DESTINATION_2', states.NavigateToWaypoint(robot, beverage_dest_desig, radius = WAYPOINT_RADIUS),
transitions={ 'arrived' :'SAY_TAKE_ORDER',
'unreachable' :'failed',
'goal_not_defined' :'failed'})
@smach.cb_interface(outcomes=['spoken'])
def instruct_guest(userdata=None):
try:
order = ORDERS[order_type]
robot.speech.speak("Dear guest at table {location}, you can get your {name} from my basket.".format(**order))
except KeyError:
rospy.logerr("No beverage in ORDERS")
return 'spoken'
smach.StateMachine.add( 'SAY_TAKE_ORDER',
smach.CBState(instruct_guest),
transitions={'spoken' :'AWAIT_TAKE_ORDER_CONFIRMATION'})
smach.StateMachine.add( 'AWAIT_TAKE_ORDER_CONFIRMATION',
states.WaitTime(robot, 5),
transitions={ 'waited' :'SAY_ENJOY_ORDER',
'preempted' :'failed'})
smach.StateMachine.add( 'SAY_ENJOY_ORDER',
states.Say(robot, ["Enjoy your {}".format(order_type)], block=False),
transitions={ 'spoken' :'succeeded'})
def __init__(self, robot, grab_designator=None):
""" Constructor
:param robot: robot object
:param grab_designator: EdEntityDesignator designating the item to grab. If not provided, a default one is
constructed (grabs the closest object in the volume of the surface)
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"])
# Create designators
self.empty_arm_designator = ds.UnoccupiedArmDesignator(robot.arms, robot.leftArm, name="empty_arm_designator")
self.grab_designator = ds.LockToId(robot=robot, to_be_locked=grab_designator)
with self:
@smach.cb_interface(outcomes=["locked"])
def lock(userdata=None):
""" 'Locks' a locking designator """
# This determines that self.current_item cannot not resolve to a new value until it is unlocked again.
self.grab_designator.lock()
if self.grab_designator.resolve():
rospy.loginfo("Current_item is now locked to {0}".format(self.grab_designator.resolve().id))
return "locked"
smach.StateMachine.add("LOCK_ITEM",
smach.CBState(lock),
transitions={'locked': 'ANNOUNCE_ITEM'})
smach.StateMachine.add("ANNOUNCE_ITEM",
states.Say(robot, EntityDescriptionDesignator(self.grab_designator,
name="current_item_desc"),
block=False),
transitions={'spoken': 'GRAB_ITEM'})
smach.StateMachine.add("GRAB_ITEM",
states.Grab(robot, self.grab_designator, self.empty_arm_designator),
transitions={'done': 'UNLOCK_ITEM_SUCCEED',
'failed': 'UNLOCK_ITEM_FAIL'})
@smach.cb_interface(outcomes=["unlocked"])
def lock(userdata=None):
""" 'Locks' a locking designator """
# This determines that self.current_item cannot not resolve to a new value until it is unlocked again.
self.grab_designator.unlock()
return "unlocked"
smach.StateMachine.add("UNLOCK_ITEM_SUCCEED",
smach.CBState(lock),
transitions={'unlocked': 'succeeded'})
smach.StateMachine.add("UNLOCK_ITEM_FAIL",
smach.CBState(lock),
transitions={'unlocked': 'failed'})
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=["Done"])
self.robot = robot
with self:
# smach.StateMachine.add("LOOK_AT_PERSON",
# LookAtPerson(robot),
# transitions={ 'Done' :'SAY_HI',
# 'Aborted' :'SAY_HI',
# 'Failed' :'SAY_HI'})
smach.StateMachine.add("SAY_HI",
states.Say(robot, [
"Howdy, I got a joke for you",
"Hi there, I have a joke for you"
]),
transitions={"spoken": "MAKE_JOKES"})
smach.StateMachine.add("MAKE_JOKES",
states.Say(robot, jokes),
transitions={"spoken": "Done"})
def __init__(self, robot, personNameDesLocal):
@smach.cb_interface(outcomes=['spoken'])
def saySearchingOperatorCB(userdata=None):
printOk("saySearchingOperatorCB")
robot.speech.speak("I am searching for " +
personNameDesLocal.resolve() + "!",
block=False)
return 'spoken'
smach.StateMachine.__init__(
self, outcomes=['container_success', 'container_failed'])
with self:
smach.StateMachine.add(
'NAV_TO_WAYPOINT',
states.NavigateToWaypoint(
robot,
EdEntityDesignator(robot,
id=challenge_knowledge.wp_test_nav)),
transitions={
'arrived': 'SAY_SEARCHING_OPERATOR',
'unreachable': 'SAY_FAILED_WAYPOINT',
'goal_not_defined': 'SAY_FAILED_WAYPOINT'
})
smach.StateMachine.add(
'SAY_FAILED_WAYPOINT',
states.Say(robot, "Failed reaching the waypoint.", block=True),
transitions={'spoken': 'SAY_SEARCHING_OPERATOR'})
smach.StateMachine.add('SAY_SEARCHING_OPERATOR',
smach.CBState(saySearchingOperatorCB),
transitions={'spoken': 'SAY_UNFINSHED'})
smach.StateMachine.add('SAY_UNFINSHED',
states.Say(robot,
"This part is not finished yet.",
block=False),
transitions={'spoken': 'container_success'})
def __init__(self, robot, personNameDesLocal):
smach.StateMachine.__init__(
self, outcomes=['container_success', 'container_failed'])
with self:
smach.StateMachine.add(
'SAY_LOOK_AT_ME',
states.Say(
robot,
"Please stand in front of me and look at my camera while I learn your face.",
block=False),
transitions={'spoken': 'LOOK_AT_OPERATOR'})
smach.StateMachine.add(
'LOOK_AT_OPERATOR',
states_interaction.LookAtPersonInFront(robot, lookDown=True),
transitions={
'succeeded': 'LEARN_PERSON',
# 'failed':'SAY_LEARN_FACE_FAILED'})
'failed': 'LEARN_PERSON'
})
smach.StateMachine.add(
'LEARN_PERSON',
states.LearnPerson(robot, name_designator=personNameDesLocal),
transitions={
'succeeded_learning': 'container_success',
'failed_learning': 'SAY_LEARN_FACE_FAILED',
'timeout_learning': 'SAY_LEARN_FACE_FAILED'
})
smach.StateMachine.add('SAY_LEARN_FACE_FAILED',
states.Say(robot,
"I could not learn your face.",
block=False),
transitions={'spoken': 'container_failed'})
def __init__(self, robot, arm_designator, grabbed_entity_label="", grabbed_entity_designator=None, timeout=15,
arm_configuration="handover_to_human"):
"""
Hold up hand to accept an object and close hand once something is inserted
:param robot: Robot with which to execute this behavior
:param arm_designator: ArmDesignator resolving to arm accept item into
:param grabbed_entity_label: What ID to give a dummy item in case no grabbed_entity_designator is supplied
:param grabbed_entity_designator: EntityDesignator resolving to the accepted item. Can be a dummy
:param timeout: How long to hold hand over before closing without anything
:param arm_configuration: Which pose to put arm in when holding hand up for the item.
"""
smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed', 'timeout'])
ds.check_type(arm_designator, PublicArm)
if not grabbed_entity_designator and grabbed_entity_label == "":
rospy.logerr("No grabbed entity label or grabbed entity designator given")
with self:
smach.StateMachine.add("POSE", manipulation.ArmToJointConfig(robot, arm_designator, arm_configuration),
transitions={'succeeded': 'OPEN_BEFORE_INSERT', 'failed': 'OPEN_BEFORE_INSERT'})
smach.StateMachine.add('OPEN_BEFORE_INSERT', manipulation.SetGripper(robot=robot,
arm_designator=arm_designator,
gripperstate=manipulation.GripperState.
OPEN),
transitions={'succeeded': 'SAY1', 'failed': 'SAY1'})
smach.StateMachine.add("SAY1", states.Say(robot, 'Please hand over the trash by putting the top of the bag'
' between my grippers and push firmly into my camera as'
' will be shown on my screen.'),
transitions={'spoken': 'SHOW_IMAGE'})
smach.StateMachine.add("SHOW_IMAGE",
states.ShowImageState(
robot=robot,
image_filename="~/ros/kinetic/system/src/challenge_take_out_the_garbage/src"
"/challenge_take_out_the_garbage/beun_picture.png",
seconds=5),
transitions={'succeeded': 'CLOSE_AFTER_INSERT'})
smach.StateMachine.add('CLOSE_AFTER_INSERT', manipulation.CloseGripperOnHandoverToRobot(
robot,
arm_designator,
grabbed_entity_label=grabbed_entity_label,
grabbed_entity_designator=grabbed_entity_designator,
timeout=timeout),
transitions={'succeeded': 'succeeded',
'timeout': 'failed',
'failed': 'failed'})
def __init__(self, robot, category_grammar, categoryw):
smach.StateMachine.__init__(self, outcomes=["done"])
hmi_result_des = ds.VariableDesignator(resolve_type=hmi.HMIResult, name="hmi_result_des")
category_des = ds.FuncDesignator(ds.AttrDesignator(hmi_result_des, "semantics", resolve_type=unicode),
str, resolve_type=str)
@smach.cb_interface(outcomes=['done'])
def write_category(ud, des_read, des_write):
# type: (object, ds.Designator, ds.Designator) -> str
assert(ds.is_writeable(des_write))
assert(des_write.resolve_type == des_read.resolve_type)
des_write.write(des_read.resolve())
return 'done'
with self:
smach.StateMachine.add("ASK_WHERE_TO_DROP", robot_smach_states.Say(robot,
"Please look at the object in my gripper and tell me which category it is. If it should be thrown away,"
"call it trash", block=True),
transitions={"spoken": "HEAR_LOCATION"})
smach.StateMachine.add("HEAR_LOCATION", robot_smach_states.HearOptionsExtra(robot, category_grammar,
ds.writeable(hmi_result_des)),
transitions={"heard": "SAY_HEARD_CORRECT",
"no_result": "ASK_WHERE_TO_DROP"})
smach.StateMachine.add("SAY_HEARD_CORRECT", robot_smach_states.Say(
robot, "I understood that the object is of category {category}, is this correct?",
category=category_des,
block=True),
transitions={"spoken": "HEAR_CORRECT"})
smach.StateMachine.add("HEAR_CORRECT", robot_smach_states.AskYesNo(robot),
transitions={"yes": "WRITE_CATEGORY",
"no": "ASK_WHERE_TO_DROP",
"no_result": "ASK_WHERE_TO_DROP"})
smach.StateMachine.add('WRITE_CATEGORY', smach.CBState(write_category, cb_args=[category_des, categoryw]),
transitions={'done': 'done'})
def __init__(self, robot):
smach.StateMachine.__init__(self,
outcomes=["Done", "Failed", "Aborted"])
self.robot = robot
self.query_detect_face = Conjunction(
Compound("property_expected", "ObjectID", "class_label", "face"),
Compound("property_expected", "ObjectID", "position",
Compound("in_front_of", "amigo")),
Compound("property_expected", "ObjectID", "position",
Sequence("X", "Y", "Z")))
with self:
#TODO: Make sure the arms are straight down and the head is staring away into the distance
smach.StateMachine.add("RESET_RIGHT",
states.ArmToJointPos(
robot, robot.rightArm,
[0, 0, 0, 0, 0, 0, 0]),
transitions={
"done": "RESET_LEFT",
"failed": "RESET_LEFT"
})
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'})
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
with sm:
# Start challenge via StartChallengeRobust
smach.StateMachine.add("START_CHALLENGE_ROBUST",
states.Initialize(robot),
transitions={
'initialized': "SAY_1",
"abort": "Aborted"
})
smach.StateMachine.add('SAY_1',
states.Say(robot, "Please ask me question one"),
transitions={'spoken': 'QUESTION_1'})
smach.StateMachine.add('QUESTION_1',
HearQuestion(robot),
transitions={'answered': 'SAY_2'})
smach.StateMachine.add('SAY_2',
states.Say(robot, "Please ask me question two"),
transitions={'spoken': 'QUESTION_2'})
smach.StateMachine.add('QUESTION_2',
HearQuestion(robot),
transitions={'answered': 'SAY_3'})
smach.StateMachine.add('SAY_3',
states.Say(robot,
"Please ask me question three"),
transitions={'spoken': 'QUESTION_3'})
smach.StateMachine.add('QUESTION_3',
HearQuestion(robot),
transitions={'answered': 'SAY_4'})
smach.StateMachine.add('SAY_4',
states.Say(robot,
"Please ask me question four"),
transitions={'spoken': 'QUESTION_4'})
smach.StateMachine.add('QUESTION_4',
HearQuestion(robot),
transitions={'answered': 'SAY_5'})
smach.StateMachine.add('SAY_5',
states.Say(robot,
"Please ask me question five"),
transitions={'spoken': 'QUESTION_5'})
smach.StateMachine.add('QUESTION_5',
HearQuestion(robot),
transitions={'answered': 'AT_END'})
smach.StateMachine.add('AT_END',
states.Say(robot, "That was all folks!"),
transitions={'spoken': 'Done'})
return sm
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
self.turned = False
with self:
smach.StateMachine.add('SAY_SEARCHING_CROWD',
states.Say(robot,
"I'm looking for the crowd.",
block=False),
transitions={'spoken': 'RECOGNIZE_PERSONS'})
smach.StateMachine.add('RECOGNIZE_PERSONS',
RecognizePersons(robot),
transitions={
'succeeded': 'succeeded',
'failed': 'failed'
})
def __init__(self, robot, drop_zone_id):
"""
:param robot: robot object
:param drop_designator: EdEntityDesignator designating the collection zone
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"])
arm_designator = ds.OccupiedArmDesignator(robot=robot, arm_properties={})
with self:
smach.StateMachine.add("GO_TO_COLLECTION_ZONE",
states.NavigateToWaypoint(robot, ds.EntityByIdDesignator(robot, id=drop_zone_id),
radius=0.5),
transitions={"arrived": "DROP_TRASH",
"goal_not_defined": "aborted",
"unreachable": "OPEN_DOOR_PLEASE"})
smach.StateMachine.add("OPEN_DOOR_PLEASE",
states.Say(robot, "Can you please open the door for me? It seems blocked!"),
transitions={"spoken": "WAIT_FOR_DOOR_OPEN"})
smach.StateMachine.add("WAIT_FOR_DOOR_OPEN",
states.WaitTime(robot=robot, waittime=5),
transitions={"waited": "GO_TO_COLLECTION_ZONE2",
"preempted": "GO_TO_COLLECTION_ZONE2"})
smach.StateMachine.add("GO_TO_COLLECTION_ZONE2",
states.NavigateToWaypoint(robot, ds.EntityByIdDesignator(robot, id=drop_zone_id),
radius=0.5),
transitions={"arrived": "DROP_TRASH",
"goal_not_defined": "aborted",
"unreachable": "failed"})
smach.StateMachine.add("DROP_TRASH", DropTrash(robot=robot, arm_designator=arm_designator),
transitions={"succeeded": "succeeded",
"failed": "HANDOVER"})
smach.StateMachine.add("HANDOVER",
states.HandoverToHuman(robot=robot, arm_designator=arm_designator),
transitions={"succeeded": "succeeded",
"failed": "failed"})
def __init__(self, robot, object_category_des, room_des):
smach.StateMachine.__init__(self, outcomes=["done", "failed"])
room_id_des = ds.AttrDesignator(room_des, "id", resolve_type=str)
with self:
smach.StateMachine.add("LOOK_INTO_ROOM", robot_smach_states.NavigateToRoom(robot, room_des, room_des),
transitions={"arrived": "SAY_COME_TO_ME",
"unreachable": "SAY_COME_TO_ME",
"goal_not_defined": "SAY_COME_TO_ME"})
smach.StateMachine.add("SAY_COME_TO_ME", robot_smach_states.Say(robot,
"Operator, please come to me in the {room}",
room=room_id_des, block=True),
transitions={"spoken": "WAIT_FOR_OPERATOR"})
smach.StateMachine.add("WAIT_FOR_OPERATOR", robot_smach_states.WaitTime(4),
transitions={"waited": "ASK_WHICH_CATERGORY",
"preempted": "ASK_WHICH_CATERGORY"})
smach.StateMachine.add("ASK_WHICH_CATERGORY", AskWhichCategory(robot,
ds.Designator(challenge_knowledge.category_grammar),
object_category_des),
transitions={"done": "done"})
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
self.target_destination = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.default_place)
self.car_waypoint = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.waypoint_car['id'])
self.place_position = ds.LockingDesignator(ds.EmptySpotDesignator(
robot,
self.target_destination,
name="placement",
area=challenge_knowledge.default_area),
name="place_position")
self.empty_arm_designator = ds.UnoccupiedArmDesignator(
robot, {}, name="empty_arm_designator")
# With the empty_arm_designator locked, it will ALWAYS resolve to the same arm, unless it is unlocked.
# For this challenge, unlocking is not needed.
self.bag_arm_designator = self.empty_arm_designator.lockable()
self.bag_arm_designator.lock()
# We don't actually grab something, so there is no need for an actual thing to grab
self.current_item = ds.VariableDesignator(Entity(
"dummy", "dummy", "/{}/base_link".format(robot.robot_name),
kdl_conversions.kdl_frame_from_XYZRPY(0.6, 0, 0.5), None, {}, [],
datetime.datetime.now()),
name="current_item")
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'SET_INITIAL_POSE',
'abort': 'Aborted'
})
smach.StateMachine.add('SET_INITIAL_POSE',
states.SetInitialPose(
robot,
challenge_knowledge.starting_point),
transitions={
'done': 'FOLLOW_OPERATOR',
'preempted': 'Aborted',
'error': 'FOLLOW_OPERATOR'
})
# Follow the operator until (s)he states that you have arrived at the "car".
# smach.StateMachine.add('FOLLOW_OPERATOR',
# states.FollowOperator(robot, operator_timeout=30, ask_follow=True, learn_face=True, replan=True),
# transitions={'stopped': 'ASK_FOR_TASK',
# 'lost_operator': 'ASK_FOR_TASK',
# 'no_operator': 'FOLLOW_OPERATOR'})
# Use NEW:
smach.StateMachine.add('FOLLOW_OPERATOR',
states.FollowOperator2(robot),
transitions={
'Done': 'ASK_FOR_TASK',
'Failed': 'ASK_FOR_TASK',
'Aborted': 'FOLLOW_OPERATOR'
})
smach.StateMachine.add('ASK_FOR_TASK',
states.Say(robot,
["Are we at the car already?"],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'WAIT_FOR_TASK'})
smach.StateMachine.add('WAIT_FOR_TASK',
states.HearOptions(robot, ['yes', 'no']),
transitions={
'no': 'FOLLOW_OPERATOR',
'yes': 'CONFIRM_CAR_LOCATION',
'no_result': 'ASK_FOR_TASK'
})
smach.StateMachine.add(
'CONFIRM_CAR_LOCATION',
states.Say(
robot,
["OK, I will remember this location as the car location."],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'REMEMBER_CAR_LOCATION'})
smach.StateMachine.add('REMEMBER_CAR_LOCATION',
hmc_states.StoreCarWaypoint(robot),
transitions={
'success': 'ASK_FOR_DESTINATION',
'abort': 'Aborted'
})
smach.StateMachine.add(
'ASK_FOR_DESTINATION',
states.Say(robot, ["Where should I bring the groceries?"],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'RECEIVE_DESTINATION'})
smach.StateMachine.add(
'RECEIVE_DESTINATION',
hmc_states.WaitForOperatorCommand(
robot,
possible_commands=challenge_knowledge.destinations,
commands_as_userdata=True,
target=self.target_destination),
transitions={
'success': 'GRAB_ITEM',
'abort': 'Aborted'
})
#
# smach.StateMachine.add('CONFIRM_DESTINATION',
# states.Say(robot, [
# "I will deliver the groceries to the %s" % ds.EntityByIdDesignator(self.target_destination)],
# block=True,
# look_at_standing_person=True),
# transitions={'spoken': 'GRAB_ITEM'})
# Grab the item (bag) the operator hands to the robot, when they are at the "car".
smach.StateMachine.add(
'GRAB_ITEM',
# states.HandoverFromHuman(robot, self.bag_arm_designator, "current_item",
# self.current_item,
# arm_configuration=challenge_knowledge.carrying_bag_pose),
# transitions={'succeeded': 'ARM_DRIVING_POSE',
# 'timeout': 'BACKUP_CLOSE_GRIPPER',
# # For now in simulation timeout is considered a success.
# 'failed': 'BACKUP_CLOSE_GRIPPER'})
states.Say(robot, [
"I can't pick up the groceries since I don't have arms. Please place them in my basket."
],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'WAIT_FOR_GRAB_ITEM'})
smach.StateMachine.add('WAIT_FOR_GRAB_ITEM',
states.WaitTime(robot),
transitions={
'waited': 'SAY_GOING_TO_ROOM',
'preempted': 'Aborted'
})
# smach.StateMachine.add('BACKUP_CLOSE_GRIPPER',
# states.SetGripper(robot, self.bag_arm_designator, gripperstate=GripperState.CLOSE),
# transitions={'succeeded': 'ARM_DRIVING_POSE',
# 'failed': 'ARM_DRIVING_POSE'})
#
# smach.StateMachine.add('ARM_DRIVING_POSE',
# states.ArmToJointConfig(robot, self.bag_arm_designator,
# challenge_knowledge.driving_bag_pose),
# transitions={'succeeded': 'SAY_GOING_TO_ROOM',
# 'failed': 'SAY_GOING_TO_ROOM'})
smach.StateMachine.add(
'SAY_GOING_TO_ROOM',
states.Say(robot, [
"Let me bring in your groceries",
"Helping you carry stuff", "I'm going back inside"
],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'GOTO_DESTINATION'})
smach.StateMachine.add(
'GOTO_DESTINATION',
states.NavigateToSymbolic(
robot, {self.target_destination: "in_front_of"},
self.target_destination),
transitions={
'arrived': 'PUTDOWN_ITEM',
'unreachable': 'TURN_180_TO_REPLAN',
# implement avoid obstacle behaviour later
'goal_not_defined': 'Aborted'
})
smach.StateMachine.add(
'TURN_180_TO_REPLAN',
hmc_states.TurnToReplan(robot),
transitions={
'success': 'GOTO_DESTINATION_BACKUP',
'abort': 'GOTO_DESTINATION_BACKUP',
# implement avoid obstacle behaviour later
#'goal_not_defined': 'Aborted'})
})
smach.StateMachine.add(
'GOTO_DESTINATION_BACKUP',
states.NavigateToSymbolic(
robot, {self.target_destination: "in_front_of"},
self.target_destination),
transitions={
'arrived': 'PUTDOWN_ITEM',
'unreachable': 'PUTDOWN_ITEM',
# implement avoid obstacle behaviour later
'goal_not_defined': 'Aborted'
})
# Put the item (bag) down when the robot has arrived at the "drop-off" location (house).
smach.StateMachine.add(
'PUTDOWN_ITEM',
# hmc_states.DropBagOnGround(robot, self.bag_arm_designator,
# challenge_knowledge.drop_bag_pose),
states.Say(robot, [
"I can't put the groceries down since I have no arms. Please take them from my basket and put it down."
],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'WAIT_FOR_PUTDOWN_ITEM'})
smach.StateMachine.add('WAIT_FOR_PUTDOWN_ITEM',
states.WaitTime(robot),
transitions={
'waited': 'ASKING_FOR_HELP',
'preempted': 'Aborted'
})
smach.StateMachine.add(
'ASKING_FOR_HELP',
# TODO: look and then face new operator
states.Say(
robot,
"Please follow me and help me carry groceries into the house",
block=True,
look_at_standing_person=True),
transitions={'spoken': 'GOTO_CAR'}) #'LEARN_OPERATOR'})
# smach.StateMachine.add('LEARN_OPERATOR',
# hmc_states.LearnOperator(robot),
# transitions={'learned': 'GOTO_CAR',
# 'failed': 'GOTO_CAR'})
smach.StateMachine.add(
'GOTO_CAR',
states.NavigateToWaypoint(
robot, self.car_waypoint,
challenge_knowledge.waypoint_car['radius']),
# TODO: detect closed door
transitions={
'unreachable': 'OPEN_DOOR',
'arrived': 'AT_END',
'goal_not_defined': 'Aborted'
})
smach.StateMachine.add(
'OPEN_DOOR',
# TODO: implement functionality
states.Say(robot, "Please open the door for me"),
transitions={'spoken': 'GOTO_CAR'})
smach.StateMachine.add(
'AT_END',
states.Say(robot, [
"We arrived at the car, goodbye",
"You have reached your destination, goodbye",
"The car is right here, see you later!"
],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "help_me_carry")
def __init__(self, robot, operator_name, drink_designator,
available_drinks_designator, unavailable_drink_designator,
objects):
# type (Robot, VariableDesignator) -> None
"""
Initialization method
:param robot: robot api object
:param operator_name: (EntityDesignator) in which the operator's name is stored
:param drink_designator: (VariableDesignator) in which the drink to fetch is stored
:param available_drinks_designator: (VariableDesignator) in which the available drinks are stored
:param unavailable_drink_designator: (VariableDesignator) in which the unavailable drink is stored
:param objects: Objects from common knowledge
"""
smach.StateMachine.__init__(
self, outcomes=["succeeded", "failed", "aborted"])
with self:
# Ask for order
smach.StateMachine.add("RISE_FOR_HMI",
states.RiseForHMI(robot=robot),
transitions={
"succeeded": "ASK_FOR_ORDER",
"failed": "ASK_FOR_ORDER"
})
smach.StateMachine.add(
"ASK_FOR_ORDER",
states.Say(robot=robot,
sentence=random.choice([
"What would you like to drink?",
"Which drink would you like?",
"What would you like to quench your thirst?",
"What can I get you?",
"Please tell me which drink you want"
]),
look_at_standing_person=True),
transitions={"spoken": "CHECK_AVAILABILITY"})
# Check availability
smach.StateMachine.add(
"CHECK_AVAILABILITY",
CheckAvailability(
robot=robot,
drink_designator=drink_designator,
available_drinks_designator=available_drinks_designator,
unavailable_drink_designator=unavailable_drink_designator,
objects=objects),
transitions={
"available": "ASK_FOR_CONFIRMATION",
"unavailable": "STATE_UNAVAILABLE",
"aborted": "aborted",
"bad_operator": "SAY_BAD_OPERATOR"
})
# Ask for confirmation
smach.StateMachine.add(
"ASK_FOR_CONFIRMATION",
states.Say(robot=robot,
sentence=DescriptionStrDesignator(
"confirmation_available", drink_designator,
operator_name),
look_at_standing_person=True),
transitions={"spoken": "HEAR_CONFIRMATION"})
# Hear the confirmation
smach.StateMachine.add("HEAR_CONFIRMATION",
states.HearOptions(robot=robot,
options=["yes", "no"]),
transitions={
"yes": "succeeded",
"no": "ASK_FOR_ORDER",
"no_result": "ASK_FOR_ORDER"
}) # ToDo: fallback?
# Announce that the drink is unavailable
smach.StateMachine.add(
"STATE_UNAVAILABLE",
states.Say(robot=robot,
sentence=DescriptionStrDesignator(
"state_unavailable",
unavailable_drink_designator, operator_name),
look_at_standing_person=True),
transitions={"spoken": "ASK_FOR_ORDER"})
# Tell the operator to stop f*****g around!
smach.StateMachine.add(
"SAY_BAD_OPERATOR",
states.Say(
robot=robot,
sentence=
"I'm not going to ask you again as I have already informed you multiple times that your request is unavailable",
look_at_standing_person=True),
transitions={"spoken": "aborted"})
def __init__(self, robot):
""" Initialization method
:param robot: robot api object
"""
smach.StateMachine.__init__(
self, outcomes=["succeeded", "failed", "aborted"])
# Create designators
trashbin_designator = ds.EdEntityDesignator(
robot=robot,
id=CHALLENGE_KNOWLEDGE.trashbin_id,
name='trashbin_designator')
# Look if there is a second trash bin present
# trashbin_designator2 = None
if hasattr(CHALLENGE_KNOWLEDGE, "trashbin_id2"):
trashbin_designator2 = ds.EdEntityDesignator(
robot=robot,
id=CHALLENGE_KNOWLEDGE.trashbin_id2,
name='trashbin_designator2')
next_state = "HELPER_WAYPOINT"
rospy.loginfo("There is a second trash bin")
else:
rospy.loginfo("There is no second trash bin")
next_state = "ANNOUNCE_END"
# drop_zone_designator = ds.EdEntityDesignator(robot=robot, id=CHALLENGE_KNOWLEDGE.drop_zone_id)
helper_waypoint_designator = ds.EdEntityDesignator(
robot=robot, id=CHALLENGE_KNOWLEDGE.helper_waypoint)
end_waypoint_designator = ds.EdEntityDesignator(
robot=robot, id=CHALLENGE_KNOWLEDGE.end_waypoint)
arm_designator = self.empty_arm_designator = ds.UnoccupiedArmDesignator(
robot, {}, name="empty_arm_designator")
with self:
smach.StateMachine.add("START_CHALLENGE_ROBUST",
states.StartChallengeRobust(
robot,
CHALLENGE_KNOWLEDGE.starting_point),
transitions={
"Done": "SAY_START_CHALLENGE",
"Aborted": "SAY_START_CHALLENGE",
"Failed": "SAY_START_CHALLENGE"
})
smach.StateMachine.add("SAY_START_CHALLENGE",
states.Say(
robot,
"I will start cleaning up the trash",
block=True),
transitions={'spoken': "PICK_UP_TRASH"})
smach.StateMachine.add("PICK_UP_TRASH",
PickUpTrash(
robot=robot,
trashbin_designator=trashbin_designator,
arm_designator=arm_designator),
transitions={
"succeeded": "DROP_DOWN_TRASH",
"failed": "HELPER_WAYPOINT",
"aborted": "ANNOUNCE_END"
})
smach.StateMachine.add(
"DROP_DOWN_TRASH",
DropDownTrash(robot=robot,
drop_zone_id=CHALLENGE_KNOWLEDGE.drop_zone_id),
transitions={
"succeeded": "ANNOUNCE_TASK",
"failed": "failed",
"aborted": "aborted"
})
smach.StateMachine.add(
"ANNOUNCE_TASK",
states.Say(robot,
"First bag has been dropped at the collection zone",
block=False),
transitions={'spoken': next_state})
if next_state == "HELPER_WAYPOINT":
smach.StateMachine.add(
"HELPER_WAYPOINT",
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=helper_waypoint_designator),
transitions={
"arrived": "PICK_UP_TRASH2",
"goal_not_defined": "PICK_UP_TRASH2",
"unreachable": "PICK_UP_TRASH2"
})
smach.StateMachine.add(
"PICK_UP_TRASH2",
PickUpTrash(robot=robot,
trashbin_designator=trashbin_designator2,
arm_designator=arm_designator),
transitions={
"succeeded": "DROP_DOWN_TRASH2",
"failed": "ANNOUNCE_END",
"aborted": "ANNOUNCE_END"
})
smach.StateMachine.add(
"DROP_DOWN_TRASH2",
DropDownTrash(
robot=robot,
drop_zone_id=CHALLENGE_KNOWLEDGE.drop_zone_id),
transitions={
"succeeded": "ANNOUNCE_TASK2",
"failed": "failed",
"aborted": "aborted"
})
smach.StateMachine.add(
"ANNOUNCE_TASK2",
states.Say(
robot,
"Second bag has been dropped at the collection zone."
"All the thrash has been taken care of",
block=False),
transitions={'spoken': 'ANNOUNCE_END'})
smach.StateMachine.add("ANNOUNCE_END",
states.Say(
robot,
"I have finished taking out the trash.",
block=False),
transitions={'spoken': 'NAVIGATE_OUT'})
smach.StateMachine.add(
"NAVIGATE_OUT",
states.NavigateToWaypoint(
robot=robot, waypoint_designator=end_waypoint_designator),
transitions={
"arrived": "succeeded",
"goal_not_defined": "succeeded",
"unreachable": "succeeded"
})
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
with sm:
# Start challenge via StartChallengeRobust
smach.StateMachine.add("START_CHALLENGE_ROBUST",
states.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',
states.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',
states.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',
states.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",
states.AskContinue(robot, 30),
transitions={
'continue': 'SAY_CONTINUEING',
'no_response': 'SAY_CONTINUEING'
})
smach.StateMachine.add(
'SAY_CONTINUEING',
states.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',
states.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',
states.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',
states.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',
states.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',
states.Say(robot, "Goodbye"),
transitions={'spoken': 'Done'})
analyse_designators(sm, "rips")
return sm
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# start_waypoint = ds.EntityByIdDesignator(robot, id="manipulation_init_pose", name="start_waypoint")
pdf_writer = WritePdf(robot=robot)
with self:
single_item = ManipulateMachine(robot, pdf_writer=pdf_writer)
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized':
'SAY_UNABLE_TO_OPEN_DOOR',
'abort': 'Aborted'
})
smach.StateMachine.add('SAY_UNABLE_TO_OPEN_DOOR',
states.Say(
robot,
"I am unable to open the shelf door, "
"can you please open it for me?"),
transitions={'spoken': 'AWAIT_START'})
smach.StateMachine.add("AWAIT_START",
states.AskContinue(robot),
transitions={
'continue': "MOVE_TABLE",
'no_response': 'AWAIT_START'
})
cabinet = ds.EntityByIdDesignator(robot, id=CABINET)
room = ds.EntityByIdDesignator(robot, id=ROOM)
@smach.cb_interface(outcomes=["done"])
def move_table(userdata=None, manipulate_machine=None):
""" Moves the entities for this challenge to the correct poses"""
# Determine where to perform the challenge
robot_pose = robot.base.get_location()
ENTITY_POSES.sort(key=lambda tup:
(tup[0].frame.p - robot_pose.frame.p).Norm())
cabinet_id = ENTITY_POSES[0][2]
table_id = ENTITY_POSES[0][3]
# Update the world model
robot.ed.update_entity(id="balcony_shelf",
frame_stamped=FrameStamped(
kdl.Frame(kdl.Rotation(),
kdl.Vector(0.0, 3.0,
0.0)),
frame_id="map"))
robot.ed.update_entity(id=cabinet_id,
frame_stamped=ENTITY_POSES[0][0])
robot.ed.update_entity(id=table_id,
frame_stamped=ENTITY_POSES[0][1])
# Update designators
cabinet.id_ = ENTITY_POSES[0][2]
room.id_ = ENTITY_POSES[0][4]
# Update manipulate machine
manipulate_machine.place_entity_designator.id_ = cabinet_id
manipulate_machine.place_designator._area = ENTITY_POSES[0][5]
manipulate_machine.place_designator.place_location_designator.id = cabinet_id
manipulate_machine.table_designator.id_ = table_id
manipulate_machine.cabinet.id_ = ENTITY_POSES[0][2]
return "done"
smach.StateMachine.add("MOVE_TABLE",
smach.CBState(move_table,
cb_args=[single_item]),
transitions={'done': 'NAV_TO_START'})
smach.StateMachine.add("NAV_TO_START",
states.NavigateToSymbolic(
robot, {cabinet: "in_front_of"},
cabinet),
transitions={
'arrived': 'INSPECT_SHELVES',
'unreachable': 'INSPECT_SHELVES',
'goal_not_defined': 'INSPECT_SHELVES'
})
smach.StateMachine.add("INSPECT_SHELVES",
InspectShelves(robot, cabinet),
transitions={
'succeeded': 'WRITE_PDF_SHELVES',
'nothing_found': 'WRITE_PDF_SHELVES',
'failed': 'WRITE_PDF_SHELVES'
})
smach.StateMachine.add("WRITE_PDF_SHELVES",
pdf_writer,
transitions={"done": "RANGE_ITERATOR"})
# Begin setup iterator
# The exhausted argument should be set to the prefered state machine outcome
range_iterator = smach.Iterator(
outcomes=['succeeded',
'failed'], # Outcomes of the iterator state
input_keys=[],
output_keys=[],
it=lambda: range(5),
it_label='index',
exhausted_outcome='succeeded')
with range_iterator:
smach.Iterator.set_contained_state(
'SINGLE_ITEM',
single_item,
loop_outcomes=['succeeded', 'failed'])
smach.StateMachine.add('RANGE_ITERATOR', range_iterator, {
'succeeded': 'AT_END',
'failed': 'Aborted'
})
# End setup iterator
smach.StateMachine.add('AT_END',
states.Say(robot, "Goodbye"),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "manipulation")
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# - - - - - - - - - - - - - - - - - - - Callback States - - - - - - - - - - - - - - - - - - -
@smach.cb_interface(outcomes=['spoken'])
def sayHelloCB(userdata=None):
printOk("sayHelloCB")
robot.speech.speak("Hello " + personNameDes.resolve() + "!",
block=False)
return 'spoken'
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'INIT_WM',
'abort': 'Aborted'
})
smach.StateMachine.add(
"INIT_WM",
states.InitializeWorldModel(robot),
transitions={'done': 'ENTER_ROOM_CONTAINER'})
# smach.StateMachine.add( 'SELECT_NEXT_CONTAINER',
# test_states.SelectNextContainer(robot, containerResultDes),
# transitions={ 'go_to_enter_room':'ENTER_ROOM_CONTAINER',
# 'go_to_wait_person':'WAIT_PERSON_CONTAINER',
# 'go_to_pick_up':'PICK_UP_CONTAINER',
# 'go_to_recognize_people':'RECOGNIZE_PEOPLE_CONTAINER',
# 'go_to_search_people':'SEARCH_PEOPLE_CONTAINER',
# 'go_to_end_challenge':'END_CHALLENGE'})
# Navigation test
smach.StateMachine.add('ENTER_ROOM_CONTAINER',
EnterRoomContainer(robot),
transitions={
'container_success':
'WAIT_PERSON_CONTAINER',
'container_failed':
'WAIT_PERSON_CONTAINER'
})
# Human Interaction Test
smach.StateMachine.add('WAIT_PERSON_CONTAINER',
WaitPersonContainer(robot),
transitions={
'container_success':
'LEARN_NAME_CONTAINER',
'container_failed':
'WAIT_PERSON_CONTAINER'
})
# Speech Test
smach.StateMachine.add('LEARN_NAME_CONTAINER',
LearnNameContainer(robot, personNameDes),
transitions={
'container_failed': 'SAY_HELLO',
'container_success': 'SAY_HELLO'
})
smach.StateMachine.add(
'SAY_HELLO',
smach.CBState(sayHelloCB),
transitions={'spoken': 'LEARN_FACE_CONTAINER'})
# Face Learning Test
smach.StateMachine.add('LEARN_FACE_CONTAINER',
LearnFaceContainer(robot, personNameDes),
transitions={
'container_success':
'RECOGNIZE_PEOPLE_CONTAINER',
'container_failed':
'RECOGNIZE_PEOPLE_CONTAINER'
})
# Face Recognition Test
smach.StateMachine.add('RECOGNIZE_PEOPLE_CONTAINER',
RecognizePeopleContainer(robot),
transitions={
'container_success':
'PICK_UP_CONTAINER',
'container_failed': 'PICK_UP_CONTAINER'
})
# Manipulation Test
smach.StateMachine.add('PICK_UP_CONTAINER',
PickUpContainer(robot, objectsIDsDes),
transitions={
'container_success':
'SEARCH_PEOPLE_CONTAINER',
'container_failed':
'SEARCH_PEOPLE_CONTAINER'
})
# Face Recogniton Test
smach.StateMachine.add('SEARCH_PEOPLE_CONTAINER',
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'})
def __init__(self, robot, objectsIDsDesLocal):
smach.StateMachine.__init__(
self, outcomes=['container_success', 'container_failed'])
with self:
smach.StateMachine.add(
'SAY_STARTING_TEST',
states.Say(robot,
"Starting object pickup test test",
block=False),
transitions={'spoken': 'SAY_SEARCHING_OBJECTS'})
smach.StateMachine.add(
'SAY_SEARCHING_OBJECTS',
states.Say(
robot,
"I'm going to the dinning table to search for objects",
block=False),
transitions={'spoken': 'NAV_TO_TABLE'})
smach.StateMachine.add(
'NAV_TO_TABLE',
states.NavigateToSymbolic(
robot, {
EdEntityDesignator(robot,
id=challenge_knowledge.INSPECT_ROOM_ID):
"in"
},
EdEntityDesignator(
robot, id=challenge_knowledge.INSPECT_ENTITY_ID)),
transitions={
'arrived': 'SEGMENT_OBJECTS',
'unreachable': 'SAY_FAILED_NAV_TO_TABLE',
'goal_not_defined': 'SAY_FAILED_NAV_TO_TABLE'
})
smach.StateMachine.add(
'SAY_FAILED_NAV_TO_TABLE',
states.Say(
robot,
"I could not reach the table, but i will try to continue.",
block=True),
transitions={'spoken': 'SEGMENT_OBJECTS'})
smach.StateMachine.add(
"SEGMENT_OBJECTS",
states.SegmentObjects(
robot, objectsIDsDesLocal.writeable,
EdEntityDesignator(
robot, id=challenge_knowledge.INSPECT_ENTITY_ID),
"on_top_of"),
transitions={'done': 'PICKUP_OBJECT'})
smach.StateMachine.add('PICKUP_OBJECT',
test_states.PickUpRandomObj(
robot, objectsIDsDesLocal),
transitions={
'succeeded': 'SAY_I_HAVE_OBJ',
'failed': 'SAY_PICKUP_FAILED',
'no_objects': 'SAY_NO_OBJECTS'
})
smach.StateMachine.add('SAY_I_HAVE_OBJ',
states.Say(robot,
"I have the object!",
block=False),
transitions={'spoken': 'container_success'})
smach.StateMachine.add(
'SAY_PICKUP_FAILED',
states.Say(
robot,
"Something went wrong and i could not pick up the object!",
block=False),
transitions={'spoken': 'container_failed'})
smach.StateMachine.add('SAY_NO_OBJECTS',
states.Say(
robot,
"I don't see any objects to pick up!",
block=False),
transitions={'spoken': 'container_failed'})
