Skip to content

Commit

Permalink
Merge pull request #854 from tue-robotics/fix/style_serving_drinks
Browse files Browse the repository at this point in the history 
Challenge serving drinks: Fixed styling
  • Loading branch information
@ar13pit
ar13pit committed Jun 12, 2019
2 parents 36980d0 + 28cc1de commit 0d9edb8
Show file tree
Hide file tree
Showing 5 changed files with 234 additions and 360 deletions.
87 changes: 31 additions & 56 deletions challenge_serving_drinks/src/challenge_serving_drinks/drive_in.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,17 +9,22 @@
from .sd_states import AskAvailability

# Knowledge
CHALLENGE_KNOWLEDGE = knowledge_loader.load_knowledge("challenge_serving_drinks")
challenge_knowledge = knowledge_loader.load_knowledge("challenge_serving_drinks")


class DriveIn(smach.StateMachine):
""" Serves on drink to an operator
"""
Initialize at the designated place, drive to and inspect the bar, store the detected drinks or ask for the
unavailable drink, and navigate to the room where are the operators
"""
def __init__(self, robot, bar_designator, room_designator, objects_list_des, unav_drink_des):
""" Initialization method
"""
Initialization method
:param robot: robot api object
:param bar_designator: (EntityDesignator) in which the bar location is stored
: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
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"])

Expand All @@ -29,60 +34,30 @@ def __init__(self, robot, bar_designator, room_designator, objects_list_des, una

with self:
# Initialize
smach.StateMachine.add(
'INITIALIZE',
states.Initialize(robot),
transitions={"initialized": "WAIT_1",
"abort": "aborted"}
)

# Wait for one second so that initialise has time to finish
smach.StateMachine.add(
"WAIT_1",
states.WaitTime(
robot=robot,
waittime=1),
transitions={"waited": "SET_INITIAL_POSE",
"preempted": "SET_INITIAL_POSE"}
)

smach.StateMachine.add(
'SET_INITIAL_POSE',
states.SetInitialPose(robot, CHALLENGE_KNOWLEDGE.starting_point),
transitions={"done": "INSPECT_BAR",
"preempted": "aborted",
"error": "INSPECT_BAR"}
)
smach.StateMachine.add("INITIALIZE",
states.StartChallengeRobust(robot=robot,
initial_pose=challenge_knowledge.starting_point,
use_entry_points=False, door=False),
transitions={"Done": "INSPECT_BAR",
"Aborted": "aborted",
"Failed": "failed"})

# 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: transition to NAVIGATE_TO_ROOM
"failed": "INSPECT_FALLBACK"}
)
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: transition to 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),
transitions={"succeeded": "NAVIGATE_TO_ROOM",
"failed": "NAVIGATE_TO_ROOM"},
)
smach.StateMachine.add("INSPECT_FALLBACK",
AskAvailability(robot=robot, unavailable_drink_designator=unav_drink_des.writeable),
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": "succeeded",
"unreachable": "failed",
"goal_not_defined": "aborted"}
)
smach.StateMachine.add("NAVIGATE_TO_ROOM",
states.NavigateToRoom(robot=robot, entity_designator_room=room_designator),
transitions={"arrived": "succeeded",
"unreachable": "failed",
"goal_not_defined": "aborted"})
161 changes: 62 additions & 99 deletions challenge_serving_drinks/src/challenge_serving_drinks/get_order.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,137 +12,100 @@
from .sd_states import AskDrink, DetectWaving

# Knowledge
COMMON_KNOWLEDGE = knowledge_loader.load_knowledge("common")
common_knowledge = knowledge_loader.load_knowledge("common")


class GetOrder(smach.StateMachine):
""" Gets an order. If succeeded, the person_designator and drink_designator are filled and can be used in subsequent
"""
Gets an order. If succeeded, the person_designator and drink_designator are filled and can be used in subsequent
states.
"""
def __init__(self, robot, operator_name, drink_designator,
available_drinks_designator, unavailable_drink_designator):
# type: (Robot, str, VariableDesignator) -> None
""" Initialization method
"""
Initialization method
:param robot: robot api object
:param operator_name: name with which the operator will be stored in image recognition module
:param drink_designator: (VariableDesignator) in which the drink to fetch 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
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"])

with self:

# Operator id
caller_id = "operator"
caller_designator = ds.EdEntityDesignator(robot=robot, id=caller_id, name='caller_des')
caller_designator = ds.EdEntityDesignator(robot=robot, id=caller_id, name="caller_des")

# Detect - people holding drinks and people without drinks #ToDo: implement!

# Detect fallback - detect waving people
smach.StateMachine.add(
"ASK_FOR_WAVING",
states.Say(
robot=robot,
sentence="Looks like everyone has a drink",
look_at_standing_person=True),
transitions={"spoken": "ASK_STEP_IN_FRONT"} # ToDo: transition to WAIT_FOR_WAVING
)

smach.StateMachine.add(
"WAIT_FOR_WAVING",
DetectWaving(
robot=robot,
caller_id=caller_id),
transitions={'succeeded': 'SAY_I_HAVE_SEEN',
'aborted': 'ASK_STEP_IN_FRONT'}
)

smach.StateMachine.add(
'SAY_I_HAVE_SEEN',
states.Say(
robot=robot,
sentence='I have seen a waving person, I will be there shortly!',
look_at_standing_person=True),
transitions={"spoken": 'NAVIGATE_TO_WAVING'}
)
smach.StateMachine.add("ASK_FOR_WAVING",
states.Say(robot=robot, sentence="Looks like everyone has a drink",
look_at_standing_person=True),
transitions={"spoken": "ASK_STEP_IN_FRONT"}) # ToDo: transition to WAIT_FOR_WAVING

smach.StateMachine.add("WAIT_FOR_WAVING",
DetectWaving(robot=robot, caller_id=caller_id),
transitions={"succeeded": "SAY_I_HAVE_SEEN",
"aborted": "ASK_STEP_IN_FRONT"})

smach.StateMachine.add("SAY_I_HAVE_SEEN",
states.Say(robot=robot,
sentence="I have seen a waving person, I will be there shortly!",
look_at_standing_person=True),
transitions={"spoken": "NAVIGATE_TO_WAVING"})

# Navigate to waving people
smach.StateMachine.add(
'NAVIGATE_TO_WAVING',
states.NavigateToObserve(
robot=robot,
entity_designator=caller_designator,
radius=1.1),
transitions={'arrived': 'LEARN_NAME',
'unreachable': 'ASK_STEP_IN_FRONT',
'goal_not_defined': 'WAIT_FOR_WAVING'}
)
smach.StateMachine.add("NAVIGATE_TO_WAVING",
states.NavigateToObserve(robot=robot, entity_designator=caller_designator,
radius=1.1),
transitions={"arrived": "LEARN_NAME",
"unreachable": "ASK_STEP_IN_FRONT",
"goal_not_defined": "WAIT_FOR_WAVING"})

# Detect waving people fallback - ask operator in front
smach.StateMachine.add(
"ASK_STEP_IN_FRONT",
states.Say(
robot=robot,
sentence="Please step in front of me to give your order",
look_at_standing_person=True),
transitions={"spoken": "LEARN_NAME"}
)
smach.StateMachine.add("ASK_STEP_IN_FRONT",
states.Say(robot=robot, sentence="Please step in front of me to give your order",
look_at_standing_person=True),
transitions={"spoken": "LEARN_NAME"})

# Ask operator for his name
smach.StateMachine.add(
"LEARN_NAME",
states.AskPersonName(
robot=robot,
person_name_des=operator_name.writeable,
name_options=COMMON_KNOWLEDGE.names,
default_name='john',
nr_tries=2),
transitions={"succeeded": "LEARN_OPERATOR",
"failed": "LEARN_NAME_FALLBACK",
"timeout": "LEARN_NAME_FALLBACK"}
)
smach.StateMachine.add("LEARN_NAME",
states.AskPersonName(robot=robot, person_name_des=operator_name.writeable,
name_options=common_knowledge.names, default_name="john",
nr_tries=2),
transitions={"succeeded": "LEARN_OPERATOR",
"failed": "LEARN_NAME_FALLBACK",
"timeout": "LEARN_NAME_FALLBACK"})

# Ask operator for his name fallback
smach.StateMachine.add(
"LEARN_NAME_FALLBACK",
states.Say(
robot=robot,
sentence="Sorry, I did not get your name, I'll just call you john",
look_at_standing_person=True),
transitions={"spoken": "LEARN_OPERATOR"}
)
smach.StateMachine.add("LEARN_NAME_FALLBACK",
states.Say(robot=robot,
sentence="Sorry, I did not get your name, I'll just call you john",
look_at_standing_person=True),
transitions={"spoken": "LEARN_OPERATOR"})

# Learn operator
smach.StateMachine.add(
"LEARN_OPERATOR",
states.LearnPerson(
robot=robot,
name_designator=operator_name,
nr_tries=5),
transitions={"succeeded": "ASK_DRINK",
"failed": "LEARN_OPERATOR_FALLBACK"}
)
smach.StateMachine.add("LEARN_OPERATOR",
states.LearnPerson(robot=robot, name_designator=operator_name, nr_tries=5),
transitions={"succeeded": "ASK_DRINK",
"failed": "LEARN_OPERATOR_FALLBACK"})

# Learn operator fallback
smach.StateMachine.add(
"LEARN_OPERATOR_FALLBACK",
states.Say(
robot=robot,
sentence="Something went wrong but I will call you by name when I'm back",
look_at_standing_person=True),
transitions={"spoken": "ASK_DRINK"}
)
smach.StateMachine.add("LEARN_OPERATOR_FALLBACK",
states.Say(robot=robot,
sentence="Something went wrong but I will call you by name when I'm back",
look_at_standing_person=True),
transitions={"spoken": "ASK_DRINK"})

# Ask for preferred beverage
smach.StateMachine.add(
"ASK_DRINK",
AskDrink(
robot=robot,
operator_name=operator_name,
drink_designator=drink_designator.writeable,
available_drinks_designator=available_drinks_designator,
unavailable_drink_designator=unavailable_drink_designator),
transitions={"succeeded": "succeeded",
"failed": "failed"},
)
smach.StateMachine.add("ASK_DRINK",
AskDrink(robot=robot, operator_name=operator_name,
drink_designator=drink_designator.writeable,
available_drinks_designator=available_drinks_designator,
unavailable_drink_designator=unavailable_drink_designator),
transitions={"succeeded": "succeeded",
"failed": "failed"},)

0 comments on commit 0d9edb8

Please sign in to comment.