def amigo_navigate_to_handover_pose(amigo, frame_id=BAR_ENTITY_FRAME_ID, dist_from_bar=DIST_FROM_BAR, side=ARM_SIDE):
if side == "left":
rz = -math.pi / 2.0
elif side == "right":
rz = math.pi / 2.0
navigateToPoseSM = states.NavigateToPose(amigo,
x=dist_from_bar,
y=0.0,
rz=rz,
radius=HANDOVER_POSE_RADIUS,
frame_id=frame_id)
nav_res = navigateToPoseSM.execute()
if nav_res == "arrived":
return ActionResult(ActionResult.SUCCEEDED, "Amigo: Arrived at handover pose")
else:
return ActionResult(ActionResult.FAILED, ("Amigo: handover pose %s",nav_res))
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):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# Create designators
grasp_designator1 = ds.EdEntityDesignator(robot, type="temp")
grasp_designator2 = ds.EdEntityDesignator(robot, type="temp")
grasp_designator3 = ds.EdEntityDesignator(robot, type="temp")
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]
with self:
# Part I: Set a table
smach.StateMachine.add('ENTER_ROOM', # Enter the room
states.Initialize(robot),
transitions={'initialized': 'ANNOUNCEMENT',
'abort': 'Aborted'})
smach.StateMachine.add('ANNOUNCEMENT',
states.Say(robot, "Let's see if my master has a task for me! ", block=True),
transitions={'spoken': 'FETCH_COMMAND_I'})
smach.StateMachine.add('FETCH_COMMAND_I', # Hear "set the table"
HearFetchCommand(robot, 15.0, "set"),
transitions={'done': 'ASK_FOR_MEAL'})
smach.StateMachine.add('ASK_FOR_MEAL',
states.Say(robot, "What should I serve, master?", block=True),
transitions={'spoken': 'GET_ORDER'})
smach.StateMachine.add('GET_ORDER',
GetBreakfastOrder(robot, knowledge.options,
grasp_designator1,
grasp_designator2,
grasp_designator3,
timeout=15.0),
transitions={'done': 'SET_THE_TABLE'})
smach.StateMachine.add('SET_THE_TABLE', # Take order and Set the table (bring the objects to the table)
ManipulateMachine(robot=robot,
grasp_designator1=grasp_designator1,
grasp_designator2=grasp_designator2,
grasp_designator3=grasp_designator3,
grasp_furniture_id1=knowledge.cupboard,
grasp_furniture_id3=knowledge.cupboard,
place_furniture_id=knowledge.table),
transitions={'succeeded': 'ANNOUNCE_TASK_COMPLETION',
'failed': 'RETURN_TO_START_2'})
smach.StateMachine.add('ANNOUNCE_TASK_COMPLETION',
states.Say(robot, "The table is set! Moving to the meeting point for the next task.",
block=False),
transitions={'spoken': 'RETURN_TO_START_2'})
# Part II: Clean the table
smach.StateMachine.add('RETURN_TO_START_2',
states.NavigateToPose(robot=robot, x=start_x, y=start_y, rz=start_rz, radius=0.3),
transitions={'arrived': 'FETCH_COMMAND_II',
'unreachable': 'FETCH_COMMAND_II',
'goal_not_defined': 'FETCH_COMMAND_II'})
smach.StateMachine.add('FETCH_COMMAND_II', # Hear "clear up the table"
HearFetchCommand(robot, 15.0, "clear"),
transitions={'done': 'CLEAR_UP'})
smach.StateMachine.add('CLEAR_UP', # Clear the table
ClearManipulateMachine(robot=robot, grasp_furniture_id=knowledge.table,
place_furniture_id1=knowledge.cupboard,
place_furniture_id3=knowledge.cupboard),
transitions={'succeeded': 'END_CHALLENGE',
'failed': 'END_CHALLENGE'})
# End
smach.StateMachine.add('END_CHALLENGE',
states.Say(robot, "I am done here"),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "set_a_table")
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# Create designators
grasp_designator1 = ds.EdEntityDesignator(robot, type="temp")
grasp_designator2 = ds.EdEntityDesignator(robot, type="temp")
grasp_designator3 = ds.EdEntityDesignator(robot, type="temp")
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]
with self:
# Start challenge via StartChallengeRobust
smach.StateMachine.add("START_CHALLENGE_ROBUST",
states.StartChallengeRobust(robot, STARTING_POINT, use_entry_points=True),
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': 'ANNOUNCEMENT',
'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_1),
radius=0.7),
transitions={'arrived': 'ANNOUNCEMENT',
'unreachable': 'ANNOUNCEMENT',
'goal_not_defined': 'ANNOUNCEMENT'})
# Part I: Set a table
smach.StateMachine.add('ANNOUNCEMENT',
states.Say(robot, "Let's see if my master has a task for me! ", block=True),
transitions={'spoken': 'FETCH_COMMAND_I'})
smach.StateMachine.add('FETCH_COMMAND_I', # Hear "set the table"
HearFetchCommand(robot, 15.0, "set"),
transitions={'done': 'ASK_FOR_MEAL'})
smach.StateMachine.add('ASK_FOR_MEAL',
states.Say(robot, "What should I serve, master?", block=True),
transitions={'spoken': 'GET_ORDER'})
smach.StateMachine.add('GET_ORDER',
GetBreakfastOrder(robot, knowledge.options,
grasp_designator1,
grasp_designator2,
grasp_designator3,
timeout=15.0),
transitions={'done': 'SET_THE_TABLE'})
smach.StateMachine.add('SET_THE_TABLE', # Take order and Set the table (bring the objects to the table)
ManipulateMachine(robot=robot,
grasp_designator1=grasp_designator1,
grasp_designator2=grasp_designator2,
grasp_designator3=grasp_designator3,
grasp_furniture_id1=knowledge.grasp_furniture_id1,
grasp_furniture_id2=knowledge.grasp_furniture_id2,
place_furniture_id=knowledge.place_furniture_id),
transitions={'succeeded': 'ANNOUNCE_TASK_COMPLETION',
'failed': 'RETURN_TO_START_2'})
smach.StateMachine.add('ANNOUNCE_TASK_COMPLETION',
states.Say(robot, "The table is set! Moving to the meeting point for the next task.",
block=False),
transitions={'spoken': 'RETURN_TO_START_2'})
# Part II: Clean the table
smach.StateMachine.add('RETURN_TO_START_2',
states.NavigateToPose(robot=robot, x=start_x, y=start_y, rz=start_rz, radius=0.3),
transitions={'arrived': 'FETCH_COMMAND_II',
'unreachable': 'FETCH_COMMAND_II',
'goal_not_defined': 'FETCH_COMMAND_II'})
smach.StateMachine.add('FETCH_COMMAND_II', # Hear "clear up the table"
HearFetchCommand(robot, 15.0, "clear"),
transitions={'done': 'CLEAR_UP'})
smach.StateMachine.add('CLEAR_UP', # Clear the table
ClearManipulateMachine(robot=robot, grasp_furniture_id=knowledge.place_furniture_id,
place_furniture_id1=knowledge.grasp_furniture_id1,
place_furniture_id2=knowledge.grasp_furniture_id2),
transitions={'succeeded': 'END_CHALLENGE',
'failed': 'END_CHALLENGE'})
# End
smach.StateMachine.add('END_CHALLENGE',
states.Say(robot, "I am done here"),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "set_a_table")
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 execute(self, userdata=None):
robot_frame = self._robot.base.get_location().frame
robot_pose = robot_frame.p
r, p, y = robot_frame.M.GetRPY()
states.NavigateToPose(self._robot, robot_pose.position.x,
robot_pose.position.y, y + math.pi)