def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
self.robot = robot
self.position_constraint = PositionConstraint()
self.orientation_constraint = OrientationConstraint()
self.position_constraint.constraint = "x^2+y^2 < 1"
self.requested_location = None
rospy.Subscriber("/location_request", std_msgs.msg.String,
self.requestedLocationcallback)
self.random_nav_designator = RandomNavDesignator(self.robot)
with self:
smach.StateMachine.add("WAIT_A_SEC",
states.WaitTime(robot, waittime=1.0),
transitions={
'waited': "SELECT_ACTION",
'preempted': "Aborted"
})
smach.StateMachine.add("SELECT_ACTION",
SelectAction(),
transitions={
'continue': "DETERMINE_TARGET",
'pause': "SELECT_ACTION",
'stop': "SAY_DONE"
})
@smach.cb_interface(
outcomes=['target_determined', 'no_targets_available'],
input_keys=[],
output_keys=[])
def determine_target(userdata, designator):
entity_id = designator.getRandomGoal()
sentences = [
"Lets go look at the %s", "Lets have a look at the %s",
"Lets go to the %s", "Lets move to the %s",
"I will go to the %s", "I will now move to the %s",
"I will now drive to the %s", "I will look the %s",
"The %s will be my next location", "The %s it is",
"New goal, the %s", "Going to look at the %s",
"Moving to the %s", "Driving to the %s", "On to the %s",
"On the move to the %s", "Going to the %s"
]
robot.speech.speak(random.choice(sentences) % entity_id,
block=False)
return 'target_determined'
smach.StateMachine.add('DETERMINE_TARGET',
smach.CBState(determine_target,
cb_kwargs={
'designator':
self.random_nav_designator
}),
transitions={
'target_determined': 'DRIVE',
'no_targets_available': 'SELECT_ACTION'
})
smach.StateMachine.add('DRIVE',
states.NavigateToObserve(
robot, self.random_nav_designator),
transitions={
"arrived": "SAY_SUCCEEDED",
"unreachable": 'SAY_UNREACHABLE',
"goal_not_defined": 'SELECT_ACTION'
})
smach.StateMachine.add("SAY_SUCCEEDED",
states.Say(robot, [
"I am here",
"Goal succeeded",
"Another goal succeeded",
"Goal reached",
"Another goal reached",
"Target reached",
"Another target reached",
"Destination reached",
"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'})
from robot_smach_states.util.designators import Designator, VariableDesignator
# def setup_statemachine(robot):
# sm = smach.StateMachine(outcomes=['Done','Aborted'])
# with sm:
# smach.StateMachine.add('DRIVE',
# states.NavigateToPose(robot, x = 1, y = 3, rz = -1.57),
# transitions={ 'arrived':'Done',
# 'unreachable':'Aborted',
# 'goal_not_defined':'Aborted'})
############################## initializing program ##############################
if __name__ == '__main__':
rospy.init_node('test_exec')
if len(sys.argv) > 1:
robot_name = sys.argv[1]
if robot_name == 'sergio':
robot = Sergio()
elif robot_name == 'amigo':
robot = Amigo()
else:
robot = Amigo()
#testexec = states.NavigateToPose(robot, 3, 1, -1.57, 0.15)
testexec = states.NavigateToObserve(robot, Designator("dinner_table"))
testexec.execute()
#startup(setup_statemachine, robot_name=robot_name)
def __init__(self, robot):
""" Constructor
:param robot: robot object
"""
smach.StateMachine.__init__(self, outcomes=['STOP'])
start_pose = robot.base.get_location()
start_x = start_pose.frame.p.x()
start_y = start_pose.frame.p.y()
start_rz = start_pose.frame.M.GetRPY()[2]
kitchen_id = "kitchen"
kitchen_designator = states.util.designators.ed_designators.EdEntityDesignator(
robot=robot, id=kitchen_id)
customer_id = 'current_customer'
customer_designator = states.util.designators.VariableDesignator(
resolve_type=Entity, name=customer_id)
orders = []
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'SAY_WAVING',
'abort': 'STOP'
})
smach.StateMachine.add(
'SAY_WAVING',
states.Say(robot,
"Mr. Barman, please make sure that the people wave "
"slowly and put their arm up high. Like is shown "
"on my screen",
block=True),
transitions={'spoken': 'SHOW_IMAGE'})
smach.StateMachine.add(
'SHOW_IMAGE',
states.ShowImageState(
robot, "~/ros/kinetic/system/src/challenge_restaurant/"
"images/waving.jpg",
seconds=10),
transitions={
'succeeded': 'STORE_KITCHEN',
'failed': 'STORE_KITCHEN'
})
smach.StateMachine.add('STORE_KITCHEN',
StoreWaypoint(robot=robot,
location_id=kitchen_id),
transitions={'done': 'WAIT_FOR_CUSTOMER'})
# smach.StateMachine.add('WAIT_FOR_CUSTOMER',
# WaitForCustomer(robot, caller_id, kitchen_designator),
# transitions={'succeeded': 'SAY_I_HAVE_SEEN',
# 'aborted': 'STOP'})
# Implement new find state to detect nearest waving person
smach.StateMachine.add(
'WAIT_FOR_CUSTOMER',
states.FindFirstPerson(robot,
customer_designator.writeable,
properties={'tags': ['LWave', 'RWave']},
strict=False,
nearest=True,
speak=True,
look_range=(-np.pi / 4, np.pi / 4),
look_steps=4,
search_timeout=600), # 10 minutes
transitions={
'found': 'SAY_I_HAVE_SEEN',
'failed': 'WAIT_FOR_CUSTOMER'
})
# No Asking
# smach.StateMachine.add('SAY_I_HAVE_SEEN',
# states.Say(robot, 'I have seen a waving person, I will take the order, I will be there shortly! Coming your way my amigo!'),
# transitions={"spoken": 'NAVIGATE_TO_CUSTOMER'})
# End No Asking
# Asking for confirmation
smach.StateMachine.add(
'SAY_I_HAVE_SEEN',
states.Say(
robot,
'I have seen a waving person, should I take the order? '
'Please say "{0} take the order" or "{0} wait"'.format(
robot.robot_name)),
transitions={"spoken": 'WAIT_FOR_START'})
smach.StateMachine.add('WAIT_FOR_START',
AskTakeTheOrder(robot),
transitions={
'yes': 'SAY_NAVIGATE_TO_CUSTOMER',
'wait': 'WAIT_FOR_CUSTOMER',
'timeout': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add(
'SAY_NAVIGATE_TO_CUSTOMER',
states.Say(
robot,
"I am at your service, I will be there shortly! Coming your way my amigo!",
block=True),
transitions={'spoken': 'NAVIGATE_TO_CUSTOMER'})
# End Asking for confirmation
smach.StateMachine.add(
'NAVIGATE_TO_CUSTOMER',
states.NavigateToObserve(robot=robot,
entity_designator=customer_designator,
radius=0.8),
transitions={
'arrived': 'TAKE_ORDER',
'unreachable': 'SAY_NAVIGATE_TO_CUSTOMER_FALLBACK',
'goal_not_defined': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add('SAY_NAVIGATE_TO_CUSTOMER_FALLBACK',
states.Say(robot,
"Help, lets try it another way"),
transitions={'spoken': 'TURN_AROUND'})
smach.StateMachine.add(
'TURN_AROUND',
states.Turn(robot, radians=2 * math.pi),
transitions={'turned': 'NAVIGATE_TO_CUSTOMER_FALLBACK'})
smach.StateMachine.add('NAVIGATE_TO_CUSTOMER_FALLBACK',
states.NavigateToObserve(
robot=robot,
entity_designator=customer_designator,
radius=1.1),
transitions={
'arrived': 'TAKE_ORDER',
'unreachable': 'RETURN_TO_START',
'goal_not_defined': 'RETURN_TO_START'
})
smach.StateMachine.add('TAKE_ORDER',
TakeOrder(
robot=robot,
entity_designator=customer_designator,
orders=orders),
transitions={
'succeeded': 'NAVIGATE_TO_KITCHEN',
'failed': 'RETURN_TO_START'
})
smach.StateMachine.add('NAVIGATE_TO_KITCHEN',
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=kitchen_designator,
radius=0.15),
transitions={
'arrived':
'RECITE_ORDER',
'unreachable':
'SAY_NAVIGATE_TO_KITCHEN_FALLBACK',
'goal_not_defined':
'SAY_NAVIGATE_TO_KITCHEN_FALLBACK'
})
smach.StateMachine.add(
'SAY_NAVIGATE_TO_KITCHEN_FALLBACK',
states.Say(robot, "Help, how do I get there?", block=False),
transitions={'spoken': 'TURN_AROUND_KITCHEN_FALLBACK'})
smach.StateMachine.add(
'TURN_AROUND_KITCHEN_FALLBACK',
states.Turn(robot, radians=math.pi),
transitions={'turned': 'NAVIGATE_TO_KITCHEN_FALLBACK'})
smach.StateMachine.add('NAVIGATE_TO_KITCHEN_FALLBACK',
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=kitchen_designator,
radius=0.20),
transitions={
'arrived':
'RECITE_ORDER',
'unreachable':
'SAY_NAVIGATE_TO_KITCHEN_FALLBACK',
'goal_not_defined':
'SAY_NAVIGATE_TO_KITCHEN_FALLBACK'
})
smach.StateMachine.add('RECITE_ORDER',
ReciteOrders(robot=robot, orders=orders),
transitions={'spoken': 'CLEAR_ORDER'})
smach.StateMachine.add(
'CLEAR_ORDER',
ClearOrders(orders=orders),
transitions={'succeeded': 'SAY_CANNOT_GRASP'})
smach.StateMachine.add('SAY_CANNOT_GRASP',
states.Say(
robot,
"I am unable to grasp my own order, "
"could you please put it in my basket"),
transitions={'spoken': 'WAIT_FOR_OBJECTS'})
smach.StateMachine.add('WAIT_FOR_OBJECTS',
states.WaitTime(robot=robot, waittime=10.0),
transitions={
'waited': 'BRING_OBJECTS',
'preempted': 'STOP'
})
smach.StateMachine.add(
'BRING_OBJECTS',
states.NavigateToObserve(robot=robot,
entity_designator=customer_designator,
radius=1.1),
transitions={
'arrived': 'SAY_OBJECTS',
'unreachable': 'SAY_BRING_OBJECTS_FALLBACK',
'goal_not_defined': 'RETURN_TO_START'
})
smach.StateMachine.add(
'SAY_BRING_OBJECTS_FALLBACK',
states.Say(robot, "Help, how do I get there?"),
transitions={'spoken': 'TURN_AROUND_BRING_OBJECTS_FALLBACK'})
smach.StateMachine.add(
'TURN_AROUND_BRING_OBJECTS_FALLBACK',
states.Turn(robot, radians=2 * math.pi),
transitions={'turned': 'BRING_OBJECTS_FALLBACK'})
smach.StateMachine.add('BRING_OBJECTS_FALLBACK',
states.NavigateToObserve(
robot=robot,
entity_designator=customer_designator,
radius=1.1),
transitions={
'arrived': 'SAY_OBJECTS',
'unreachable': 'SAY_OBJECTS',
'goal_not_defined': 'RETURN_TO_START'
})
smach.StateMachine.add(
'SAY_OBJECTS',
states.Say(
robot, "Hi there handsome, here are your objects, "
"please take them from my basket"),
transitions={'spoken': 'WAIT_TO_TAKE_OBJECTS'})
smach.StateMachine.add('WAIT_TO_TAKE_OBJECTS',
states.WaitTime(robot=robot, waittime=10.0),
transitions={
'waited': 'RETURN_TO_START',
'preempted': 'STOP'
})
smach.StateMachine.add('RETURN_TO_START',
states.NavigateToPose(robot=robot,
x=start_x,
y=start_y,
rz=start_rz,
radius=0.3),
transitions={
'arrived': 'WAIT_FOR_CUSTOMER',
'unreachable':
'SAY_RETURN_TO_START_FALLBACK',
'goal_not_defined': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add(
'SAY_RETURN_TO_START_FALLBACK',
states.Say(robot, "Help, how do I get back?"),
transitions={'spoken': 'RETURN_TO_START_TURN_AROUND'})
smach.StateMachine.add(
'RETURN_TO_START_TURN_AROUND',
states.Turn(robot, radians=math.pi),
transitions={'turned': 'RETURN_TO_START_FALLBACK'})
smach.StateMachine.add('RETURN_TO_START_FALLBACK',
states.NavigateToObserve(
robot=robot,
entity_designator=customer_designator,
radius=0.7),
transitions={
'arrived': 'WAIT_FOR_CUSTOMER',
'unreachable': 'WAIT_FOR_CUSTOMER',
'goal_not_defined': 'WAIT_FOR_CUSTOMER'
})
def __init__(self, robot):
""" Constructor
:param robot: robot object
"""
smach.StateMachine.__init__(self, outcomes=['STOP'])
start_pose = robot.base.get_location()
start_x = start_pose.frame.p.x()
start_y = start_pose.frame.p.y()
start_rz = start_pose.frame.M.GetRPY()[2]
kitchen_id = "kitchen"
kitchen_designator = states.util.designators.ed_designators.EdEntityDesignator(
robot=robot, id=kitchen_id)
caller_id = "customer"
caller_designator = states.util.designators.ed_designators.EdEntityDesignator(
robot=robot, id=caller_id)
orders = {}
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'STORE_KITCHEN',
'abort': 'STOP'
})
smach.StateMachine.add('STORE_KITCHEN',
StoreWaypoint(robot=robot,
location_id=kitchen_id),
transitions={'done': 'WAIT_FOR_CUSTOMER'})
smach.StateMachine.add('WAIT_FOR_CUSTOMER',
WaitForCustomer(robot, caller_id,
kitchen_designator),
transitions={
'succeeded': 'NAVIGATE_TO_CUSTOMER',
'aborted': 'STOP',
'rejected': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add(
'NAVIGATE_TO_CUSTOMER',
states.NavigateToObserve(robot=robot,
entity_designator=caller_designator,
radius=0.85),
transitions={
'arrived': 'TAKE_ORDER',
'unreachable': 'SAY_NAVIGATE_TO_CUSTOMER_FALLBACK',
'goal_not_defined': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add('SAY_NAVIGATE_TO_CUSTOMER_FALLBACK',
states.Say(robot,
"Help, how do I get there?"),
transitions={'spoken': 'TURN_AROUND'})
smach.StateMachine.add(
'TURN_AROUND',
states.Turn(robot, radians=2 * math.pi),
transitions={'turned': 'NAVIGATE_TO_CUSTOMER_FALLBACK'})
smach.StateMachine.add('NAVIGATE_TO_CUSTOMER_FALLBACK',
states.NavigateToObserve(
robot=robot,
entity_designator=caller_designator,
radius=0.85),
transitions={
'arrived': 'TAKE_ORDER',
'unreachable': 'RETURN_TO_START',
'goal_not_defined': 'RETURN_TO_START'
})
smach.StateMachine.add('TAKE_ORDER',
TakeOrder(robot=robot,
location=caller_id,
orders=orders),
transitions={
'succeeded': 'NAVIGATE_TO_KITCHEN',
'failed': 'RETURN_TO_START'
})
smach.StateMachine.add('NAVIGATE_TO_KITCHEN',
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=kitchen_designator,
radius=0.15),
transitions={
'arrived':
'RECITE_ORDER',
'unreachable':
'SAY_NAVIGATE_TO_KITCHEN_FALLBACK',
'goal_not_defined':
'SAY_NAVIGATE_TO_KITCHEN_FALLBACK'
})
smach.StateMachine.add(
'SAY_NAVIGATE_TO_KITCHEN_FALLBACK',
states.Say(robot, "Help, how do I get there?"),
transitions={'spoken': 'TURN_AROUND_KITCHEN_FALLBACK'})
smach.StateMachine.add(
'TURN_AROUND_KITCHEN_FALLBACK',
states.Turn(robot, radians=math.pi),
transitions={'turned': 'NAVIGATE_TO_KITCHEN_FALLBACK'})
smach.StateMachine.add('NAVIGATE_TO_KITCHEN_FALLBACK',
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=kitchen_designator,
radius=0.20),
transitions={
'arrived': 'RECITE_ORDER',
'unreachable': 'RECITE_ORDER',
'goal_not_defined': 'RECITE_ORDER'
})
smach.StateMachine.add('RECITE_ORDER',
ReciteOrders(robot=robot, orders=orders),
transitions={'spoken': 'SAY_CANNOT_GRASP'})
smach.StateMachine.add('SAY_CANNOT_GRASP',
states.Say(
robot,
"I am unable to grasp my own order,"
"could you please put it in my basket"),
transitions={'spoken': 'WAIT_FOR_OBJECTS'})
smach.StateMachine.add('WAIT_FOR_OBJECTS',
states.WaitTime(robot=robot, waittime=5.0),
transitions={
'waited': 'BRING_OBJECTS',
'preempted': 'STOP'
})
smach.StateMachine.add(
'BRING_OBJECTS',
states.NavigateToObserve(robot=robot,
entity_designator=caller_designator,
radius=0.85),
transitions={
'arrived': 'SAY_OBJECTS',
'unreachable': 'SAY_BRING_OBJECTS_FALLBACK',
'goal_not_defined': 'RETURN_TO_START'
})
smach.StateMachine.add(
'SAY_BRING_OBJECTS_FALLBACK',
states.Say(robot, "Help, how do I get there?"),
transitions={'spoken': 'TURN_AROUND_BRING_OBJECTS_FALLBACK'})
smach.StateMachine.add(
'TURN_AROUND_BRING_OBJECTS_FALLBACK',
states.Turn(robot, radians=2 * math.pi),
transitions={'turned': 'BRING_OBJECTS_FALLBACK'})
smach.StateMachine.add('BRING_OBJECTS_FALLBACK',
states.NavigateToObserve(
robot=robot,
entity_designator=caller_designator,
radius=0.85),
transitions={
'arrived': 'SAY_OBJECTS',
'unreachable': 'SAY_OBJECTS',
'goal_not_defined': 'RETURN_TO_START'
})
smach.StateMachine.add(
'SAY_OBJECTS',
states.Say(
robot, "Dear mister, here are your objects, "
"please take them from my basket"),
transitions={'spoken': 'WAIT_TO_TAKE_OBJECTS'})
smach.StateMachine.add('WAIT_TO_TAKE_OBJECTS',
states.WaitTime(robot=robot, waittime=5.0),
transitions={
'waited': 'RETURN_TO_START',
'preempted': 'STOP'
})
smach.StateMachine.add('RETURN_TO_START',
states.NavigateToPose(robot=robot,
x=start_x,
y=start_y,
rz=start_rz,
radius=0.3),
transitions={
'arrived': 'WAIT_FOR_CUSTOMER',
'unreachable':
'SAY_RETURN_TO_START_FALLBACK',
'goal_not_defined': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add(
'SAY_RETURN_TO_START_FALLBACK',
states.Say(robot, "Help, how do I get back?"),
transitions={'spoken': 'RETURN_TO_START_TURN_AROUND'})
smach.StateMachine.add(
'RETURN_TO_START_TURN_AROUND',
states.Turn(robot, radians=math.pi),
transitions={'turned': 'RETURN_TO_START_FALLBACK'})
smach.StateMachine.add('RETURN_TO_START_FALLBACK',
states.NavigateToObserve(
robot=robot,
entity_designator=caller_designator,
radius=0.7),
transitions={
'arrived': 'WAIT_FOR_CUSTOMER',
'unreachable': 'WAIT_FOR_CUSTOMER',
'goal_not_defined': 'WAIT_FOR_CUSTOMER'
})
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"})
def __init__(self, robot, operator_name, drink_designator,
available_drinks_designator, unavailable_drink_designator,
name_options, objects, learn_check_designator, target_room_designator):
# type: (Robot, str, VariableDesignator) -> None
"""
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 available_drinks_designator: (VariableDesignator) in which the available drinks are stored
:param unavailable_drink_designator: (VariableDesignator) in which the unavailable drink is stored
:param name_options: Names from common knowledge
:param objects: Objects from common knowledge
:param learn_check_designator: (VariableDesignator) Bool flag indicating whether the operator was learnt
successfully
:param target_room_designator: (EdEntityDesignator) Entity specifying the target room where the operator needs
to be searched for getting an order
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"])
hacky_arm_des = ds.VariableDesignator(initial_value=robot.get_arm(), name='hacky_arm_3')
with self:
# Operator id
caller_id = "operator"
caller_designator = ds.EdEntityDesignator(robot=robot, id=caller_id, name="caller_des", none_resolve=True)
smach.StateMachine.add("RESET_ROBOT_GET_ORDER",
states.ArmToJointConfig(robot=robot,
arm_designator=hacky_arm_des,
configuration="reset"),
transitions={'succeeded': "SAY_PEOPLE_WITHOUT_DRINKS",
'failed': "SAY_PEOPLE_WITHOUT_DRINKS"})
# Detect - people holding drinks and people without drinks
smach.StateMachine.add("SAY_PEOPLE_WITHOUT_DRINKS",
states.Say(robot=robot, sentence="Trying to find people without a drink",
look_at_standing_person=True,
block=True),
transitions={"spoken": "FIND_PERSON_WITHOUT_DRINK"})
# TODO: Change DummyState to actual state
smach.StateMachine.add("FIND_PERSON_WITHOUT_DRINK",
states.SetPoseFirstFoundPersonToEntity(robot=robot,
properties={'tags': ['LNotHolding', 'RNotHolding']},
strict=True,
dst_entity_designator=caller_id,
query_entity_designator=target_room_designator),
transitions={"done": "SAY_I_HAVE_SEEN",
"failed": "SAY_PEOPLE_WITHOUT_DRINKS_FAILED"})
# Detect fallback - detect waving people
smach.StateMachine.add("SAY_PEOPLE_WITHOUT_DRINKS_FAILED",
states.Say(robot=robot,
sentence="Could not detect people without drinks",
look_at_standing_person=True,
block=True),
transitions={"spoken": "ASK_FOR_WAVING"})
smach.StateMachine.add("ASK_FOR_WAVING",
states.Say(robot=robot,
sentence="Please raise your arm completely and wave, if you want me to bring you something",
look_at_standing_person=True,
block=True),
transitions={"spoken": "WAIT_FOR_WAVING"}) # Change to WAIT_FOR_WAVING
smach.StateMachine.add("WAIT_FOR_WAVING",
states.SetPoseFirstFoundPersonToEntity(robot=robot,
properties={'tags': ['LWave', 'RWave']},
strict=False,
dst_entity_designator=caller_id,
query_entity_designator=target_room_designator),
transitions={"done": "SAY_I_HAVE_SEEN",
"failed": "SAY_COULD_NOT_FIND_WAVING"})
# Navigate to person who wants to place an order
smach.StateMachine.add("SAY_COULD_NOT_FIND_WAVING",
states.Say(robot=robot,
sentence="I did not find any waving person.",
look_at_standing_person=True,
block=True),
transitions={"spoken": "ASK_STEP_IN_FRONT"})
smach.StateMachine.add("SAY_I_HAVE_SEEN",
states.Say(robot=robot,
sentence="Found person who might want to place an order. I will be there shortly!",
look_at_standing_person=True,
block=True),
transitions={"spoken": "NAVIGATE_TO_PERSON"})
# Navigate to waving people
smach.StateMachine.add("NAVIGATE_TO_PERSON",
states.NavigateToObserve(robot=robot, entity_designator=caller_designator,
radius=1.1),
transitions={"arrived": "LEARN_NAME",
"unreachable": "SAY_COULD_NOT_NAVIGATE",
"goal_not_defined": "SAY_PEOPLE_WITHOUT_DRINKS"})
# Detect waving people fallback - ask operator in front
smach.StateMachine.add("SAY_COULD_NOT_NAVIGATE",
states.Say(robot=robot,
sentence="Sorry! I could not navigate to you.",
look_at_standing_person=True),
transitions={"spoken": "ASK_STEP_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,
block=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=name_options,
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"})
# 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"})
# Learn operator fallback
smach.StateMachine.add("LEARN_OPERATOR_FALLBACK",
states.Say(robot=robot,
sentence="I will call you by your name when I'm back",
look_at_standing_person=True,
block=True),
transitions={"spoken": "LEARN_OPERATOR_FLAG_TOGGLE"})
smach.StateMachine.add("LEARN_OPERATOR_FLAG_TOGGLE",
states.ToggleBool(learn_check_designator),
transitions={"done": "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,
objects=objects),
transitions={"succeeded": "succeeded",
"failed": "failed",
"aborted": "aborted"})
def __init__(self, robot):
""" Constructor
:param robot: robot object
"""
smach.StateMachine.__init__(self,
outcomes=['Done', 'WAIT_FOR_CUSTOMER'])
kitchen_id = "kitchen"
kitchen_designator = states.util.designators.ed_designators.EdEntityDesignator(
robot=robot, id=kitchen_id)
caller_id = "customer"
caller_designator = states.util.designators.ed_designators.EdEntityDesignator(
robot=robot, id=caller_id)
orders = {}
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'STORE_KITCHEN',
'abort': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add('STORE_KITCHEN',
StoreWaypoint(robot=robot,
location_id=kitchen_id),
transitions={'done': 'WAIT_FOR_CUSTOMER'})
smach.StateMachine.add('WAIT_FOR_CUSTOMER',
WaitForClickedCustomer(robot, caller_id),
transitions={
'succeeded': 'NAVIGATE_TO_CUSTOMER',
'failed': 'WAIT_FOR_CUSTOMER',
'aborted': 'WAIT_FOR_CUSTOMER',
'rejected': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add('NAVIGATE_TO_CUSTOMER',
states.NavigateToObserve(
robot=robot,
entity_designator=caller_designator,
radius=0.7),
transitions={
'arrived': 'TAKE_ORDER',
'unreachable': 'TAKE_ORDER',
'goal_not_defined': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add('TAKE_ORDER',
TakeOrder(robot=robot,
location=caller_id,
orders=orders),
transitions={
'succeeded': 'NAVIGATE_TO_KITCHEN',
'failed': 'WAIT_FOR_CUSTOMER',
'misunderstood': 'TAKE_ORDER'
})
smach.StateMachine.add('NAVIGATE_TO_KITCHEN',
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=kitchen_designator,
radius=0.15),
transitions={
'arrived': 'RECITE_ORDER',
'unreachable': 'RECITE_ORDER',
'goal_not_defined': 'RECITE_ORDER'
})
smach.StateMachine.add('RECITE_ORDER',
ReciteOrders(robot=robot, orders=orders),
transitions={'spoken': 'SAY_CANNOT_GRASP'})
smach.StateMachine.add('SAY_CANNOT_GRASP',
states.Say(
robot,
"I am unable to grasp my own order,"
"could you please put it in my basket"),
transitions={'spoken': 'WAIT_FOR_OBJECTS'})
smach.StateMachine.add('WAIT_FOR_OBJECTS',
states.WaitTime(robot=robot, waittime=5.0),
transitions={
'waited': 'BRING_OBJECTS',
'preempted': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add('BRING_OBJECTS',
states.NavigateToObserve(
robot=robot,
entity_designator=caller_designator,
radius=0.7),
transitions={
'arrived': 'SAY_OBJECTS',
'unreachable': 'SAY_OBJECTS',
'goal_not_defined': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add(
'SAY_OBJECTS',
states.Say(
robot, "Dear mister, here are your objects, "
"please take them from my basket"),
transitions={'spoken': 'WAIT_TO_TAKE_OBJECTS'})
smach.StateMachine.add('WAIT_TO_TAKE_OBJECTS',
states.WaitTime(robot=robot, waittime=5.0),
transitions={
'waited': 'RETURN_TO_KITCHEN',
'preempted': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add('RETURN_TO_KITCHEN',
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=kitchen_designator,
radius=0.15),
transitions={
'arrived': 'WAIT_FOR_CUSTOMER',
'unreachable': 'WAIT_FOR_CUSTOMER',
'goal_not_defined': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add('SAY_DONE',
states.Say(robot,
"That's it for today, I'm done"),
transitions={'spoken': 'Done'})