def setup_statemachine(robot):
load_waypoints(robot)
operator_id = VariableDesignator(resolve_type=str)
sm = smach.StateMachine(outcomes=['done', 'aborted'])
with sm:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={'initialized': 'STORE_KITCHEN',
'abort': 'aborted'}
)
smach.StateMachine.add('STORE_KITCHEN',
StoreKitchen(robot),
transitions={'done': 'HEAD_STRAIGHT'}
)
smach.StateMachine.add('HEAD_STRAIGHT',
HeadStraight(robot),
transitions={'done': 'SAY_INTRO'}
)
smach.StateMachine.add('SAY_INTRO',
states.Say(robot, "Hi, Show me your restaurant please."),
transitions={'spoken': 'FOLLOW_INITIAL'}
)
smach.StateMachine.add('FOLLOW_INITIAL',
states.FollowOperator(robot,
operator_timeout=30,
operator_id_des=operator_id
),
transitions={'stopped': 'STORE',
'lost_operator': 'FOLLOW_INITIAL',
'no_operator': 'FOLLOW_INITIAL'}
)
smach.StateMachine.add('FOLLOW',
states.FollowOperator(robot,
operator_timeout=30,
ask_follow=False,
operator_id_des=operator_id
),
transitions={'stopped': 'STORE',
'lost_operator': 'FOLLOW_INITIAL',
'no_operator': 'FOLLOW_INITIAL'}
)
smach.StateMachine.add('STORE',
StoreWaypoint(robot),
transitions={'done': 'CHECK_KNOWLEDGE',
'continue': 'FOLLOW'}
)
smach.StateMachine.add('CHECK_KNOWLEDGE',
CheckKnowledge(robot),
transitions={'yes': 'SAY_FOLLOW_TO_KITCHEN',
'no':'FOLLOW'}
)
smach.StateMachine.add('SAY_FOLLOW_TO_KITCHEN',
states.Say(robot,
"Please bring me back to the kitchen!"
),
transitions={'spoken': 'FOLLOW_TO_KITCHEN'}
)
smach.StateMachine.add('FOLLOW_TO_KITCHEN',
states.FollowOperator(robot,
operator_timeout=30,
ask_follow=True,
learn_face=False,
operator_id_des=operator_id
),
transitions={'stopped': 'CHECK_IN_KITCHEN',
'lost_operator': 'SAY_GOTO_KITCHEN',
'no_operator': 'SAY_GOTO_KITCHEN'}
)
smach.StateMachine.add('SAY_GOTO_KITCHEN',
states.Say(robot,
"You know what? I will go back to the kitchen on my own!",
block=False
),
transitions={'spoken': 'GOTO_KITCHEN'})
smach.StateMachine.add('GOTO_KITCHEN',
states.NavigateToWaypoint(robot,
EntityByIdDesignator(robot, id="kitchen")
),
transitions={'arrived': 'SAY_IN_KITCHEN',
'unreachable': 'SAY_I_DONT_KNOW_HOW',
'goal_not_defined': 'SAY_I_DONT_KNOW_HOW'}
)
smach.StateMachine.add('SAY_I_DONT_KNOW_HOW',
states.Say(robot,
"Oops, I don't know the way back.",
block=True
),
transitions={'spoken': 'GOTO_KITCHEN'})
# smach.StateMachine.add('FOLLOW_TO_KITCHEN',
# states.FollowOperator(robot,
# operator_timeout=30,
# ask_follow=False
# ),
# transitions={'stopped': 'CHECK_IN_KITCHEN',
# 'lost_operator': 'FOLLOW_TO_KITCHEN_INITIAL',
# 'no_operator': 'FOLLOW_TO_KITCHEN_INITIAL'}
# )
smach.StateMachine.add('CHECK_IN_KITCHEN',
CheckInKitchen(robot),
transitions={'not_in_kitchen': 'FOLLOW_TO_KITCHEN',
'in_kitchen':'SAY_IN_KITCHEN'}
)
smach.StateMachine.add('SAY_IN_KITCHEN',
states.Say(robot,
"We are in the kitchen again!"
),
transitions={'spoken': 'SAY_WHICH_ORDER'}
)
# Where to take the order from?
smach.StateMachine.add('SAY_WHICH_ORDER', states.Say(robot, "From which table should I take the first order?"), transitions={ 'spoken' :'HEAR_WHICH_ORDER'})
smach.StateMachine.add('HEAR_WHICH_ORDER', HearWhichTable(robot),
transitions={ 'no_result' :'SAY_WHICH_ORDER', 'one' : 'FIRST_SAY_TAKE_ORDER_FROM_TABLE_1', 'two': 'FIRST_SAY_TAKE_ORDER_FROM_TABLE_2', 'three' : "FIRST_SAY_TAKE_ORDER_FROM_TABLE_3"})
# ############## first table ##############
for i, name in tables.iteritems():
next_i = i+1
if next_i > 3:
next_i = 1
smach.StateMachine.add('FIRST_SAY_TAKE_ORDER_FROM_TABLE_%d'%i, states.Say(robot, "Okay, I will take an order from table %d"%i, block=False),
transitions={ 'spoken' :'FIRST_NAVIGATE_TO_WAYPOINT_TABLE_%d'%i})
smach.StateMachine.add('FIRST_NAVIGATE_TO_WAYPOINT_TABLE_%d'%i, states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=name), radius = WAYPOINT_RADIUS),
transitions={'arrived': 'FIRST_ASK_ORDER_TABLE_%d'%i, 'unreachable':'NAVIGATE_TO_WAYPOINT_TABLE_%d'%next_i, 'goal_not_defined':'NAVIGATE_TO_WAYPOINT_TABLE_%d'%next_i})
smach.StateMachine.add('FIRST_ASK_ORDER_TABLE_%d'%i, AskOrder(robot, name),
transitions={'next_order':'NAVIGATE_TO_WAYPOINT_TABLE_%d'%next_i, 'orders_done' : 'SAY_ORDERS_DONE'})
# ############## Loop over the reset of the tables until we have a beverage and a combo ##############
smach.StateMachine.add('NAVIGATE_TO_WAYPOINT_TABLE_1', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="one"), radius = WAYPOINT_RADIUS),
transitions={'arrived': 'SAY_IF_ORDER_TABLE_1', 'unreachable':'NAVIGATE_TO_WAYPOINT_TABLE_2', 'goal_not_defined':'NAVIGATE_TO_WAYPOINT_TABLE_2'})
smach.StateMachine.add('SAY_IF_ORDER_TABLE_1', states.Say(robot, ["Hello, are you ready to order?", "Would you like to order something?"]),
transitions={ 'spoken' :'HEAD_DOWN_TABLE_1'})
smach.StateMachine.add('HEAD_DOWN_TABLE_1', LookAtPersonSitting(robot),
transitions={ 'done' :'HEAR_IF_ORDER_TABLE_1'})
smach.StateMachine.add('HEAR_IF_ORDER_TABLE_1', states.HearOptions(robot, ['yes','no'], timeout = rospy.Duration(10),look_at_standing_person=False),
transitions={ 'no_result' :'NAVIGATE_TO_WAYPOINT_TABLE_2', 'yes':'ASK_ORDER_TABLE_1','no':'NAVIGATE_TO_WAYPOINT_TABLE_2'})
smach.StateMachine.add('ASK_ORDER_TABLE_1', AskOrder(robot, "one"),
transitions={'next_order':'NAVIGATE_TO_WAYPOINT_TABLE_2', 'orders_done' : 'SAY_ORDERS_DONE'})
smach.StateMachine.add('NAVIGATE_TO_WAYPOINT_TABLE_2', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="two"), radius = WAYPOINT_RADIUS),
transitions={'arrived': 'SAY_IF_ORDER_TABLE_2', 'unreachable':'NAVIGATE_TO_WAYPOINT_TABLE_3', 'goal_not_defined':'NAVIGATE_TO_WAYPOINT_TABLE_3'})
smach.StateMachine.add('SAY_IF_ORDER_TABLE_2', states.Say(robot, ["Hello, are you ready to order?", "Would you like to order something?"]),
transitions={ 'spoken' :'HEAD_DOWN_TABLE_2'})
smach.StateMachine.add('HEAD_DOWN_TABLE_2', LookAtPersonSitting(robot),
transitions={ 'done' :'HEAR_IF_ORDER_TABLE_2'})
smach.StateMachine.add('HEAR_IF_ORDER_TABLE_2', states.HearOptions(robot, ['yes','no'], timeout = rospy.Duration(10),look_at_standing_person=False),
transitions={ 'no_result' :'NAVIGATE_TO_WAYPOINT_TABLE_3', 'yes':'ASK_ORDER_TABLE_2','no':'NAVIGATE_TO_WAYPOINT_TABLE_3'})
smach.StateMachine.add('ASK_ORDER_TABLE_2', AskOrder(robot, "two"),
transitions={'next_order':'NAVIGATE_TO_WAYPOINT_TABLE_3', 'orders_done' : 'SAY_ORDERS_DONE'})
smach.StateMachine.add('NAVIGATE_TO_WAYPOINT_TABLE_3', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="three"), radius = WAYPOINT_RADIUS),
transitions={'arrived': 'SAY_IF_ORDER_TABLE_3', 'unreachable':'NAVIGATE_TO_WAYPOINT_TABLE_1', 'goal_not_defined':'NAVIGATE_TO_WAYPOINT_TABLE_1'})
smach.StateMachine.add('SAY_IF_ORDER_TABLE_3', states.Say(robot, ["Hello, are you ready to order?", "Would you like to order something?"]),
transitions={ 'spoken' :'HEAD_DOWN_TABLE_3'})
smach.StateMachine.add('HEAD_DOWN_TABLE_3', LookAtPersonSitting(robot),
transitions={ 'done' :'HEAR_IF_ORDER_TABLE_3'})
smach.StateMachine.add('HEAR_IF_ORDER_TABLE_3', states.HearOptions(robot, ['yes','no'], timeout = rospy.Duration(10),look_at_standing_person=False),
transitions={ 'no_result' :'NAVIGATE_TO_WAYPOINT_TABLE_1', 'yes':'ASK_ORDER_TABLE_3','no':'NAVIGATE_TO_WAYPOINT_TABLE_1'})
smach.StateMachine.add('ASK_ORDER_TABLE_3', AskOrder(robot, "three"),
transitions={'next_order':'NAVIGATE_TO_WAYPOINT_TABLE_1', 'orders_done' : 'SAY_ORDERS_DONE'})
smach.StateMachine.add('SAY_ORDERS_DONE', states.Say(robot, "I received enough orders for now, going back to the kitchen!", block=False),
transitions={ 'spoken' :'NAVIGATE_BACK_TO_THE_KITCHEN'})
smach.StateMachine.add('NAVIGATE_BACK_TO_THE_KITCHEN', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="kitchen"), radius = WAYPOINT_RADIUS),
transitions={'arrived': 'SPEAK_ORDERS', 'unreachable':'NAVIGATE_BACK_TO_THE_KITCHEN_BACKUP', 'goal_not_defined':'NAVIGATE_BACK_TO_THE_KITCHEN_BACKUP'})
smach.StateMachine.add('NAVIGATE_BACK_TO_THE_KITCHEN_BACKUP', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="kitchen"), radius = WAYPOINT_RADIUS+0.2),
transitions={'arrived': 'SPEAK_ORDERS', 'unreachable':'NAVIGATE_BACK_TO_THE_KITCHEN_BACKUP_2', 'goal_not_defined':'NAVIGATE_BACK_TO_THE_KITCHEN_BACKUP_2'})
smach.StateMachine.add('NAVIGATE_BACK_TO_THE_KITCHEN_BACKUP_2', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="kitchen"), radius = WAYPOINT_RADIUS+0.4),
transitions={'arrived': 'SPEAK_ORDERS', 'unreachable':'SPEAK_ORDERS', 'goal_not_defined':'SPEAK_ORDERS'})
smach.StateMachine.add('SPEAK_ORDERS', SpeakOrders(robot),
transitions={ 'spoken' :'STORE_BEVERAGE_SIDE'})
smach.StateMachine.add('STORE_BEVERAGE_SIDE', StoreBeverageSide(robot),
transitions={ 'done' : 'NAVIGATE_TO_BEVERAGES'})
smach.StateMachine.add('NAVIGATE_TO_BEVERAGES', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="beverages"), radius = WAYPOINT_RADIUS),
transitions={'arrived': 'SPEAK_I_SEE_THE_BEVERAGES', 'unreachable':'NAVIGATE_TO_BEVERAGES_BACKUP', 'goal_not_defined':'NAVIGATE_TO_BEVERAGES_BACKUP'})
smach.StateMachine.add('NAVIGATE_TO_BEVERAGES_BACKUP', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="beverages"), radius = WAYPOINT_RADIUS+0.1),
transitions={'arrived': 'SPEAK_I_SEE_THE_BEVERAGES', 'unreachable':'STORE_BEVERAGE_SIDE', 'goal_not_defined':'STORE_BEVERAGE_SIDE'})
smach.StateMachine.add('SPEAK_I_SEE_THE_BEVERAGES', states.Say(robot, "The beverages are in front of me", block=False),
transitions={ 'spoken' :'DELIVER_BEVERAGE'})
smach.StateMachine.add('DELIVER_BEVERAGE', DeliverOrderWithBasket(robot, "beverage"),
transitions={'succeeded':'NAVIGATE_TO_KITCHEN', 'failed':'NAVIGATE_TO_KITCHEN'})
smach.StateMachine.add('NAVIGATE_TO_KITCHEN', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="kitchen"), radius = WAYPOINT_RADIUS),
transitions={'arrived': 'DELIVER_COMBO', 'unreachable':'NAVIGATE_TO_KITCHEN_BACKUP', 'goal_not_defined':'NAVIGATE_TO_KITCHEN_BACKUP'})
smach.StateMachine.add('NAVIGATE_TO_KITCHEN_BACKUP', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="kitchen"), radius = WAYPOINT_RADIUS+0.1),
transitions={'arrived': 'DELIVER_COMBO', 'unreachable':'DELIVER_COMBO', 'goal_not_defined':'DELIVER_COMBO'})
smach.StateMachine.add('DELIVER_COMBO', DeliverOrderWithBasket(robot, "combo"),
transitions={'succeeded':'NAVIGATE_BACK_TO_THE_KITCHEN_2', 'failed':'NAVIGATE_BACK_TO_THE_KITCHEN_2'})
smach.StateMachine.add('NAVIGATE_BACK_TO_THE_KITCHEN_2', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="kitchen"), radius = WAYPOINT_RADIUS),
transitions={'arrived': 'SAY_DONE_WITH_CHALLENGE', 'unreachable':'SAY_DONE_WITH_CHALLENGE', 'goal_not_defined':'SAY_DONE_WITH_CHALLENGE'})
smach.StateMachine.add('SAY_DONE_WITH_CHALLENGE', states.Say(robot, "I'm done with this challenge and you are the banana king!"), transitions={ 'spoken' :'done'})
analyse_designators(sm, "restaurant")
return sm
def setup_statemachine(robot):
place_name = ds.EntityByIdDesignator(robot, id=challenge_knowledge.default_place, name="place_name")
place_position = ds.LockingDesignator(ds.EmptySpotDesignator(robot,
place_name,
name="placement",
area=challenge_knowledge.default_area),
name="place_position")
empty_arm_designator = ds.UnoccupiedArmDesignator(robot.arms,
robot.rightArm,
name="empty_arm_designator")
# With the empty_arm_designator locked, it will ALWAYS resolve to the same arm (first resolve is cached), unless it is unlocked
# For this challenge, unlocking is not needed
bag_arm_designator = empty_arm_designator.lockable()
bag_arm_designator.lock()
# We don't actually grab something, so there is no need for an actual thing to grab
current_item = ds.VariableDesignator(Entity("dummy",
"dummy",
"/{}/base_link".format(robot.robot_name),
kdl_conversions.kdlFrameFromXYZRPY(0.6, 0, 0.5),
None,
{},
[],
datetime.datetime.now()),
name="current_item")
sm = smach.StateMachine(outcomes=['Done','Aborted'])
with sm:
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'})
# TODO: learn operator state needs to be added before follow
# smach.StateMachine.add('WAIT_TO_FOLLOW',
# WaitForOperatorCommand(robot, possible_commands=['follow', 'follow me']),
# transitions={'success': 'FOLLOW_OPERATOR',
# 'abort': 'Aborted'})
smach.StateMachine.add('ASK_FOLLOW_OR_REMEMBER',
states.Say(robot, ["Are we at the car or should I follow you?"], block=True),
transitions={'spoken': 'WAIT_TO_FOLLOW_OR_REMEMBER'})
smach.StateMachine.add('WAIT_TO_FOLLOW_OR_REMEMBER',
WaitForOperatorCommand(robot,
possible_commands=[
"follow",
'follow me',
"here is the car",
"stop following",
"stop following me",
],
commands_as_outcomes=True),
transitions={'follow': 'FOLLOW_OPERATOR',
'follow me': 'FOLLOW_OPERATOR',
'here is the car': 'REMEMBER_CAR_LOCATION',
'stop following': 'REMEMBER_CAR_LOCATION',
'stop following me': 'REMEMBER_CAR_LOCATION',
'abort': 'Aborted'})
# 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_FOLLOW_OR_REMEMBER',
'lost_operator': 'ASK_FOLLOW_OR_REMEMBER',
'no_operator': 'ASK_FOLLOW_OR_REMEMBER'})
smach.StateMachine.add('REMEMBER_CAR_LOCATION',
StoreCarWaypoint(robot),
transitions={'success': 'ASK_DESTINATION',
'abort': 'Aborted'})
smach.StateMachine.add('ASK_DESTINATION',
states.Say(robot, ["Where should I bring the groceries?"], block=True),
transitions={'spoken': 'WAIT_FOR_DESTINATION'})
smach.StateMachine.add('WAIT_FOR_DESTINATION',
WaitForOperatorCommand(robot,
possible_commands=challenge_knowledge.waypoints.keys(),
commands_as_userdata=True),
transitions={'success': 'GRAB_ITEM',
'abort': 'Aborted'})
# Grab the item (bag) the operator hands to the robot, when they are at the "car".
smach.StateMachine.add('GRAB_ITEM',
GrabItem(robot, bag_arm_designator, current_item),
transitions={'succeeded': 'ARM_DRIVING_POSE',
'timeout': 'BACKUP_CLOSE_GRIPPER', # For now in simulation timeout is considered a succes.
'failed': 'BACKUP_CLOSE_GRIPPER'},
remapping={'target_room_in': 'command_recognized',
'target_room_out': 'target_room'})
smach.StateMachine.add('BACKUP_CLOSE_GRIPPER',
states.SetGripper(robot, bag_arm_designator, gripperstate=GripperState.CLOSE),
transitions={'succeeded': 'ARM_DRIVING_POSE',
'failed': 'ARM_DRIVING_POSE'})
smach.StateMachine.add('ARM_DRIVING_POSE',
states.ArmToJointConfig(robot, bag_arm_designator, '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),
transitions={'spoken': 'GOTO_DESTINATION'})
smach.StateMachine.add('GOTO_DESTINATION',
NavigateToRoom(robot),
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',
DropBagOnGround(robot, bag_arm_designator),
transitions={'succeeded': 'ASKING_FOR_HELP',
'failed': 'ASKING_FOR_HELP'})
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"),
transitions={'spoken': 'GOTO_CAR'})
#transitions={'success': 'GOTO_CAR',
# 'abort': 'Aborted'})
smach.StateMachine.add('GOTO_CAR',
states.NavigateToWaypoint(robot,
ds.EntityByIdDesignator(robot,
id=challenge_knowledge.waypoint_car['id']),
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'})
#transitions={'success': 'GOTO_CAR',
# 'abort': 'Aborted'})
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),
transitions={'spoken': 'Done'})
ds.analyse_designators(sm, "help_me_carry")
return sm
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,
challenge_knowledge.starting_point,
use_entry_points=True),
transitions={
"Done": "SAY_GOTO_TARGET2",
"Aborted": "SAY_GOTO_TARGET2",
"Failed": "SAY_GOTO_TARGET2"
})
smach.StateMachine.add(
'SAY_GOTO_TARGET1',
states.Say(robot, [
"I will go to target 1 now", "I will now go to target 1",
"Lets go to target 1", "Going to target 1"
],
block=False),
transitions={'spoken': 'GOTO_TARGET1'})
######################################################################################################################################################
#
# TARGET 1
#
######################################################################################################################################################
smach.StateMachine.add('GOTO_TARGET1',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.target1),
challenge_knowledge.target1_radius1),
transitions={
'arrived': 'SAY_TARGET1_REACHED',
'unreachable': 'RESET_ED_TARGET1',
'goal_not_defined': 'RESET_ED_TARGET1'
})
smach.StateMachine.add(
'SAY_TARGET1_REACHED',
states.Say(robot, [
"Reached target 1", "I have arrived at target 1",
"I am now at target 1"
],
block=True),
transitions={'spoken': 'SAY_GOTO_TARGET3'})
smach.StateMachine.add('RESET_ED_TARGET1',
states.ResetED(robot),
transitions={'done': 'GOTO_TARGET1_BACKUP'})
smach.StateMachine.add('GOTO_TARGET1_BACKUP',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.target1),
challenge_knowledge.target1_radius2),
transitions={
'arrived': 'SAY_TARGET1_REACHED',
'unreachable': 'TIMEOUT1',
'goal_not_defined': 'TIMEOUT1'
})
smach.StateMachine.add('TIMEOUT1',
checkTimeOut(
robot,
challenge_knowledge.time_out_seconds),
transitions={
'not_yet': 'GOTO_TARGET1',
'time_out': 'SAY_TARGET1_FAILED'
})
# Should we mention that we failed???
smach.StateMachine.add(
'SAY_TARGET1_FAILED',
states.Say(robot, [
"I am not able to reach target 1", "I cannot reach target 1",
"Target 1 is unreachable"
],
block=True),
transitions={'spoken': 'SAY_GOTO_TARGET3'})
######################################################################################################################################################
#
# TARGET 2
#
######################################################################################################################################################
smach.StateMachine.add(
'SAY_GOTO_TARGET2',
states.Say(robot, [
"I will go to target 2 now", "I will now go to target 2",
"Lets go to target 2", "Going to target 2"
],
block=False),
transitions={'spoken': 'GOTO_TARGET2_PRE'})
smach.StateMachine.add(
'GOTO_TARGET2_PRE',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id=challenge_knowledge.target2_pre),
challenge_knowledge.target2_pre_radius1,
EntityByIdDesignator(robot, id=challenge_knowledge.target2)),
transitions={
'arrived': 'GOTO_TARGET2',
'unreachable': 'TIMEOUT2',
'goal_not_defined': 'TIMEOUT2'
})
smach.StateMachine.add('GOTO_TARGET2',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.target2),
challenge_knowledge.target2_radius1),
transitions={
'arrived': 'SAY_TARGET2_REACHED',
'unreachable': 'DETERMINE_OBJECT',
'goal_not_defined': 'DETERMINE_OBJECT'
})
smach.StateMachine.add(
'DETERMINE_OBJECT',
DetermineObject(robot, challenge_knowledge.target2,
challenge_knowledge.target2_obstacle_radius),
transitions={
'done': 'GOTO_TARGET2_AGAIN',
'timeout': 'GOTO_TARGET2_AGAIN'
})
smach.StateMachine.add('GOTO_TARGET2_AGAIN',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.target2),
challenge_knowledge.target2_radius1),
transitions={
'arrived': 'SAY_TARGET2_REACHED',
'unreachable': 'RESET_ED_TARGET2',
'goal_not_defined': 'RESET_ED_TARGET2'
})
smach.StateMachine.add(
'SAY_TARGET2_REACHED',
states.Say(robot, [
"Reached target 2", "I have arrived at target 2",
"I am now at target 2"
],
block=True),
transitions={'spoken': 'SAY_GOTO_TARGET1'})
smach.StateMachine.add('RESET_ED_TARGET2',
states.ResetED(robot),
transitions={'done': 'GOTO_TARGET2_BACKUP'})
smach.StateMachine.add('GOTO_TARGET2_BACKUP',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.target2),
challenge_knowledge.target2_radius2),
transitions={
'arrived': 'SAY_TARGET2_REACHED',
'unreachable': 'TIMEOUT2',
'goal_not_defined': 'TIMEOUT2'
})
smach.StateMachine.add('TIMEOUT2',
checkTimeOut(
robot,
challenge_knowledge.time_out_seconds),
transitions={
'not_yet': 'GOTO_TARGET2_PRE',
'time_out': 'SAY_TARGET2_FAILED'
})
smach.StateMachine.add(
'SAY_TARGET2_FAILED',
states.Say(robot, [
"I am unable to reach target 2", "I cannot reach target 2",
"Target 2 is unreachable"
],
block=True),
transitions={'spoken': 'SAY_GOTO_TARGET1'})
######################################################################################################################################################
#
# TARGET 3
#
######################################################################################################################################################
smach.StateMachine.add(
'SAY_GOTO_TARGET3',
states.Say(robot, [
"I will go to target 3 now", "I will now go to target 3",
"Lets go to target 3", "Going to target 3"
],
block=False),
transitions={'spoken': 'GOTO_TARGET3'})
smach.StateMachine.add('GOTO_TARGET3',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.target3),
challenge_knowledge.target3_radius1),
transitions={
'arrived': 'SAY_TARGET3_REACHED',
'unreachable': 'RESET_ED_TARGET3',
'goal_not_defined': 'RESET_ED_TARGET3'
})
smach.StateMachine.add(
'SAY_TARGET3_REACHED',
states.Say(robot, [
"Reached target 3", "I have arrived at target 3",
"I am now at target 3"
],
block=True),
transitions={'spoken': 'TURN'})
smach.StateMachine.add('RESET_ED_TARGET3',
states.ResetED(robot),
transitions={'done': 'GOTO_TARGET3_BACKUP'})
smach.StateMachine.add('GOTO_TARGET3_BACKUP',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.target3),
challenge_knowledge.target3_radius2),
transitions={
'arrived': 'SAY_TARGET3_REACHED',
'unreachable': 'TIMEOUT3',
'goal_not_defined': 'TIMEOUT3'
})
smach.StateMachine.add('TIMEOUT3',
checkTimeOut(
robot,
challenge_knowledge.time_out_seconds),
transitions={
'not_yet': 'GOTO_TARGET3',
'time_out': 'SAY_TARGET3_FAILED'
})
# Should we mention that we failed???
smach.StateMachine.add(
'SAY_TARGET3_FAILED',
states.Say(robot, [
"I am unable to reach target 3", "I cannot reach target 3",
"Target 3 is unreachable"
],
block=True),
transitions={'spoken': 'TURN'})
######################################################################################################################################################
#
# Follow waiter
#
######################################################################################################################################################
smach.StateMachine.add('TURN',
Turn(robot, challenge_knowledge.rotation),
transitions={'turned': 'SAY_STAND_IN_FRONT'})
smach.StateMachine.add(
'SAY_STAND_IN_FRONT',
states.Say(robot,
"Please stand in front of me!",
block=True,
look_at_standing_person=True),
transitions={'spoken': 'FOLLOW_WITH_DOOR_CHECK'})
# TODO: Fix concurrence
door_id_designator = VariableDesignator(
challenge_knowledge.target_door_1)
open_door_wp1_des = VariableDesignator(resolve_type=str)
open_door_wp2_des = VariableDesignator(resolve_type=str)
cc = smach.Concurrence(
['stopped', 'no_operator', 'lost_operator'],
default_outcome='no_operator',
child_termination_cb=lambda so: True,
outcome_map={
'stopped': {
'FOLLOW_OPERATOR': 'stopped'
},
# 'stopped': {'FOLLOW_OPERATOR': 'stopped', 'DETERMINE_DOOR': 'door_found'},
'no_operator': {
'FOLLOW_OPERATOR': 'no_operator'
},
# 'no_operator': {'FOLLOW_OPERATOR': 'no_operator', 'DETERMINE_DOOR': 'door_found'},
# 'lost_operator': {'FOLLOW_OPERATOR': 'lost_operator', 'DETERMINE_DOOR': 'preempted'},
'lost_operator': {
'FOLLOW_OPERATOR': 'lost_operator'
}
})
with cc:
smach.Concurrence.add('FOLLOW_OPERATOR',
states.FollowOperator(robot, replan=True))
smach.Concurrence.add('DETERMINE_DOOR',
DetermineDoor(robot, door_id_designator))
smach.StateMachine.add('FOLLOW_WITH_DOOR_CHECK',
cc,
transitions={
'no_operator': 'FOLLOW_WITH_DOOR_CHECK',
'stopped': 'SAY_SHOULD_I_RETURN',
'lost_operator': 'SAY_SHOULD_I_RETURN'
})
# smach.StateMachine.add( 'FOLLOW_OPERATOR', states.FollowOperator(robot, replan=True), transitions={ 'no_operator':'SAY_SHOULD_I_RETURN', 'stopped' : 'SAY_SHOULD_I_RETURN', 'lost_operator' : 'SAY_SHOULD_I_RETURN'})
smach.StateMachine.add('SAY_SHOULD_I_RETURN',
states.Say(robot,
"Should I return to target 3?",
look_at_standing_person=True),
transitions={'spoken': 'HEAR_SHOULD_I_RETURN'})
smach.StateMachine.add('HEAR_SHOULD_I_RETURN',
states.HearOptions(robot, ["yes", "no"]),
transitions={
'no_result': 'SAY_STAND_IN_FRONT',
"yes": "SELECT_WAYPOINTS",
"no": "SAY_STAND_IN_FRONT"
})
smach.StateMachine.add('SELECT_WAYPOINTS',
SelectWaypoints(door_id_designator,
open_door_wp1_des,
open_door_wp2_des),
transitions={'done': 'SAY_GOBACK_ARENA'})
######################################################################################################################################################
#
# RETURN TO ARENA DOOR
#
######################################################################################################################################################
smach.StateMachine.add(
'SAY_GOBACK_ARENA',
states.Say(robot, [
"I will go back to the arena", "I will return to the arena",
"Lets return to the arena", "Going back to the arena",
"Returning to the arena"
],
block=False),
transitions={'spoken': 'GOTO_ARENA_DOOR'})
smach.StateMachine.add(
'GOTO_ARENA_DOOR',
states.NavigateToWaypoint(
robot,
EdEntityDesignator(robot, id_designator=door_id_designator),
challenge_knowledge.target_door_radius),
transitions={
'arrived': 'ARENA_DOOR_REACHED',
'unreachable': 'RESET_ED_ARENA_DOOR',
'goal_not_defined': 'RESET_ED_ARENA_DOOR'
})
smach.StateMachine.add('ARENA_DOOR_REACHED',
states.Say(robot, [
"I am at the door of the arena",
"I have arrived at the door of the arena",
"I am now at the door of the arena"
],
block=True),
transitions={'spoken': 'SAY_OPEN_DOOR'})
smach.StateMachine.add('RESET_ED_ARENA_DOOR',
states.ResetED(robot),
transitions={'done': 'GOTO_ARENA_DOOR_BACKUP'})
smach.StateMachine.add(
'GOTO_ARENA_DOOR_BACKUP',
states.NavigateToWaypoint(
robot,
EdEntityDesignator(robot, id_designator=door_id_designator),
challenge_knowledge.target_door_radius),
transitions={
'arrived': 'ARENA_DOOR_REACHED',
'unreachable': 'TIMEOUT_ARENA_DOOR',
'goal_not_defined': 'TIMEOUT_ARENA_DOOR'
})
smach.StateMachine.add('TIMEOUT_ARENA_DOOR',
checkTimeOut(
robot,
challenge_knowledge.time_out_seconds_door),
transitions={
'not_yet': 'GOTO_ARENA_DOOR',
'time_out': 'SAY_GOTO_ARENA_DOOR_FAILED'
})
smach.StateMachine.add('SAY_GOTO_ARENA_DOOR_FAILED',
states.Say(robot, [
"I am unable to reach the arena door",
"I cannot reach the arena door",
"The arena door is unreachable"
],
block=True),
transitions={'spoken': 'Done'})
######################################################################################################################################################
#
# Opening Door
#
######################################################################################################################################################
smach.StateMachine.add(
'OPEN_DOOR',
states.OpenDoorByPushing(
robot,
EdEntityDesignator(robot, id_designator=open_door_wp1_des),
EdEntityDesignator(robot, id_designator=open_door_wp2_des)),
transitions={
'succeeded': 'SAY_RETURN_TARGET3',
'failed': 'TIMEOUT_ARENA_DOOR_OPENING'
})
smach.StateMachine.add(
'SAY_OPEN_DOOR',
states.Say(robot, [
"I am going to open the door",
"Going to open the door of the arena", "Door, open sesame"
],
block=True),
transitions={'spoken': 'OPEN_DOOR'})
smach.StateMachine.add(
'SAY_OPEN_DOOR_AGAIN',
states.Say(robot, [
"I failed to open the door. I will try it again",
"Let me try again to open the door"
],
block=True),
transitions={'spoken': 'OPEN_DOOR'})
smach.StateMachine.add('TIMEOUT_ARENA_DOOR_OPENING',
checkTimeOut(
robot,
challenge_knowledge.time_out_seconds),
transitions={
'not_yet': 'SAY_OPEN_DOOR_AGAIN',
'time_out': 'SAY_OPEN_DOOR_FAILED'
})
smach.StateMachine.add(
'SAY_OPEN_DOOR_FAILED',
states.Say(robot, [
"I was not able to open the door. I am done with this challange",
"I was not able to open the door. I am done with this challange"
],
block=True),
transitions={'spoken': 'Done'})
######################################################################################################################################################
#
# RETURN TO TARGET 3
#
######################################################################################################################################################
smach.StateMachine.add(
'SAY_RETURN_TARGET3',
states.Say(robot, [
"I will go back to target 3 now", "I will return to target 3",
"Lets go to target 3 again", "Going to target 3, again"
],
block=False),
transitions={'spoken': 'RETURN_TARGET3'})
smach.StateMachine.add(
'RETURN_TARGET3',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot, id=challenge_knowledge.target4),
challenge_knowledge.target4_radius1),
transitions={
'arrived': 'SAY_TARGET3_RETURN_REACHED',
'unreachable': 'RESET_ED_RETURN_TARGET3',
'goal_not_defined': 'RESET_ED_RETURN_TARGET3'
})
smach.StateMachine.add(
'SAY_TARGET3_RETURN_REACHED',
states.Say(robot, [
"Reached target 3 again", "I have arrived at target 3 again",
"I am now at target 3 again"
],
block=True),
transitions={'spoken': 'SAY_GOTO_EXIT'})
smach.StateMachine.add(
'RESET_ED_RETURN_TARGET3',
states.ResetED(robot),
transitions={'done': 'GOTO_RETURN_TARGET3_BACKUP'})
smach.StateMachine.add('GOTO_RETURN_TARGET3_BACKUP',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.target4),
challenge_knowledge.target4_radius2),
transitions={
'arrived': 'SAY_TARGET3_RETURN_REACHED',
'unreachable': 'TIMEOUT3_RETURN',
'goal_not_defined': 'TIMEOUT3_RETURN'
})
smach.StateMachine.add('TIMEOUT3_RETURN',
checkTimeOut(
robot,
challenge_knowledge.time_out_seconds),
transitions={
'not_yet': 'RETURN_TARGET3',
'time_out': 'SAY_RETURN_TARGET3_FAILED'
})
# Should we mention that we failed???
smach.StateMachine.add(
'SAY_RETURN_TARGET3_FAILED',
states.Say(robot, [
"I am unable to reach target 3 again",
"I cannot reach target 3 again", "Target 3 is unreachable"
],
block=True),
transitions={'spoken': 'SAY_GOTO_EXIT'})
######################################################################################################################################################
#
# TARGET EXIT
#
######################################################################################################################################################
smach.StateMachine.add(
'SAY_GOTO_EXIT',
states.Say(robot, [
"I will move to the exit now. See you guys later!",
"I am done with this challenge. Going to the exit"
],
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=challenge_knowledge.exit1),
radius=0.6),
transitions={
'arrived': 'AT_END',
'unreachable': 'RESET_ED_EXIT',
'goal_not_defined': 'RESET_ED_EXIT'
})
smach.StateMachine.add('RESET_ED_EXIT',
states.ResetED(robot),
transitions={'done': 'GO_TO_EXIT_BACKUP'})
smach.StateMachine.add('GO_TO_EXIT_BACKUP',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.exit2),
radius=0.6),
transitions={
'arrived': 'AT_END',
'unreachable': 'RESET_ED_EXIT2',
'goal_not_defined': 'RESET_ED_EXIT2'
})
smach.StateMachine.add('RESET_ED_EXIT2',
states.ResetED(robot),
transitions={'done': 'GO_TO_EXIT_BACKUP2'})
smach.StateMachine.add('GO_TO_EXIT_BACKUP2',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.exit3),
radius=0.6),
transitions={
'arrived': 'GO_TO_EXIT_BACKUP3',
'unreachable': 'RESET_ED_EXIT3',
'goal_not_defined': 'RESET_ED_EXIT3'
})
smach.StateMachine.add('RESET_ED_EXIT3',
states.ResetED(robot),
transitions={'done': 'GO_TO_EXIT_BACKUP3'})
smach.StateMachine.add('GO_TO_EXIT_BACKUP3',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(
robot, id=challenge_knowledge.exit4),
radius=0.6),
transitions={
'arrived': 'AT_END',
'unreachable': 'AT_END',
'goal_not_defined': 'AT_END'
})
smach.StateMachine.add('AT_END',
states.Say(robot, "Goodbye"),
transitions={'spoken': 'Done'})
analyse_designators(sm, "navigation")
return sm
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
msg = "\n".join([
"==============================================",
"== CHALLENGE HELP ME CARRY ==",
"=============================================="
])
rospy.loginfo("\n" + msg)
self.target_destination = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.default_place)
self.car_waypoint = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.waypoint_car['id'])
self.empty_arm_designator = ds.UnoccupiedArmDesignator(
robot,
arm_properties={
"required_goals": [
"handover_to_human", "reset",
challenge_knowledge.driving_bag_pose,
challenge_knowledge.drop_bag_pose
],
"required_gripper_types": [GripperTypes.GRASPING]
},
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()
self.place_position = ds.LockingDesignator(EmptySpotDesignator(
robot,
self.target_destination,
arm_designator=self.bag_arm_designator,
name="placement",
area=challenge_knowledge.default_area),
name="place_position")
# 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': 'ASK_FOR_TASK'
})
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',
hmc_states.WaitForOperatorCommand(
robot,
possible_commands=challenge_knowledge.commands.keys(),
commands_as_outcomes=True),
transitions={
'no': 'FOLLOW_OPERATOR',
'yes': 'REMEMBER_CAR_LOCATION',
'abort': 'Aborted'
})
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'
})
# 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'
})
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.NavigateToWaypoint(
robot, self.target_destination,
challenge_knowledge.default_target_radius),
transitions={
'arrived': 'PUTDOWN_ITEM',
'unreachable': 'GOTO_DESTINATION_BACKUP',
# implement avoid obstacle behaviour later
'goal_not_defined': 'Aborted'
})
smach.StateMachine.add(
'GOTO_DESTINATION_BACKUP',
states.NavigateToWaypoint(
robot, self.target_destination,
challenge_knowledge.backup_target_radius),
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),
transitions={'done': 'ASKING_FOR_HELP'})
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'})
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 setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['done', 'aborted'])
with sm:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'STORE_ROBOCUP_ARENA',
'abort': 'aborted'
})
smach.StateMachine.add('STORE_ROBOCUP_ARENA',
StoreRobocupArena(robot),
transitions={'done': 'HEAD_STRAIGHT'})
smach.StateMachine.add('HEAD_STRAIGHT',
HeadStraight(robot),
transitions={'done': 'SAY_INTRO'})
smach.StateMachine.add('SAY_INTRO',
states.Say(
robot,
"Hi, Guide me out of the arena please.",
look_at_standing_person=True),
transitions={'spoken': 'FOLLOW_INITIAL'})
smach.StateMachine.add('FOLLOW_INITIAL',
states.FollowOperator(robot,
operator_timeout=30,
replan=True),
transitions={
'stopped': 'WAIT_FOR_OPERATOR_COMMAND',
'lost_operator':
'WAIT_FOR_OPERATOR_COMMAND',
'no_operator': 'FOLLOW_INITIAL'
})
smach.StateMachine.add('FOLLOW',
states.FollowOperator(robot,
operator_timeout=30,
ask_follow=False,
learn_face=False,
replan=True),
transitions={
'stopped': 'WAIT_FOR_OPERATOR_COMMAND',
'lost_operator': 'SAY_GUIDE',
'no_operator': 'SAY_GUIDE'
})
smach.StateMachine.add('WAIT_FOR_OPERATOR_COMMAND',
WaitForOperatorCommand(robot),
transitions={
'follow': 'FOLLOW',
'command': 'SAY_GUIDE'
})
smach.StateMachine.add(
'SAY_GUIDE',
states.Say(robot,
"I will guide you back to the robocup arena!",
look_at_standing_person=True),
transitions={'spoken': 'GUIDE_TO_ROBOCUP_ARENA'})
smach.StateMachine.add('GUIDE_TO_ROBOCUP_ARENA',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id="robocup_arena"),
radius=knowledge.back_radius),
transitions={
'arrived':
'SAY_BACK',
'unreachable':
'GUIDE_TO_ROBOCUP_ARENA_BACKUP',
'goal_not_defined':
'GUIDE_TO_ROBOCUP_ARENA_BACKUP'
})
smach.StateMachine.add('GUIDE_TO_ROBOCUP_ARENA_BACKUP',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id="robocup_arena"),
radius=knowledge.back_radius + 0.1),
transitions={
'arrived': 'SAY_BACK',
'unreachable': 'GUIDE_TO_ROBOCUP_ARENA',
'goal_not_defined': 'GUIDE_TO_ROBOCUP_ARENA'
})
smach.StateMachine.add('SAY_BACK',
states.Say(robot,
"We are back in the robocup arena!",
look_at_standing_person=True),
transitions={'spoken': 'done'})
return sm