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, STARTING_POINT),
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':
'ASK_CONTINUE',
'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_2),
radius=0.5),
transitions={
'arrived':
'ASK_CONTINUE',
'unreachable':
'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2',
'goal_not_defined':
'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2'
})
smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id=INTERMEDIATE_3),
radius=0.5),
transitions={
'arrived': 'ASK_CONTINUE',
'unreachable': 'ASK_CONTINUE',
'goal_not_defined': 'ASK_CONTINUE'
})
smach.StateMachine.add("ASK_CONTINUE",
states.AskContinue(robot, 30),
transitions={
'continue': 'SAY_CONTINUEING',
'no_response': 'SAY_CONTINUEING'
})
smach.StateMachine.add(
'SAY_CONTINUEING',
states.Say(robot, [
"I heard continue, so I will move to the exit now. See you guys later!"
],
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=EXIT_1),
radius=0.7),
transitions={
'arrived': 'AT_END',
'unreachable': 'GO_TO_EXIT_2',
'goal_not_defined': 'GO_TO_EXIT_2'
})
smach.StateMachine.add('GO_TO_EXIT_2',
states.NavigateToWaypoint(robot,
EntityByIdDesignator(
robot, id=EXIT_2),
radius=0.5),
transitions={
'arrived': 'AT_END',
'unreachable': 'GO_TO_EXIT_3',
'goal_not_defined': 'GO_TO_EXIT_3'
})
smach.StateMachine.add('GO_TO_EXIT_3',
states.NavigateToWaypoint(robot,
EntityByIdDesignator(
robot, id=EXIT_3),
radius=0.5),
transitions={
'arrived': 'AT_END',
'unreachable': 'RESET_ED_TARGET',
'goal_not_defined': 'AT_END'
})
smach.StateMachine.add('RESET_ED_TARGET',
states.ResetED(robot),
transitions={'done': 'GO_TO_EXIT'})
# Finally amigo will stop and says 'goodbye' to show that he's done.
smach.StateMachine.add('AT_END',
states.Say(robot, "Goodbye"),
transitions={'spoken': 'Done'})
analyse_designators(sm, "rips")
return sm
def __init__(self, robot):
""" Initialization method
:param robot: robot api object
"""
smach.StateMachine.__init__(
self, outcomes=["succeeded", "failed", "aborted"])
# Create designators
trashbin_designator = ds.EdEntityDesignator(
robot=robot,
id=CHALLENGE_KNOWLEDGE.trashbin_id,
name='trashbin_designator')
# Look if there is a second trash bin present
# trashbin_designator2 = None
if hasattr(CHALLENGE_KNOWLEDGE, "trashbin_id2"):
trashbin_designator2 = ds.EdEntityDesignator(
robot=robot,
id=CHALLENGE_KNOWLEDGE.trashbin_id2,
name='trashbin_designator2')
next_state = "HELPER_WAYPOINT"
rospy.loginfo("There is a second trash bin")
else:
rospy.loginfo("There is no second trash bin")
next_state = "ANNOUNCE_END"
# drop_zone_designator = ds.EdEntityDesignator(robot=robot, id=CHALLENGE_KNOWLEDGE.drop_zone_id)
helper_waypoint_designator = ds.EdEntityDesignator(
robot=robot, id=CHALLENGE_KNOWLEDGE.helper_waypoint)
end_waypoint_designator = ds.EdEntityDesignator(
robot=robot, id=CHALLENGE_KNOWLEDGE.end_waypoint)
arm_designator = self.empty_arm_designator = ds.UnoccupiedArmDesignator(
robot, {}, name="empty_arm_designator")
with self:
smach.StateMachine.add("START_CHALLENGE_ROBUST",
states.StartChallengeRobust(
robot,
CHALLENGE_KNOWLEDGE.starting_point),
transitions={
"Done": "SAY_START_CHALLENGE",
"Aborted": "SAY_START_CHALLENGE",
"Failed": "SAY_START_CHALLENGE"
})
smach.StateMachine.add("SAY_START_CHALLENGE",
states.Say(
robot,
"I will start cleaning up the trash",
block=True),
transitions={'spoken': "PICK_UP_TRASH"})
smach.StateMachine.add("PICK_UP_TRASH",
PickUpTrash(
robot=robot,
trashbin_designator=trashbin_designator,
arm_designator=arm_designator),
transitions={
"succeeded": "DROP_DOWN_TRASH",
"failed": "HELPER_WAYPOINT",
"aborted": "ANNOUNCE_END"
})
smach.StateMachine.add(
"DROP_DOWN_TRASH",
DropDownTrash(robot=robot,
drop_zone_id=CHALLENGE_KNOWLEDGE.drop_zone_id),
transitions={
"succeeded": "ANNOUNCE_TASK",
"failed": "failed",
"aborted": "aborted"
})
smach.StateMachine.add(
"ANNOUNCE_TASK",
states.Say(robot,
"First bag has been dropped at the collection zone",
block=False),
transitions={'spoken': next_state})
if next_state == "HELPER_WAYPOINT":
smach.StateMachine.add(
"HELPER_WAYPOINT",
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=helper_waypoint_designator),
transitions={
"arrived": "PICK_UP_TRASH2",
"goal_not_defined": "PICK_UP_TRASH2",
"unreachable": "PICK_UP_TRASH2"
})
smach.StateMachine.add(
"PICK_UP_TRASH2",
PickUpTrash(robot=robot,
trashbin_designator=trashbin_designator2,
arm_designator=arm_designator),
transitions={
"succeeded": "DROP_DOWN_TRASH2",
"failed": "ANNOUNCE_END",
"aborted": "ANNOUNCE_END"
})
smach.StateMachine.add(
"DROP_DOWN_TRASH2",
DropDownTrash(
robot=robot,
drop_zone_id=CHALLENGE_KNOWLEDGE.drop_zone_id),
transitions={
"succeeded": "ANNOUNCE_TASK2",
"failed": "failed",
"aborted": "aborted"
})
smach.StateMachine.add(
"ANNOUNCE_TASK2",
states.Say(
robot,
"Second bag has been dropped at the collection zone."
"All the thrash has been taken care of",
block=False),
transitions={'spoken': 'ANNOUNCE_END'})
smach.StateMachine.add("ANNOUNCE_END",
states.Say(
robot,
"I have finished taking out the trash.",
block=False),
transitions={'spoken': 'NAVIGATE_OUT'})
smach.StateMachine.add(
"NAVIGATE_OUT",
states.NavigateToWaypoint(
robot=robot, waypoint_designator=end_waypoint_designator),
transitions={
"arrived": "succeeded",
"goal_not_defined": "succeeded",
"unreachable": "succeeded"
})
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=["Done", "Aborted"])
hmi_result_des = ds.VariableDesignator(resolve_type=HMIResult)
information_point_id_designator = ds.FuncDesignator(ds.AttrDesignator(
hmi_result_des, "semantics", resolve_type=unicode),
str,
resolve_type=str)
information_point_designator = ds.EdEntityDesignator(
robot, id_designator=information_point_id_designator)
with self:
single_item = InformMachine(robot)
if START_ROBUST:
smach.StateMachine.add("START_CHALLENGE",
states.StartChallengeRobust(
robot, INITIAL_POSE_ID),
transitions={
"Done": "ASK_WHERE_TO_GO",
"Aborted": "Aborted",
"Failed": "Aborted"
})
smach.StateMachine.add(
"ASK_WHERE_TO_GO",
states.Say(
robot,
"Near which furniture object should I go to start guiding people?"
),
transitions={"spoken": "WAIT_WHERE_TO_GO"})
smach.StateMachine.add(
"WAIT_WHERE_TO_GO",
states.HearOptionsExtra(
robot=robot,
spec_designator=ds.Designator(
initial_value=START_GRAMMAR),
speech_result_designator=hmi_result_des.writeable),
transitions={
"heard": "ASK_CONFIRMATION",
"no_result": "ASK_WHERE_TO_GO"
}) # ToDo: add fallbacks #option: STORE_STARTING_POSE
smach.StateMachine.add(
"ASK_CONFIRMATION",
states.Say(robot, [
"I hear that you would like me to start the tours at"
" the {place}, is this correct?"
],
place=information_point_id_designator,
block=True),
transitions={"spoken": "CONFIRM_LOCATION"})
smach.StateMachine.add("CONFIRM_LOCATION",
states.HearOptions(
robot=robot, options=["yes", "no"]),
transitions={
"yes": "MOVE_OUT_OF_MY_WAY",
"no": "ASK_WHERE_TO_GO",
"no_result": "ASK_WHERE_TO_GO"
})
smach.StateMachine.add(
"MOVE_OUT_OF_MY_WAY",
states.Say(robot,
"Please move your ass so I can get going!"),
transitions={"spoken": "TC_MOVE_TIME"})
smach.StateMachine.add("TC_MOVE_TIME",
states.WaitTime(robot=robot,
waittime=3),
transitions={
"waited": "NAV_TO_START",
"preempted": "Aborted"
})
smach.StateMachine.add(
"NAV_TO_START",
states.NavigateToSymbolic(
robot=robot,
entity_designator_area_name_map={
information_point_designator: "in_front_of"
},
entity_lookat_designator=information_point_designator),
transitions={
"arrived": "TURN_AROUND",
"unreachable": "WAIT_NAV_BACKUP",
"goal_not_defined": "Aborted"
}) # If this happens: never mind
smach.StateMachine.add("WAIT_NAV_BACKUP",
states.WaitTime(robot, 3.0),
transitions={
"waited": "NAV_TO_START_BACKUP",
"preempted": "Aborted"
})
smach.StateMachine.add(
"NAV_TO_START_BACKUP",
states.NavigateToSymbolic(
robot=robot,
entity_designator_area_name_map={
information_point_designator: "near"
},
entity_lookat_designator=information_point_designator),
transitions={
"arrived": "TURN_AROUND",
"unreachable":
"SAY_CANNOT_REACH_WAYPOINT", # Current pose backup
"goal_not_defined": "Aborted"
}) # If this happens: never mind
@smach.cb_interface(outcomes=["done"])
def _turn_around(userdata=None):
""" Turns the robot approximately 180 degrees around """
v_th = 0.5
robot.base.force_drive(vx=0.0,
vy=0.0,
vth=v_th,
timeout=math.pi / v_th)
return "done"
smach.StateMachine.add(
"TURN_AROUND",
smach.CBState(_turn_around),
transitions={"done": "STORE_STARTING_POSE"})
smach.StateMachine.add(
"SAY_CANNOT_REACH_WAYPOINT",
states.Say(
robot, "I am not able to reach the starting point."
"I'll use this as starting point"),
transitions={"spoken": "STORE_STARTING_POSE"})
else:
smach.StateMachine.add("INITIALIZE",
states.Initialize(robot),
transitions={
"initialized":
"STORE_STARTING_POSE",
"abort": "Aborted"
})
## This is purely for a back up scenario until the range iterator
@smach.cb_interface(outcomes=["succeeded"])
def store_pose(userdata=None):
base_loc = robot.base.get_location()
base_pose = base_loc.frame
location_id = INFORMATION_POINT_ID
robot.ed.update_entity(id=location_id,
frame_stamped=FrameStamped(
base_pose, "/map"),
type="waypoint")
return "succeeded"
smach.StateMachine.add("STORE_STARTING_POSE",
smach.CBState(store_pose),
transitions={"succeeded": "RANGE_ITERATOR"})
# Begin setup iterator
# The exhausted argument should be set to the prefered state machine outcome
range_iterator = smach.Iterator(
outcomes=["succeeded",
"failed"], # Outcomes of the iterator state
input_keys=[],
output_keys=[],
it=lambda: range(1000),
it_label="index",
exhausted_outcome="succeeded")
with range_iterator:
smach.Iterator.set_contained_state(
"SINGLE_ITEM",
single_item,
loop_outcomes=["succeeded", "failed"])
smach.StateMachine.add("RANGE_ITERATOR", range_iterator, {
"succeeded": "AT_END",
"failed": "Aborted"
})
# End setup iterator
smach.StateMachine.add("AT_END",
states.Say(robot, "Goodbye"),
transitions={"spoken": "Done"})
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 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 setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
robot.ed.reset()
with sm:
smach.StateMachine.add("INITIALIZE",
robot_smach_states.Initialize(robot),
transitions={
"initialized": "SAY_WAITING_FOR_TRIGGER",
"abort": "Aborted"
})
# Start challenge via StartChallengeRobust, skipped atm
smach.StateMachine.add("START_CHALLENGE_ROBUST",
robot_smach_states.StartChallengeRobust(
robot,
challenge_knowledge.starting_point,
door=False),
transitions={
"Done": "SAY_WAITING_FOR_TRIGGER",
"Failed": "Aborted",
"Aborted": "Aborted"
})
smach.StateMachine.add(
'SAY_WAITING_FOR_TRIGGER',
robot_smach_states.Say(robot, [
"Trigger me if you need me!", "Waiting for trigger",
"Waiting for you to call me!"
],
block=False),
transitions={"spoken": "WAIT_FOR_TRIGGER"})
smach.StateMachine.add('WAIT_FOR_TRIGGER',
robot_smach_states.WaitForTrigger(
robot, ["gpsr"], "/amigo/trigger"),
transitions={
"gpsr": "VERIFY",
"preempted": "VERIFY"
})
smach.StateMachine.add('VERIFY',
VerifyWorldModelInfo(robot),
transitions={
"done": "NAVIGATE_TO_ASK_WAYPOINT",
"failed": "SAY_KNOWLEDGE_NOT_COMPLETE"
})
smach.StateMachine.add(
'SAY_KNOWLEDGE_NOT_COMPLETE',
robot_smach_states.Say(robot, [
"My knowledge of the world is not complete!",
"Please give me some more information!"
],
block=False),
transitions={"spoken": "SAY_WAITING_FOR_TRIGGER"})
smach.StateMachine.add("NAVIGATE_TO_ASK_WAYPOINT",
robot_smach_states.NavigateToWaypoint(
robot=robot,
waypoint_designator=EntityByIdDesignator(
robot=robot,
id=challenge_knowledge.ask_waypoint),
radius=0.3),
transitions={
'arrived':
'DETERMINE_WHAT_TO_CLEAN_INSPECT',
'unreachable':
'DETERMINE_WHAT_TO_CLEAN_INSPECT',
'goal_not_defined':
'DETERMINE_WHAT_TO_CLEAN_INSPECT'
})
smach.StateMachine.add(
"DETERMINE_WHAT_TO_CLEAN_INSPECT",
DetermineWhatToCleanInspect(robot),
transitions={
place["entity_id"]: "CLEAN_INSPECT_%s" % place["entity_id"]
for place in challenge_knowledge.inspection_places
})
for place in challenge_knowledge.inspection_places:
smach.StateMachine.add(
"CLEAN_INSPECT_%s" % place["entity_id"],
CleanInspect(robot, place["entity_id"], place["room_id"],
place["navigate_area"], place["segment_areas"]),
transitions={"done": "NAVIGATE_TO_ASK_WAYPOINT"})
return sm
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
with self:
#Part I: Set a table
smach.StateMachine.add(
'ENTER_ROOM', # Enter the room
states.StartChallengeRobust(robot, knowledge.initial_pose),
transitions={
'Done': 'ANNOUNCEMENT',
'Aborted': 'Aborted',
'Failed': 'Aborted'
})
smach.StateMachine.add(
'ANNOUNCEMENT',
states.Say(
robot,
"Let's see if my master has a task for me! Moving to the meeting point.",
block=True),
transitions={'spoken': 'NAVIGATE_TO_WAYPOINT_I'})
smach.StateMachine.add('NAVIGATE_TO_WAYPOINT_I',
states.NavigateToWaypoint(
robot,
ds.EntityByIdDesignator(
robot=robot,
id=knowledge.starting_pose),
radius=0.3),
transitions={
'arrived': 'FETCH_COMMAND_I',
'unreachable': 'FETCH_COMMAND_I',
'goal_not_defined': 'FETCH_COMMAND_I'
})
smach.StateMachine.add(
'FETCH_COMMAND_I', # Hear "set the table"
HearFetchCommand(robot, 15.0),
transitions={'heard': 'ASK_FOR_MEAL'})
smach.StateMachine.add('ASK_FOR_MEAL',
states.Say(robot,
"What should I serve, master?",
block=True),
transitions={'spoken': 'SET_THE_TABLE'})
smach.StateMachine.add(
'SET_THE_TABLE', # Take order and Set the table (bring the objects to the table)
states.Initialize(robot),
transitions={
'initialized': 'SERVE_MEAL',
'abort': 'Aborted'
},
remapping={'meal': 'meal'})
smach.StateMachine.add(
'SERVE_MEAL', # Serve the meal (for example: pour milk into the bowl)
states.Initialize(robot),
transitions={
'initialized': 'CORRECT_OBJECT_POSITIONS',
'abort': 'Aborted'
})
smach.StateMachine.add(
'CORRECT_OBJECT_POSITIONS', # Inspect table and correct the moved objects
states.Initialize(robot),
transitions={
'initialized': 'ANNOUNCE_TASK_COMPLETION',
'abort': 'Aborted'
})
smach.StateMachine.add(
'ANNOUNCE_TASK_COMPLETION',
states.Say(
robot,
"The table is set! Moving to the meeting point for the next task.",
block=True),
transitions={'spoken': 'NAVIGATE_TO_WAYPOINT_II'})
#Part II: Clean the table
smach.StateMachine.add('NAVIGATE_TO_WAYPOINT_II',
states.NavigateToWaypoint(
robot,
ds.EntityByIdDesignator(
robot=robot,
id=knowledge.starting_pose),
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),
transitions={'heard': 'CLEAR_UP'})
smach.StateMachine.add(
'CLEAR_UP', # Clear up the table (bring the objects to their default location)
states.Initialize(robot),
transitions={
'initialized': 'CLEAN_THE_TABLE',
'abort': 'Aborted'
})
smach.StateMachine.add(
'CLEAN_THE_TABLE', # Inspect for spots and spills and clean them
states.Initialize(robot),
transitions={
'initialized': 'END_CHALLENGE',
'abort': 'Aborted'
})
# End
smach.StateMachine.add('END_CHALLENGE',
states.Say(robot, "I am finally free!"),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "set_a_table")
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['done', 'failed', 'aborted'])
with sm:
smach.StateMachine.add('START_CHALLENGE_ROBUST',
states.StartChallengeRobust(
robot, STARTING_POINT),
transitions={
'Done': 'GO_TO_SEARCH_POSE',
'Aborted': 'aborted',
'Failed': 'GO_TO_SEARCH_POSE'
})
smach.StateMachine.add('SAY_START',
states.Say(robot,
"Finding your mates, here we go!",
block=False),
transitions={'spoken': 'GO_TO_SEARCH_POSE'})
smach.StateMachine.add('GO_TO_SEARCH_POSE',
states.NavigateToWaypoint(
robot,
ds.EntityByIdDesignator(robot,
id=SEARCH_POINT),
radius=0.375),
transitions={
'arrived': 'RISE_FOR_THE_PEOPLE',
'goal_not_defined': 'failed',
'unreachable': 'WAIT_SEARCH_POSE'
})
smach.StateMachine.add('WAIT_SEARCH_POSE',
states.WaitTime(robot, 5),
transitions={
'preempted': 'aborted',
'waited': 'GO_TO_SEARCH_POSE'
})
# noinspection PyUnusedLocal
@smach.cb_interface(outcomes=["done"])
def _rise_for_the_people(userdata=None):
""" Resets the location hmi attempt so that each operator gets three attempts """
robot.arms.values()[0]._send_joint_trajectory(
[[0.70, -1.9, 0.0, -1.57, 0.0]])
robot.speech.speak(
"Hi there. My Name is Hero. I'm looking for the mates of my operator",
block=False)
return "done"
smach.StateMachine.add("RISE_FOR_THE_PEOPLE",
smach.CBState(_rise_for_the_people),
transitions={"done": "LOCATE_PEOPLE"})
# locate all four people
smach.StateMachine.add('LOCATE_PEOPLE',
LocatePeople(robot, room_id=ROOM_ID),
transitions={
'done': 'RESET_FOR_DRIVING',
})
# noinspection PyUnusedLocal
@smach.cb_interface(outcomes=["done"])
def _reset_for_driving(userdata=None):
""" Resets the location hmi attempt so that each operator gets three attempts """
robot.speech.speak("Thank you for your attention", block=False)
robot.arms.values()[0]._send_joint_trajectory([
[0.01, -1.9, 0.0, -1.57, 0.0], # Inspect with q0 low
[0.01, 0.0, -1.57, -1.57, 0.0]
]) # Reset
return "done"
smach.StateMachine.add("RESET_FOR_DRIVING",
smach.CBState(_reset_for_driving),
transitions={"done": "GO_BACK_TO_OPERATOR"})
# drive back to the operator to describe the mates
smach.StateMachine.add('GO_BACK_TO_OPERATOR',
states.NavigateToWaypoint(
robot,
ds.EntityByIdDesignator(robot,
id=OPERATOR_POINT),
radius=0.7,
look_at_designator=ds.EntityByIdDesignator(
robot, id=OPERATOR_POINT)),
transitions={
'arrived': 'REPORT_PEOPLE',
'goal_not_defined': 'REPORT_PEOPLE',
'unreachable': 'WAIT_GO_BACK'
})
smach.StateMachine.add('WAIT_GO_BACK',
states.WaitTime(robot, 5),
transitions={
'preempted': 'aborted',
'waited': 'GO_BACK_TO_OPERATOR'
})
# check how to uniquely define them # ToDo: make this more interesting
smach.StateMachine.add('REPORT_PEOPLE',
ReportPeople(robot),
transitions={'done': 'done'})
return sm
def __init__(self, robot):
""" Constructor
:param robot: robot object
"""
smach.StateMachine.__init__(self, outcomes=["Done", "Aborted"])
# Designators
furniture_move_designator = PointingDesignator(robot=robot)
furniture_pick_designator = PointingDesignator(robot=robot)
furniture_place_designator = PointingDesignator(robot=robot)
with self:
# Start challenge
smach.StateMachine.add("START_CHALLENGE",
states.StartChallengeRobust(
robot=robot,
initial_pose=knowledge.initial_pose),
transitions={
"Done": "NAVIGATE_TO_MEETING_POINT0",
"Aborted": "Aborted",
"Failed": "Aborted"
})
# Move to meeting point
# ToDo: add challenge knowledge
smach.StateMachine.add("NAVIGATE_TO_MEETING_POINT0",
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=EdEntityDesignator(
robot=robot,
id=knowledge.meeting_point),
radius=0.15),
transitions={
"arrived": "DETECT_POINTING0",
"unreachable": "DETECT_POINTING0",
"goal_not_defined": "DETECT_POINTING0"
})
# Wait for the operator to appear and detect what he's pointing at
smach.StateMachine.add("DETECT_POINTING0",
PointingDetector(
robot=robot,
designator=furniture_move_designator,
default_entity_id="couch_table",
super_type="furniture"),
transitions={
"succeeded": "MOVE_TO_ITEM",
"failed": "DETECT_POINTING0"
})
# Move to jury table
smach.StateMachine.add("MOVE_TO_ITEM",
states.NavigateToSymbolic(
robot,
{furniture_move_designator: "near"},
furniture_move_designator),
transitions={
"arrived": "SAY_HI_TO_JURY",
"unreachable": "SAY_HI_TO_JURY",
"goal_not_defined": "SAY_HI_TO_JURY"
})
smach.StateMachine.add(
"SAY_HI_TO_JURY",
states.Say(robot, "Hi guys, I'm glad you're here", block=True),
transitions={"spoken": "NAVIGATE_TO_MEETING_POINT1"})
smach.StateMachine.add("NAVIGATE_TO_MEETING_POINT1",
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=EdEntityDesignator(
robot=robot,
id=knowledge.meeting_point),
radius=0.15),
transitions={
"arrived": "DETECT_POINTING1",
"unreachable": "DETECT_POINTING1",
"goal_not_defined": "DETECT_POINTING1"
})
# Wait for the operator to appear and detect what he's pointing at
smach.StateMachine.add("DETECT_POINTING1",
PointingDetector(
robot=robot,
designator=furniture_pick_designator,
default_entity_id="desk",
super_type="furniture"),
transitions={
"succeeded": "PICKUP_ITEM",
"failed": "DETECT_POINTING1"
})
# Inspect and pickup
smach.StateMachine.add(
"PICKUP_ITEM",
PickupItem(robot=robot,
furniture_designator=furniture_pick_designator),
transitions={
"succeeded": "NAVIGATE_TO_MEETING_POINT2",
"failed": "NAVIGATE_TO_MEETING_POINT2"
})
# Move back to meeting point
# ToDo: add challenge knowledge
smach.StateMachine.add("NAVIGATE_TO_MEETING_POINT2",
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=EdEntityDesignator(
robot=robot,
id=knowledge.meeting_point),
radius=0.15),
transitions={
"arrived": "DETECT_POINTING2",
"unreachable": "DETECT_POINTING2",
"goal_not_defined": "DETECT_POINTING2"
})
# Wait for the operator to appear and detect what he's pointing at
smach.StateMachine.add("DETECT_POINTING2",
PointingDetector(
robot=robot,
designator=furniture_place_designator,
default_entity_id="side_table",
super_type="furniture"),
transitions={
"succeeded": "PLACE_ITEM",
"failed": "DETECT_POINTING2"
})
# Place the object
smach.StateMachine.add(
"PLACE_ITEM",
PlaceSingleItem(robot=robot,
place_designator=furniture_place_designator),
transitions={
"succeeded": "SAY_DONE",
"failed": "SAY_DONE"
})
# Say that we're done
smach.StateMachine.add(
"SAY_DONE",
states.Say(
robot,
"That's it for today, you can have a closer look at my"
"skills on the screen",
block=False),
transitions={"spoken": "RESET_HEAD"})
@smach.cb_interface(outcomes=["done"])
def reset_head(userdata=None):
""" Resets the head """
print "Resetting head"
robot.head.reset()
rospy.sleep(rospy.Duration(1.0))
return "done"
smach.StateMachine.add("RESET_HEAD",
smach.CBState(reset_head),
transitions={"done": "Done"})
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
robot.ed.reset()
with sm:
smach.StateMachine.add("INITIALIZE",
robot_smach_states.Initialize(robot),
transitions={
"initialized": "SAY_WAITING_FOR_TRIGGER",
"abort": "Aborted"
})
# Start challenge via StartChallengeRobust, skipped atm
smach.StateMachine.add("START_CHALLENGE_ROBUST",
robot_smach_states.StartChallengeRobust(
robot,
challenge_knowledge.starting_point,
door=False),
transitions={
"Done": "SAY_WAITING_FOR_TRIGGER",
"Failed": "Aborted",
"Aborted": "Aborted"
})
smach.StateMachine.add(
'SAY_WAITING_FOR_TRIGGER',
robot_smach_states.Say(robot, [
"Trigger me if you need me!", "Waiting for trigger",
"Waiting for you to call me!"
],
block=False),
transitions={"spoken": "WAIT_FOR_TRIGGER"})
smach.StateMachine.add('WAIT_FOR_TRIGGER',
robot_smach_states.WaitForTrigger(
robot, ["gpsr"], "/amigo/trigger"),
transitions={
"gpsr": "VERIFY",
"preempted": "VERIFY"
})
smach.StateMachine.add('VERIFY',
VerifyWorldModelInfo(robot),
transitions={
"done": "SAY_START_CHALLENGE",
"failed": "SAY_KNOWLEDGE_NOT_COMPLETE"
})
smach.StateMachine.add(
'SAY_KNOWLEDGE_NOT_COMPLETE',
robot_smach_states.Say(robot, [
"My knowledge of the world is not complete!",
"Please give me some more information!"
],
block=False),
transitions={"spoken": "SAY_WAITING_FOR_TRIGGER"})
smach.StateMachine.add(
'SAY_START_CHALLENGE',
robot_smach_states.Say(robot, [
"Starting R5COP Cooperative cleaning demonstrator",
"What a mess here, let's clean this room!",
"Let's see if I can find some garbage here",
"All I want to do is clean this mess up!"
],
block=False),
transitions={"spoken": "INSPECT_0"})
for i, place in enumerate(challenge_knowledge.inspection_places):
next_i = i + 1 if i + 1 < len(
challenge_knowledge.inspection_places) else 0
smach.StateMachine.add(
"INSPECT_%d" % i,
CleanInspect(robot, place["entity_id"], place["room_id"],
place["navigate_area"], place["segment_areas"],
challenge_knowledge.known_types),
transitions={"done": "INSPECT_%d" % next_i})
return sm
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
# Designators
# Room to search through
roomr = ds.VariableDesignator(resolve_type=str)
roomw = roomr.writeable
# Cleanup location as defined in local knowledge
cleanup_locationsr = ds.VariableDesignator([{'1': '2', '3': '4'}])
cleanup_locationsw = cleanup_locationsr.writeable
location_des = ds.VariableDesignator(resolve_type=dict)
with sm:
smach.StateMachine.add("START_ROBUST",
robot_smach_states.StartChallengeRobust(
robot, challenge_knowledge.starting_point),
transitions={
"Done": "GO_TO_WAITING_POINT",
"Aborted": "GO_TO_WAITING_POINT",
"Failed": "GO_TO_WAITING_POINT"
})
smach.StateMachine.add("GO_TO_WAITING_POINT",
robot_smach_states.NavigateToWaypoint(
robot,
ds.EntityByIdDesignator(
robot,
challenge_knowledge.waiting_point)),
transitions={
"arrived": "INQUIRE_ROOM",
"unreachable": "GO_TO_WAITING_POINT1",
"goal_not_defined": "GO_TO_WAITING_POINT1"
})
smach.StateMachine.add("GO_TO_WAITING_POINT1",
robot_smach_states.NavigateToWaypoint(
robot,
ds.EntityByIdDesignator(
robot,
challenge_knowledge.waiting_point),
radius=0.3),
transitions={
"arrived": "INQUIRE_ROOM",
"unreachable": "INQUIRE_ROOM",
"goal_not_defined": "INQUIRE_ROOM"
})
smach.StateMachine.add("INQUIRE_ROOM",
AskWhichRoomToClean(
robot,
ds.Designator(challenge_knowledge.grammar),
roomw, cleanup_locationsw),
transitions={"done": "VERIFY"})
smach.StateMachine.add('VERIFY',
VerifyWorldModelInfo(robot),
transitions={
"done": "SAY_START_CHALLENGE",
"failed": "SAY_KNOWLEDGE_NOT_COMPLETE"
})
smach.StateMachine.add(
'SAY_KNOWLEDGE_NOT_COMPLETE',
robot_smach_states.Say(robot, [
"My knowledge of the world is not complete!",
"Please give me some more information!"
],
block=False),
transitions={"spoken": "Aborted"})
smach.StateMachine.add('SAY_START_CHALLENGE',
robot_smach_states.Say(robot, [
"Starting the cleanup challenge",
"What a mess here, let's clean this room!",
"Let's see if I can find some garbage here",
"All I want to do is clean this mess up!"
],
block=False),
transitions={"spoken": "ITERATE_NEXT_LOC"})
smach.StateMachine.add('ITERATE_NEXT_LOC',
robot_smach_states.IterateDesignator(
cleanup_locationsr, location_des.writeable),
transitions={
"next": "INSPECT",
"stop_iteration": "RETURN_TO_OPERATOR"
})
smach.StateMachine.add("INSPECT",
CleanInspect(robot, location_des),
transitions={"done": "ITERATE_NEXT_LOC"})
smach.StateMachine.add("RETURN_TO_OPERATOR",
robot_smach_states.NavigateToWaypoint(
robot=robot,
waypoint_designator=ds.EntityByIdDesignator(
robot=robot,
id=challenge_knowledge.starting_point),
radius=0.3),
transitions={
"arrived": "SAY_CLEANED_ROOM",
"unreachable": "SAY_CLEANED_ROOM",
"goal_not_defined": "SAY_CLEANED_ROOM"
})
smach.StateMachine.add(
'SAY_CLEANED_ROOM',
robot_smach_states.Say(robot, [
"I successfully cleaned the {room}!",
"All done in the {room}. Am I a good robot now?",
"There, I cleaned up your mess in the {room}, are you happy now!"
],
room=roomr,
block=False),
transitions={"spoken": "Done"})
return sm