def __init__(self, robot, area, name):
""" Constructor
:param robot: robot object
:param area: (str) if a waypoint "<area>_waypoint" is present in the world model, the robot will navigate
to this waypoint. Else, it will navigate to the room called "<area>"
:param name: (str) Name of the person to look for
"""
smach.StateMachine.__init__(self, outcomes=["found", "not_found"])
waypoint_designator = ds.EntityByIdDesignator(robot=robot,
id=area + "_waypoint")
room_designator = ds.EntityByIdDesignator(robot=robot, id=area)
with self:
smach.StateMachine.add("DECIDE_NAVIGATE_STATE",
_DecideNavigateState(
robot=robot,
waypoint_designator=waypoint_designator,
room_designator=room_designator),
transitions={
"waypoint": "NAVIGATE_TO_WAYPOINT",
"room": "NAVIGATE_TO_ROOM",
"none": "not_found"
})
smach.StateMachine.add("NAVIGATE_TO_WAYPOINT",
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=waypoint_designator,
radius=0.15),
transitions={
"arrived": "FIND_PERSON",
"unreachable": "not_found",
"goal_not_defined": "not_found"
})
smach.StateMachine.add("NAVIGATE_TO_ROOM",
states.NavigateToRoom(
robot=robot,
entity_designator_room=room_designator),
transitions={
"arrived": "FIND_PERSON",
"unreachable": "not_found",
"goal_not_defined": "not_found"
})
# Wait for the operator to appear and detect what he's pointing at
smach.StateMachine.add("FIND_PERSON",
FindPerson(robot=robot, person_label=name),
transitions={
"found": "found",
"failed": "not_found"
})
def __init__(self, robot, room, found_people_designator,
look_range=(-np.pi/2, np.pi/2),
look_steps=8):
"""
Constructor
:param robot: robot object
:param area: (str) if a waypoint "<area>_waypoint" is present in the world model, the robot will navigate
to this waypoint. Else, it will navigate to the room called "<area>"
:param name: (str) Name of the person to look for
:param discard_other_labels: (bool) Whether or not to discard faces based on label
:param found_person_designator: (Designator) A designator that will resolve to the found object
"""
smach.StateMachine.__init__(self, outcomes=["found", "not_found"])
waypoint_designator = ds.EntityByIdDesignator(robot=robot, id=room + "_waypoint")
room_designator = ds.EntityByIdDesignator(robot=robot, id=room)
ds.check_type(found_people_designator, [Entity])
ds.is_writeable(found_people_designator)
with self:
smach.StateMachine.add("DECIDE_NAVIGATE_STATE",
_DecideNavigateState(robot=robot, waypoint_designator=waypoint_designator,
room_designator=room_designator),
transitions={"waypoint": "NAVIGATE_TO_WAYPOINT",
"room": "NAVIGATE_TO_ROOM",
"none": "not_found"})
smach.StateMachine.add("NAVIGATE_TO_WAYPOINT",
states.NavigateToWaypoint(robot=robot,
waypoint_designator=waypoint_designator, radius=0.15),
transitions={"arrived": "FIND_PEOPLE",
"unreachable": "not_found",
"goal_not_defined": "not_found"})
smach.StateMachine.add("NAVIGATE_TO_ROOM", states.NavigateToRoom(robot=robot,
entity_designator_room=room_designator),
transitions={"arrived": "FIND_PEOPLE",
"unreachable": "not_found",
"goal_not_defined": "not_found"})
# Wait for the operator to appear and detect what he's pointing at
smach.StateMachine.add("FIND_PEOPLE",
FindPeople(robot=robot,
query_entity_designator=room_designator,
found_people_designator=found_people_designator,
speak=True,
look_range=look_range,
look_steps=look_steps),
transitions={"found": "found",
"failed": "not_found"})
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):
# type: (Robot) -> str
"""
Initialization method
:param robot: robot api object
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"])
# Designators
bar_designator = ds.EdEntityDesignator(robot=robot, id=challenge_knowledge.bar_id, name='bar_des')
room_designator = ds.EdEntityDesignator(robot=robot, id=challenge_knowledge.room_id, name='room_des')
objects_list_des = ds.VariableDesignator(resolve_type=[ClassificationResult], name='objects_list_des')
unav_drink_des = ds.VariableDesignator(resolve_type=str, name='unav_drink_str_des')
hacky_arm_des = ds.VariableDesignator(initial_value=robot.get_arm(), name='hacky_arm')
with self:
smach.StateMachine.add("INITIALIZE",
states.Initialize(robot=robot),
transitions={"initialized": "INITIAL_POSE",
"abort": "aborted"})
smach.StateMachine.add("INITIAL_POSE",
states.SetInitialPose(robot,
challenge_knowledge.starting_point),
transitions={"done": "INSPECT_BAR",
"preempted": "aborted",
"error": "INSPECT_BAR"})
# Inspect bar and store the list of available drinks
smach.StateMachine.add("INSPECT_BAR",
states.Inspect(robot=robot,
entityDes=bar_designator,
navigation_area="in_front_of",
objectIDsDes=objects_list_des),
transitions={"done": "INSPECT_FALLBACK", #TODO: Change to CHECK_INSPECT_RESULT after RWC2019
"failed": "INSPECT_FALLBACK"})
smach.StateMachine.add("CHECK_INSPECT_RESULT",
CheckInspect(objects_list_des,
[ClassificationResult]),
transitions={"true": "IDENTIFY_UNAVAILABLE_DRINK",
"false": "INSPECT_FALLBACK"})
smach.StateMachine.add("IDENTIFY_UNAVAILABLE_DRINK",
IdentifyUnavailableDrinkFromRecognitions(objects=common_knowledge.objects,
classification_list_designator=objects_list_des,
unavailable_drink_designator=unav_drink_des.writeable,
max_unavailable_drinks=challenge_knowledge.MAX_UNAVAILABLE_DRINKS),
transitions={"done": "NAVIGATE_TO_ROOM",
"failed": "INSPECT_FALLBACK"})
# Inspect fallback - ask the bartender which drink is unavailable and store the unavailable drink
smach.StateMachine.add("INSPECT_FALLBACK",
AskAvailability(robot=robot,
unavailable_drink_designator=unav_drink_des.writeable,
objects=common_knowledge.objects),
transitions={"succeeded": "RESET_ROBOT",
"failed": "RESET_ROBOT"})
smach.StateMachine.add("RESET_ROBOT",
states.ArmToJointConfig(robot=robot,
arm_designator=hacky_arm_des,
configuration="reset"),
transitions={'succeeded': "NAVIGATE_TO_ROOM",
'failed': "NAVIGATE_TO_ROOM"})
# Navigate to the predefined room
smach.StateMachine.add("NAVIGATE_TO_ROOM",
states.NavigateToRoom(robot=robot, entity_designator_room=room_designator),
transitions={"arrived": "SAY_HI",
"unreachable": "SAY_HI",
"goal_not_defined": "aborted"})
smach.StateMachine.add("SAY_HI",
states.Say(robot, "Hi, I am {}. I'll be your waiter today".format(robot.robot_name)),
transitions={"spoken": "SERVE_DRINK_1"})
# Explicitly add a new state for each drink, i.e., don't use a range iterator to make sure a new state
# is constructed every time
for idx in range(1, challenge_knowledge.NR_DRINKS + 1):
next_state = "SERVE_DRINK_{}".format(idx + 1) if idx < challenge_knowledge.NR_DRINKS else "SAY_DONE"
smach.StateMachine.add("SERVE_DRINK_{}".format(idx),
ServeOneDrink(robot=robot,
bar_designator=bar_designator,
room_id=challenge_knowledge.room_id,
room_designator=room_designator,
objects_list_des=objects_list_des,
unav_drink_des=unav_drink_des,
name_options=common_knowledge.names,
objects=common_knowledge.objects),
transitions={"succeeded": next_state,
"failed": next_state,
"aborted": next_state})
smach.StateMachine.add("SAY_DONE",
states.Say(robot, "My job here is done. Enjoy your day and see you next time"),
transitions={"spoken": "succeeded"})
def __init__(self, robot, bar_designator, room_id, room_designator,
objects_list_des, unav_drink_des, name_options, objects):
"""
Initialization method
:param robot: robot api object
:param bar_designator: (EntityDesignator) in which the bar location is stored
:param room_id: room ID from challenge knowledge
:param room_designator: (EntityDesignator) in which the room location is stored
:param objects_list_des: (VariableDesignator) in which the available drinks are stored
:param unav_drink_des: (VariableDesignator) in which the unavailable drink is stored
:param name_options: Names from common knowledge
:param objects: Objects from common knowledge
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"])
# Designators
arm_designator = ds.UnoccupiedArmDesignator(robot=robot,
arm_properties={},
name='arm_des').lockable()
drink_str_designator = ds.VariableDesignator(resolve_type=str, name='drink_str_des')
drink_designator = ds.EdEntityDesignator(robot=robot, type_designator=drink_str_designator, name='drink_des')
operator_name = ds.VariableDesignator(resolve_type=str, name='name_des')
operator_designator = ds.VariableDesignator(resolve_type=Entity, name='operator_des')
learn_check_designator = ds.VariableDesignator(initial_value=True, resolve_type=bool, name='learn_check_des')
hacky_arm_des = ds.VariableDesignator(initial_value=robot.get_arm(), name='hacky_arm_2')
with self:
# Lock the arm_designator
smach.StateMachine.add("LOCK_ARM",
states.LockDesignator(arm_designator),
transitions={'locked': "GET_ORDER"})
# Get order
smach.StateMachine.add("GET_ORDER",
GetOrder(robot=robot,
operator_name=operator_name,
drink_designator=drink_str_designator,
available_drinks_designator=objects_list_des,
unavailable_drink_designator=unav_drink_des,
name_options=name_options,
objects=objects,
learn_check_designator=learn_check_designator.writeable,
target_room_designator=room_designator),
transitions={"succeeded": "INSPECT_BAR",
"failed": "failed",
"aborted": "aborted"})
# Inspect bar
smach.StateMachine.add("INSPECT_BAR",
states.Inspect(robot=robot, entityDes=bar_designator, navigation_area="in_front_of"),
transitions={"done": "GRASP_DRINK",
"failed": "FALLBACK_BAR"})
# Grasp drink
smach.StateMachine.add("GRASP_DRINK",
states.Grab(robot=robot, item=drink_designator, arm=arm_designator),
transitions={"done": "FIND_OPERATOR",
"failed": "FALLBACK_BAR"})
# Inspect or grasp fallback - ask for assistance
smach.StateMachine.add("FALLBACK_BAR",
states.Say(robot=robot,
sentence=DescriptionStrDesignator("fallback_bar", drink_str_designator,
operator_name),
look_at_standing_person=True),
transitions={"spoken": "HANDOVER_FROM_HUMAN"})
# Handover from human fallback
smach.StateMachine.add("HANDOVER_FROM_HUMAN",
states.HandoverFromHuman(robot=robot, arm_designator=arm_designator,
grabbed_entity_designator=drink_designator),
transitions={"succeeded": "RESET_ROBOT_2",
"failed": "RESET_ROBOT_2",
"timeout": "RESET_ROBOT_2"})
smach.StateMachine.add("RESET_ROBOT_2",
states.ArmToJointConfig(robot=robot,
arm_designator=hacky_arm_des,
configuration="reset"),
transitions={'succeeded': "CHECK_LEARN_OPERATOR",
'failed': "CHECK_LEARN_OPERATOR"})
smach.StateMachine.add("CHECK_LEARN_OPERATOR",
states.CheckBool(learn_check_designator),
transitions={"true": "FIND_OPERATOR",
"false": "GO_TO_ROOM"})
smach.StateMachine.add("GO_TO_ROOM",
states.NavigateToRoom(robot=robot,
entity_designator_room=room_designator),
transitions={"arrived": "SAY_NOT_FOUND",
"unreachable": "failed",
"goal_not_defined": "aborted"})
# Find operator
smach.StateMachine.add("FIND_OPERATOR",
states.FindPersonInRoom(robot=robot,
area=room_id,
name=operator_name,
discard_other_labels=True,
found_entity_designator=operator_designator.writeable),
transitions={"found": "GOTO_OPERATOR",
"not_found": "SAY_NOT_FOUND"})
# Move to this person
smach.StateMachine.add("GOTO_OPERATOR",
states.NavigateToObserve(robot=robot,
entity_designator=operator_designator),
transitions={"arrived": "SAY_THE_NAME",
"unreachable": "SAY_NOT_FOUND",
"goal_not_defined": "SAY_NOT_FOUND"})
# Say not found
smach.StateMachine.add("SAY_NOT_FOUND",
states.Say(robot=robot,
sentence=DescriptionStrDesignator("not_found_operator",
drink_str_designator,
operator_name),
look_at_standing_person=True),
transitions={"spoken": "RISE_FOR_HMI_2"})
# Say the name
smach.StateMachine.add("SAY_THE_NAME",
states.Say(robot=robot,
sentence=DescriptionStrDesignator("found_operator",
drink_str_designator,
operator_name),
look_at_standing_person=True),
transitions={"spoken": "RISE_FOR_HMI_2"})
smach.StateMachine.add("RISE_FOR_HMI_2",
states.RiseForHMI(robot=robot),
transitions={"succeeded": "HAND_OVER",
"failed": "HAND_OVER"})
# Hand over the drink to the operator
smach.StateMachine.add("HAND_OVER",
states.HandoverToHuman(robot=robot,
arm_designator=arm_designator),
transitions={"succeeded": "UNLOCK_ARM",
"failed": "UNLOCK_ARM"})
smach.StateMachine.add("UNLOCK_ARM",
states.UnlockDesignator(arm_designator),
transitions={'unlocked': "RESET_ROBOT_3"})
smach.StateMachine.add("RESET_ROBOT_3",
states.ArmToJointConfig(robot=robot,
arm_designator=hacky_arm_des,
configuration="reset"),
transitions={'succeeded': "RETURN_TO_ROOM",
'failed': "RETURN_TO_ROOM"})
smach.StateMachine.add("RETURN_TO_ROOM",
states.NavigateToRoom(robot=robot,
entity_designator_room=room_designator),
transitions={"arrived": "succeeded",
"unreachable": "failed",
"goal_not_defined": "aborted"})