def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
with self:
smach.StateMachine.add('INITIALIZE',
Initialize(robot),
transitions={
'initialized': 'FETCH_COMMAND',
'abort': 'Aborted'
})
smach.StateMachine.add(
'FETCH_COMMAND', # Hear "set the table"
HearFetchCommand(robot, 15.0),
transitions={'heard': 'END_CHALLENGE'})
# End
smach.StateMachine.add('END_CHALLENGE',
Say(robot, "I am finally free!"),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "set_a_table")
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 __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
self.target_destination = ds.EntityByIdDesignator(robot, id=challenge_knowledge.default_place)
self.car_waypoint = ds.EntityByIdDesignator(robot, id=challenge_knowledge.waypoint_car['id'])
self.place_position = ds.LockingDesignator(ds.EmptySpotDesignator(robot, self.target_destination,
name="placement",
area=challenge_knowledge.default_area),
name="place_position")
self.empty_arm_designator = ds.UnoccupiedArmDesignator(robot, {}, 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()
# 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 __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 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'])
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):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# start_waypoint = ds.EntityByIdDesignator(robot, id="manipulation_init_pose", name="start_waypoint")
pdf_writer = WritePdf(robot=robot)
with self:
single_item = ManipulateMachine(robot, pdf_writer=pdf_writer)
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized':
'SAY_UNABLE_TO_OPEN_DOOR',
'abort': 'Aborted'
})
smach.StateMachine.add('SAY_UNABLE_TO_OPEN_DOOR',
states.Say(
robot,
"I am unable to open the shelf door, "
"can you please open it for me?"),
transitions={'spoken': 'AWAIT_START'})
smach.StateMachine.add("AWAIT_START",
states.AskContinue(robot),
transitions={
'continue': "MOVE_TABLE",
'no_response': 'AWAIT_START'
})
cabinet = ds.EntityByIdDesignator(robot, id=CABINET)
room = ds.EntityByIdDesignator(robot, id=ROOM)
@smach.cb_interface(outcomes=["done"])
def move_table(userdata=None, manipulate_machine=None):
""" Moves the entities for this challenge to the correct poses"""
# Determine where to perform the challenge
robot_pose = robot.base.get_location()
ENTITY_POSES.sort(key=lambda tup:
(tup[0].frame.p - robot_pose.frame.p).Norm())
cabinet_id = ENTITY_POSES[0][2]
table_id = ENTITY_POSES[0][3]
# Update the world model
robot.ed.update_entity(id="balcony_shelf",
frame_stamped=FrameStamped(
kdl.Frame(kdl.Rotation(),
kdl.Vector(0.0, 3.0,
0.0)),
frame_id="map"))
robot.ed.update_entity(id=cabinet_id,
frame_stamped=ENTITY_POSES[0][0])
robot.ed.update_entity(id=table_id,
frame_stamped=ENTITY_POSES[0][1])
# Update designators
cabinet.id_ = ENTITY_POSES[0][2]
room.id_ = ENTITY_POSES[0][4]
# Update manipulate machine
manipulate_machine.place_entity_designator.id_ = cabinet_id
manipulate_machine.place_designator._area = ENTITY_POSES[0][5]
manipulate_machine.place_designator.place_location_designator.id = cabinet_id
manipulate_machine.table_designator.id_ = table_id
manipulate_machine.cabinet.id_ = ENTITY_POSES[0][2]
return "done"
smach.StateMachine.add("MOVE_TABLE",
smach.CBState(move_table,
cb_args=[single_item]),
transitions={'done': 'NAV_TO_START'})
smach.StateMachine.add("NAV_TO_START",
states.NavigateToSymbolic(
robot, {cabinet: "in_front_of"},
cabinet),
transitions={
'arrived': 'INSPECT_SHELVES',
'unreachable': 'INSPECT_SHELVES',
'goal_not_defined': 'INSPECT_SHELVES'
})
smach.StateMachine.add("INSPECT_SHELVES",
InspectShelves(robot, cabinet),
transitions={
'succeeded': 'WRITE_PDF_SHELVES',
'nothing_found': 'WRITE_PDF_SHELVES',
'failed': 'WRITE_PDF_SHELVES'
})
smach.StateMachine.add("WRITE_PDF_SHELVES",
pdf_writer,
transitions={"done": "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(5),
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'})
ds.analyse_designators(self, "manipulation")
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'])
start_waypoint = ds.EntityByIdDesignator(robot,
id="manipulation_init_pose",
name="start_waypoint")
placed_items = []
with sm:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'INIT_WM',
'abort': 'Aborted'
})
smach.StateMachine.add("INIT_WM",
InitializeWorldModel(robot),
transitions={'done': 'AWAIT_START'})
# smach.StateMachine.add("INSTRUCT_WAIT_FOR_DOOR",
# states.Say(robot, ["Hi there, I will now wait until you remove the cup",
# "I'm waiting for you to remove the cup"], block=False),
# transitions={"spoken": "WAIT_FOR_DOOR"})
#
# smach.StateMachine.add("WAIT_FOR_DOOR",
# states.WaitForDoorOpen(robot, timeout=10),
# transitions={"closed": "DOOR_CLOSED",
# "open": "AWAIT_START"})
#
# smach.StateMachine.add("DOOR_CLOSED",
# states.Say(robot, ["I am waiting for you to remove the cup",
# "I'd start, if you remove the cup from my laser"]),
# transitions={"spoken": "WAIT_FOR_DOOR"})
if USE_SLAM:
drive_state = "RESET_ED_SLAM"
else:
drive_state = "NAV_TO_START"
smach.StateMachine.add("AWAIT_START",
states.AskContinue(robot),
transitions={
'continue': drive_state,
'no_response': 'AWAIT_START'
})
cabinet = ds.EntityByIdDesignator(robot, id=CABINET)
room = ds.EntityByIdDesignator(robot, id=ROOM)
if USE_SLAM:
# vth = 1.0
# smach.StateMachine.add("NAV_TO_FIT_POSE",
# ForceDrive(robot, 0, 0, vth, 3.14/vth),
# transitions={'done': 'FIT_ENTITY'})
smach.StateMachine.add("RESET_ED_SLAM",
states.ResetED(robot),
transitions={'done': 'FIT_ENTITY'})
smach.StateMachine.add("FIT_ENTITY",
FitEntity(robot, CABINET),
transitions={
'succeeded': 'NAV_TO_START',
'failed': 'SAY_FITTING_FAILED'
})
smach.StateMachine.add(
"SAY_FITTING_FAILED",
states.Say(robot, [
"Fitting the {0} failed, I will stop now.".format(CABINET)
],
mood="sad"),
transitions={'spoken': 'Aborted'})
smach.StateMachine.add("NAV_TO_START",
states.NavigateToSymbolic(
robot, {cabinet: "in_front_of"}, cabinet),
transitions={
'arrived': 'INSPECT_SHELVES',
'unreachable': 'FORCE_ROTATE',
'goal_not_defined': 'INSPECT_SHELVES'
})
smach.StateMachine.add("FORCE_ROTATE",
ForceRotate(robot, 0.5, 2.0, 30.0),
transitions={
'done': "NAV_TO_START",
'timedout': "INSPECT_SHELVES"
})
# smach.StateMachine.add("RESET_ED",
# states.ResetED(robot),
# transitions={'done' :'INSPECT_SHELVES'})
smach.StateMachine.add("INSPECT_SHELVES",
InspectShelves(robot, OBJECT_SHELVES),
transitions={
'succeeded': 'EXPORT_PDF',
'nothing_found': 'EXPORT_PDF',
'failed': 'EXPORT_PDF'
})
@smach.cb_interface(outcomes=["exported"])
def export_to_pdf(userdata=None):
global DETECTED_OBJECTS_WITH_PROBS
entities = [e[0] for e in DETECTED_OBJECTS_WITH_PROBS]
# Export images (Only best MAX_NUM_ENTITIES_IN_PDF)
# pdf.entities_to_pdf(robot.ed, entities[:MAX_NUM_ENTITIES_IN_PDF], "tech_united_manipulation_challenge")
return "exported"
smach.StateMachine.add('EXPORT_PDF',
smach.CBState(export_to_pdf),
transitions={'exported': 'RANGE_ITERATOR'})
# Begin setup iterator
range_iterator = smach.Iterator(
outcomes=['succeeded', 'failed'], #Outcomes of the iterator state
input_keys=[],
output_keys=[],
it=lambda: range(5),
it_label='index',
exhausted_outcome='succeeded'
) #The exhausted argument should be set to the preffered state machine outcome
with range_iterator:
single_item = ManipRecogSingleItem(
robot,
ds.VariableDesignator(placed_items, [Entity],
name="placed_items"))
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'})
ds.analyse_designators(sm, "manipulation")
return sm
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# start_waypoint = ds.EntityByIdDesignator(robot, id="manipulation_init_pose", name="start_waypoint")
pdf_writer = WritePdf(robot=robot)
with self:
single_item = ManipulateMachine(robot, pdf_writer=pdf_writer)
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized':
'SAY_UNABLE_TO_OPEN_DOOR',
'abort': 'Aborted'
})
smach.StateMachine.add('SAY_UNABLE_TO_OPEN_DOOR',
states.Say(
robot,
"I am unable to open the shelf door, "
"can you please open it for me?"),
transitions={'spoken': 'AWAIT_START'})
smach.StateMachine.add("AWAIT_START",
states.AskContinue(robot),
transitions={
'continue': "MOVE_TABLE",
'no_response': 'AWAIT_START'
})
cabinet = ds.EntityByIdDesignator(robot, id=CABINET)
@smach.cb_interface(outcomes=["done"])
def move_table(userdata=None, manipulate_machine=None):
""" Moves the entities for this challenge to the correct poses"""
# Determine where to perform the challenge
# Apparently, the indices here come from:
# (cabinet_pose, table_pose, cabinet_amcl, grasp_surface, room, default_place_area)
robot_pose = robot.base.get_location()
WORKSPACES.sort(
key=lambda ws: (ws.place_entity_conf.pose_estimate.frame.p
- robot_pose.frame.p).Norm())
closest_workspace = WORKSPACES[0]
rospy.loginfo(
"Closest workspace: grasp from '{grasp}' and place on '{place}'"
.format(
grasp=closest_workspace.grasp_entity_conf.entity_id,
place=closest_workspace.place_entity_conf.entity_id))
cabinet_id = closest_workspace.place_entity_conf.entity_id
table_id = closest_workspace.grasp_entity_conf.entity_id
# Update the world model by fitting the entities to the frame_stamped's given below.
robot.ed.update_entity(id=cabinet_id,
frame_stamped=closest_workspace.
place_entity_conf.pose_estimate)
robot.ed.update_entity(id=table_id,
frame_stamped=closest_workspace.
grasp_entity_conf.pose_estimate)
# Update designators
cabinet.id_ = closest_workspace.place_entity_conf.entity_id
# Update manipulate machine
manipulate_machine.table_designator.id_ = closest_workspace.grasp_entity_conf.entity_id
manipulate_machine.place_entity_designator.id_ = closest_workspace.place_entity_conf.entity_id
manipulate_machine.place_designator._area = closest_workspace.place_entity_conf.manipulation_volumes[
0]
manipulate_machine.place_designator.place_location_designator.id = closest_workspace.place_entity_conf.entity_id
manipulate_machine.cabinet.id_ = closest_workspace.place_entity_conf.entity_id
return "done"
smach.StateMachine.add("MOVE_TABLE",
smach.CBState(move_table,
cb_args=[single_item]),
transitions={'done': 'NAV_TO_START'})
smach.StateMachine.add("NAV_TO_START",
states.NavigateToSymbolic(
robot, {cabinet: "in_front_of"},
cabinet),
transitions={
'arrived': 'INSPECT_SHELVES',
'unreachable': 'INSPECT_SHELVES',
'goal_not_defined': 'INSPECT_SHELVES'
})
smach.StateMachine.add("INSPECT_SHELVES",
InspectShelves(robot, cabinet),
transitions={
'succeeded': 'WRITE_PDF_SHELVES',
'nothing_found': 'WRITE_PDF_SHELVES',
'failed': 'WRITE_PDF_SHELVES'
})
smach.StateMachine.add("WRITE_PDF_SHELVES",
pdf_writer,
transitions={"done": "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(5),
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'})
ds.analyse_designators(self, "manipulation")
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'])
with self:
smach.StateMachine.add('INITIALIZE',
Initialize(robot),
transitions={
'initialized': 'ANNOUNCEMENT',
'abort': 'Aborted'
})
smach.StateMachine.add("ANNOUNCEMENT",
Say(robot, "I want to play a riddle game"),
transitions={"spoken": "WAIT"})
smach.StateMachine.add("WAIT",
WaitTime(robot),
transitions={
"waited": "TURN",
"preempted": "TURN"
})
smach.StateMachine.add("TURN",
Turn(robot, math.pi),
transitions={"turned": "DETECT_CROWD"})
smach.StateMachine.add("DETECT_CROWD",
detect.DetectCrowd(robot),
transitions={
"succeeded": "REQUEST_OPERATOR",
"failed": "REQUEST_OPERATOR"
},
remapping={'crowd_data': 'crowd_data'})
smach.StateMachine.add("REQUEST_OPERATOR",
Say(robot,
"Who wants to play riddles with me?"),
transitions={"spoken": "WAIT_FOR_OPERATOR"})
smach.StateMachine.add("WAIT_FOR_OPERATOR",
WaitForPersonInFront(robot,
attempts=3,
sleep_interval=1),
transitions={
'success': 'RIDDLE_GAME',
'failed': 'RIDDLE_GAME'
})
# Riddle Game
smach.StateMachine.add('RIDDLE_GAME',
riddle_game.HearAndAnswerQuestions(
robot, num_questions=5),
transitions={'done': 'TRANSITION'})
# Transition:
smach.StateMachine.add(
"TRANSITION",
Say(robot, "Now lets play the blind mans bluff game"),
transitions={"spoken": "BLUFF_GAME"})
# Bluff Games:
smach.StateMachine.add(
'BLUFF_GAME',
bluff_game.HearTurnAndAnswerQuestions(robot,
num_questions=5,
num_operators=5),
transitions={'done': 'END_CHALLENGE'})
# End
smach.StateMachine.add('END_CHALLENGE',
Say(robot,
"My work here is done, goodbye!"),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "person_recognition")
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 __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# -----------------------------------------------------------------
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'INSTRUCT_WAIT_FOR_DOOR',
'abort': 'Aborted'
})
smach.StateMachine.add(
"INSTRUCT_WAIT_FOR_DOOR",
states.Say(robot, [
"Hi there, I will now wait until you remove the cup",
"I'm waiting for you to remove the cup"
],
block=False),
transitions={"spoken": "WAIT_FOR_DOOR"})
smach.StateMachine.add("WAIT_FOR_DOOR",
states.WaitForDoorOpen(robot, timeout=10),
transitions={
"closed": "DOOR_CLOSED",
"open": "AWAIT_START"
})
smach.StateMachine.add(
"DOOR_CLOSED",
states.Say(robot, [
"I am waiting for you to remove the cup",
"I'd start, if you remove the cup from my laser"
]),
transitions={"spoken": "WAIT_FOR_DOOR"})
smach.StateMachine.add("AWAIT_START",
states.AskContinue(robot),
transitions={
'continue': 'LEARN_OPERATOR_FACE',
'no_response': 'AWAIT_START'
})
smach.StateMachine.add('LEARN_OPERATOR_FACE',
LearnOperatorFace(robot),
transitions={
'succeeded':
'WAIT_FOR_OPERATOR_TO_JOIN',
'failed': 'LEARN_OPERATOR_FACE'
})
@smach.cb_interface(outcomes=['done'])
def wait_a_sec(userdata=None):
robot.speech.speak(
"I will wait for 10 seconds for you to join the crowd",
block=True)
start = rospy.Time.now()
stop = rospy.Duration(10) + start
last_spoken = start
while rospy.Time.now() < stop:
if (rospy.Time.now() - last_spoken).to_sec() > 1.0:
robot.speech.speak("%d" %
(stop - rospy.Time.now()).to_sec())
last_spoken = rospy.Time.now()
return 'done'
smach.StateMachine.add('WAIT_FOR_OPERATOR_TO_JOIN',
smach.CBState(wait_a_sec),
transitions={'done': 'FORCE_DRIVE'})
@smach.cb_interface(outcomes=['done'])
def force_drive(userdata=None):
vth = 0.5
th = 3.1415
robot.head.cancel_goal()
robot.base.force_drive(0, 0, vth, th / vth)
return 'done'
smach.StateMachine.add('FORCE_DRIVE',
smach.CBState(force_drive),
transitions={'done': 'DETECT'})
smach.StateMachine.add('DETECT',
Detect(robot),
transitions={
'succeeded': 'END_CHALLENGE',
'failed': 'DETECT'
})
smach.StateMachine.add('END_CHALLENGE',
states.Say(
robot,
"My work here is done, goodbye!"),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "person_recognition")
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done','Aborted'])
with self:
smach.StateMachine.add('INITIALIZE',
Initialize(robot),
transitions={'initialized': 'ANNOUNCEMENT',
'abort': 'Aborted'})
smach.StateMachine.add("ANNOUNCEMENT",
Say(robot, "I want to play a riddle game"),
transitions={"spoken": "WAIT"})
smach.StateMachine.add("WAIT",
WaitTime(robot),
transitions={"waited": "TURN",
"preempted": "TURN"})
smach.StateMachine.add("TURN",
Turn(robot, math.pi),
transitions={"turned": "DETECT_CROWD"})
smach.StateMachine.add("DETECT_CROWD",
detect.DetectCrowd(robot),
transitions={"succeeded": "REQUEST_OPERATOR",
"failed": "REQUEST_OPERATOR"},
remapping={'crowd_data': 'crowd_data'})
smach.StateMachine.add("REQUEST_OPERATOR",
Say(robot, "Who wants to play riddles with me?"),
transitions={"spoken": "WAIT_FOR_OPERATOR"})
smach.StateMachine.add("WAIT_FOR_OPERATOR",
WaitForPersonInFront(robot, attempts=3, sleep_interval=1),
transitions={'success': 'RIDDLE_GAME',
'failed': 'RIDDLE_GAME'})
# Riddle Game
smach.StateMachine.add('RIDDLE_GAME',
riddle_game.HearAndAnswerQuestions(robot, num_questions=5),
transitions={'done':'TRANSITION'})
# Transition:
smach.StateMachine.add("TRANSITION",
Say(robot, "Now lets play the blind mans bluff game"),
transitions={"spoken": "BLUFF_GAME"})
# Bluff Games:
smach.StateMachine.add('BLUFF_GAME',
bluff_game.HearTurnAndAnswerQuestions(robot, num_questions=5, num_operators=5),
transitions={'done': 'END_CHALLENGE'})
# End
smach.StateMachine.add('END_CHALLENGE',
Say(robot, "My work here is done, goodbye!"),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "person_recognition")
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done','Aborted'])
with self:
smach.StateMachine.add('INITIALIZE',
Initialize(robot),
transitions={'initialized': 'ANNOUNCEMENT',
'abort': 'Aborted'})
smach.StateMachine.add("ANNOUNCEMENT",
Say(robot, "I want to play a riddle game"),
transitions={"spoken": "WAIT"})
smach.StateMachine.add("WAIT",
WaitTime(robot),
transitions={"waited": "TURN",
"preempted": "TURN"})
smach.StateMachine.add("TURN",
Turn(robot, math.pi),
transitions={"turned": "DETECT_CROWD"})
smach.StateMachine.add("DETECT_CROWD",
detect.DetectCrowd(robot),
transitions={"succeeded": "REQUEST_OPERATOR",
"failed": "REQUEST_OPERATOR"},
remapping={'crowd_data': 'crowd_data'})
smach.StateMachine.add("REQUEST_OPERATOR",
Say(robot, "Who wants to play riddles with me?"),
transitions={"spoken": "WAIT_FOR_OPERATOR"})
smach.StateMachine.add("WAIT_FOR_OPERATOR",
WaitForPersonInFront(robot, attempts=3, sleep_interval=1),
transitions={'success': 'RIDDLE_GAME_1',
'failed': 'RIDDLE_GAME_1'})
# Riddle Game
smach.StateMachine.add('RIDDLE_GAME_1',
riddle_game.HearQuestion(robot),
transitions={ 'answered':'RIDDLE_GAME_2',
'not_answered':'RIDDLE_GAME_1_MISSED'})
smach.StateMachine.add("RIDDLE_GAME_1_MISSED",
Say(robot, "Please ask next question!"),
transitions={"spoken": "RIDDLE_GAME_2"})
smach.StateMachine.add('RIDDLE_GAME_2',
riddle_game.HearQuestion(robot),
transitions={ 'answered':'RIDDLE_GAME_3',
'not_answered':'RIDDLE_GAME_2_MISSED'})
smach.StateMachine.add("RIDDLE_GAME_2_MISSED",
Say(robot, "Please ask next question!"),
transitions={"spoken": "RIDDLE_GAME_3"})
smach.StateMachine.add('RIDDLE_GAME_3',
riddle_game.HearQuestion(robot),
transitions={ 'answered':'RIDDLE_GAME_4',
'not_answered':'RIDDLE_GAME_3_MISSED'})
smach.StateMachine.add("RIDDLE_GAME_3_MISSED",
Say(robot, "Please ask next question!"),
transitions={"spoken": "RIDDLE_GAME_4"})
smach.StateMachine.add('RIDDLE_GAME_4',
riddle_game.HearQuestion(robot),
transitions={ 'answered':'RIDDLE_GAME_5',
'not_answered':'RIDDLE_GAME_4_MISSED'})
smach.StateMachine.add("RIDDLE_GAME_4_MISSED",
Say(robot, "Please ask next question!"),
transitions={"spoken": "RIDDLE_GAME_5"})
smach.StateMachine.add('RIDDLE_GAME_5',
riddle_game.HearQuestion(robot),
transitions={ 'answered':'TRANSITION',
'not_answered':'TRANSITION'})
# Transition:
smach.StateMachine.add("TRANSITION",
Say(robot, "Now lets play the blind mans bluff game"),
transitions={"spoken": "BLUFF_GAME_1"})
# Bluff Games:
# 1
smach.StateMachine.add('BLUFF_GAME_1',
bluff_game.HearQuestion(robot),
transitions={'answered': 'BLUFF_GAME_2',
'not_answered': 'BLUFF_GAME_1_ASK_REPEAT'})
smach.StateMachine.add("BLUFF_GAME_1_ASK_REPEAT",
Say(robot, "Could you please repeat your question?"),
transitions={"spoken": "BLUFF_GAME_1_REPEAT"})
smach.StateMachine.add('BLUFF_GAME_1_REPEAT',
bluff_game.HearQuestionRepeat(robot),
transitions={'answered' :'BLUFF_GAME_2',
'not_answered': 'BLUFF_GAME_2'})
# 2
smach.StateMachine.add('BLUFF_GAME_2',
bluff_game.HearQuestion(robot),
transitions={'answered': 'BLUFF_GAME_3',
'not_answered': 'BLUFF_GAME_2_ASK_REPEAT'})
smach.StateMachine.add("BLUFF_GAME_2_ASK_REPEAT",
Say(robot, "Could you please repeat your question?"),
transitions={"spoken": "BLUFF_GAME_2_REPEAT"})
smach.StateMachine.add('BLUFF_GAME_2_REPEAT',
bluff_game.HearQuestionRepeat(robot),
transitions={'answered' :'BLUFF_GAME_3',
'not_answered': 'BLUFF_GAME_3'})
# 3
smach.StateMachine.add('BLUFF_GAME_3',
bluff_game.HearQuestion(robot),
transitions={'answered': 'BLUFF_GAME_4',
'not_answered': 'BLUFF_GAME_3_ASK_REPEAT'})
smach.StateMachine.add("BLUFF_GAME_3_ASK_REPEAT",
Say(robot, "Could you please repeat your question?"),
transitions={"spoken": "BLUFF_GAME_3_REPEAT"})
smach.StateMachine.add('BLUFF_GAME_3_REPEAT',
bluff_game.HearQuestionRepeat(robot),
transitions={'answered' :'BLUFF_GAME_4',
'not_answered': 'BLUFF_GAME_4'})
# 4
smach.StateMachine.add('BLUFF_GAME_4',
bluff_game.HearQuestion(robot),
transitions={'answered': 'BLUFF_GAME_5',
'not_answered': 'BLUFF_GAME_4_ASK_REPEAT'})
smach.StateMachine.add("BLUFF_GAME_4_ASK_REPEAT",
Say(robot, "Could you please repeat your question?"),
transitions={"spoken": "BLUFF_GAME_4_REPEAT"})
smach.StateMachine.add('BLUFF_GAME_4_REPEAT',
bluff_game.HearQuestionRepeat(robot),
transitions={'answered' :'BLUFF_GAME_5',
'not_answered': 'BLUFF_GAME_5'})
# 5
smach.StateMachine.add('BLUFF_GAME_5',
bluff_game.HearQuestion(robot),
transitions={'answered': 'BLUFF_GAME_INF',
'not_answered': 'BLUFF_GAME_5_ASK_REPEAT'})
smach.StateMachine.add("BLUFF_GAME_5_ASK_REPEAT",
Say(robot, "Could you please repeat your question?"),
transitions={"spoken": "BLUFF_GAME_5_REPEAT"})
smach.StateMachine.add('BLUFF_GAME_5_REPEAT',
bluff_game.HearQuestionRepeat(robot),
transitions={'answered': 'BLUFF_GAME_INF',
'not_answered': 'BLUFF_GAME_INF'})
# 6
smach.StateMachine.add('BLUFF_GAME_INF',
bluff_game.HearQuestion(robot),
transitions={'answered': 'BLUFF_GAME_INF',
'not_answered': 'BLUFF_GAME_INF_ASK_REPEAT'})
smach.StateMachine.add("BLUFF_GAME_INF_ASK_REPEAT",
Say(robot, "Could you please repeat your question?"),
transitions={"spoken": "BLUFF_GAME_INF_REPEAT"})
smach.StateMachine.add('BLUFF_GAME_INF_REPEAT',
bluff_game.HearQuestionRepeat(robot),
transitions={'answered' :'BLUFF_GAME_INF',
'not_answered': 'BLUFF_GAME_INF'})
# End
smach.StateMachine.add('END_CHALLENGE',
Say(robot, "My work here is done, goodbye!"),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "person_recognition")
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['Done','Aborted'])
with sm:
# Start challenge via StartChallengeRobust
smach.StateMachine.add( "START_CHALLENGE_ROBUST",
states.Initialize(robot),
transitions={ 'initialized' : "SAY_INTRO",
"abort" : "Aborted"})
smach.StateMachine.add('SAY_INTRO', states.Say(robot, "Please come a little bit closer to me, talk loudly and, very important, wait for the ping!"), transitions={ 'spoken' :'SAY_1'})
smach.StateMachine.add('SAY_1', states.Say(robot, "Please ask me question one"), transitions={ 'spoken' :'QUESTION_1'})
smach.StateMachine.add('QUESTION_1', direct_speech_recognition.HearQuestion(robot), transitions={ 'answered' :'SAY_2'})
smach.StateMachine.add('SAY_2', states.Say(robot, "Please ask me question two"), transitions={ 'spoken' :'QUESTION_2'})
smach.StateMachine.add('QUESTION_2', direct_speech_recognition.HearQuestion(robot), transitions={ 'answered' :'SAY_3'})
smach.StateMachine.add('SAY_3', states.Say(robot, "Please ask me question three"), transitions={ 'spoken' :'QUESTION_3'})
smach.StateMachine.add('QUESTION_3', direct_speech_recognition.HearQuestion(robot), transitions={ 'answered' :'SAY_4'})
smach.StateMachine.add('SAY_4', states.Say(robot, "Please ask me question four"), transitions={ 'spoken' :'QUESTION_4'})
smach.StateMachine.add('QUESTION_4', direct_speech_recognition.HearQuestion(robot), transitions={ 'answered' :'SAY_5'})
smach.StateMachine.add('SAY_5', states.Say(robot, "Please ask me question five"), transitions={ 'spoken' :'QUESTION_5'})
smach.StateMachine.add('QUESTION_5', direct_speech_recognition.HearQuestion(robot), transitions={ 'answered' :'TRANSITION'})
smach.StateMachine.add('TRANSITION', states.Say(robot, "Let's proceed to round two! yeah!"), transitions={ 'spoken' :'2SAY_1'})
smach.StateMachine.add('2SAY_1', states.Say(robot, "Please ask me question one"), transitions={ 'spoken' :'2QUESTION_1'})
smach.StateMachine.add('2QUESTION_1', indirect_speech_recognition.HearQuestion(robot), transitions={ 'answered' :'2TURN_1', 'not_answered': '2SAY_2'})
smach.StateMachine.add('2TURN_1', indirect_speech_recognition.Turn(robot), transitions={ 'turned' :'2SAY_2'})
smach.StateMachine.add('2SAY_1A', states.Say(robot, "Please repeat your question"), transitions={ 'spoken' :'2QUESTION_1A'})
smach.StateMachine.add('2QUESTION_1A', indirect_speech_recognition.HearQuestion(robot), transitions={ 'answered' :'2SAY_2', 'not_answered': '2SAY_2'})
smach.StateMachine.add('2SAY_2', states.Say(robot, "Please ask me question two"), transitions={ 'spoken' :'2QUESTION_2'})
smach.StateMachine.add('2QUESTION_2', indirect_speech_recognition.HearQuestion(robot), transitions={ 'answered' :'2TURN_2', 'not_answered': '2SAY_3'})
smach.StateMachine.add('2TURN_2', indirect_speech_recognition.Turn(robot), transitions={ 'turned' :'2SAY_3'})
smach.StateMachine.add('2SAY_2A', states.Say(robot, "Please repeat your question"), transitions={ 'spoken' :'2QUESTION_2A'})
smach.StateMachine.add('2QUESTION_2A', indirect_speech_recognition.HearQuestion(robot), transitions={ 'answered' :'2SAY_3', 'not_answered': '2SAY_3'})
smach.StateMachine.add('2SAY_3', states.Say(robot, "Please ask me question three"), transitions={ 'spoken' :'2QUESTION_3'})
smach.StateMachine.add('2QUESTION_3', indirect_speech_recognition.HearQuestion(robot), transitions={ 'answered' :'2TURN_3', 'not_answered': '2SAY_4'})
smach.StateMachine.add('2TURN_3', indirect_speech_recognition.Turn(robot), transitions={ 'turned' :'2SAY_4'})
smach.StateMachine.add('2SAY_3A', states.Say(robot, "Please repeat your question"), transitions={ 'spoken' :'2QUESTION_3A'})
smach.StateMachine.add('2QUESTION_3A', indirect_speech_recognition.HearQuestion(robot), transitions={ 'answered' :'2SAY_4', 'not_answered': '2SAY_4'})
smach.StateMachine.add('2SAY_4', states.Say(robot, "Please ask me question four"), transitions={ 'spoken' :'2QUESTION_4'})
smach.StateMachine.add('2QUESTION_4', indirect_speech_recognition.HearQuestion(robot), transitions={ 'answered' :'2TURN_4', 'not_answered': '2SAY_5'})
smach.StateMachine.add('2TURN_4', indirect_speech_recognition.Turn(robot), transitions={ 'turned' :'2SAY_5'})
smach.StateMachine.add('2SAY_4A', states.Say(robot, "Please repeat your question"), transitions={ 'spoken' :'2QUESTION_4A'})
smach.StateMachine.add('2QUESTION_4A', indirect_speech_recognition.HearQuestion(robot), transitions={ 'answered' :'2SAY_5', 'not_answered': '2SAY_5'})
smach.StateMachine.add('2SAY_5', states.Say(robot, "Please ask me question five"), transitions={ 'spoken' :'2QUESTION_5'})
smach.StateMachine.add('2QUESTION_5', indirect_speech_recognition.HearQuestion(robot), transitions={ 'answered' :'2TURN_5', 'not_answered': 'AT_END'})
smach.StateMachine.add('2TURN_5', indirect_speech_recognition.Turn(robot), transitions={ 'turned' :'AT_END'})
smach.StateMachine.add('2SAY_5A', states.Say(robot, "Please repeat your question"), transitions={ 'spoken' :'2QUESTION_5A'})
smach.StateMachine.add('2QUESTION_5A', indirect_speech_recognition.HearQuestion(robot), transitions={ 'answered' :'AT_END', 'not_answered': 'AT_END'})
smach.StateMachine.add('AT_END', states.Say(robot, "That was all folks!"), transitions={ 'spoken' :'Done'})
analyse_designators(sm, "speech_recognition")
return sm
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# start_waypoint = ds.EntityByIdDesignator(robot, id="manipulation_init_pose", name="start_waypoint")
pdf_writer = WritePdf(robot=robot)
skip_door = rospy.get_param("~skip_door", False)
with self:
single_item = ManipulateMachine(robot, pdf_writer=pdf_writer)
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={'initialized': 'AWAIT_START',
'abort': 'Aborted'})
smach.StateMachine.add("AWAIT_START",
states.AskContinue(robot),
transitions={'continue': "MOVE_TABLE",
'no_response': 'AWAIT_START'})
cabinet = ds.EntityByIdDesignator(robot, id=CABINET)
open_door = OpenDoorMachine(robot, 'temp', 'in_front_of', 'shelf6') # cabinet_id is overwritten by 'move_table' below
@smach.cb_interface(outcomes=["done"])
def move_table(userdata=None, manipulate_machine=None):
""" Moves the entities for this challenge to the correct poses"""
# Determine where to perform the challenge
# Apparently, the indices here come from:
# (cabinet_pose, table_pose, cabinet_amcl, grasp_surface, room, default_place_area)
robot_pose = robot.base.get_location()
WORKSPACES.sort(key=lambda ws: (ws.place_entity_conf.pose_estimate.frame.p - robot_pose.frame.p).Norm())
closest_workspace = WORKSPACES[0]
rospy.loginfo("Closest workspace: grasp from '{grasp}' and place on '{place}'".format(grasp=closest_workspace.grasp_entity_conf.entity_id,
place=closest_workspace.place_entity_conf.entity_id))
cabinet_id = closest_workspace.place_entity_conf.entity_id
table_id = closest_workspace.grasp_entity_conf.entity_id
# Update the world model by fitting the entities to the frame_stamped's given below.
robot.ed.update_entity(id=cabinet_id, frame_stamped=closest_workspace.place_entity_conf.pose_estimate)
robot.ed.update_entity(id=table_id, frame_stamped=closest_workspace.grasp_entity_conf.pose_estimate)
# Update designators
cabinet.id_ = closest_workspace.place_entity_conf.entity_id
# Update manipulate machine
manipulate_machine.table_designator.id_ = closest_workspace.grasp_entity_conf.entity_id
manipulate_machine.place_entity_designator.id_ = closest_workspace.place_entity_conf.entity_id
manipulate_machine.place_designator._area = closest_workspace.place_entity_conf.manipulation_volumes[0]
manipulate_machine.place_designator.place_location_designator.id = closest_workspace.place_entity_conf.entity_id
manipulate_machine.cabinet.id_ = closest_workspace.place_entity_conf.entity_id
open_door.cabinet.id_ = closest_workspace.place_entity_conf.entity_id
return "done"
smach.StateMachine.add("MOVE_TABLE",
smach.CBState(move_table, cb_args=[single_item]),
transitions={'done': 'OPEN_DOOR' if not skip_door else "SAY_UNABLE_TO_OPEN_DOOR"})
smach.StateMachine.add("OPEN_DOOR",
open_door,
transitions={'succeeded': 'RANGE_ITERATOR',
'failed': 'SAY_UNABLE_TO_OPEN_DOOR'})
smach.StateMachine.add('SAY_UNABLE_TO_OPEN_DOOR',
states.Say(robot, "I am unable to open the shelf door, "
"can you please open it for me?"),
transitions={'spoken': 'RANGE_ITERATOR'})
# If you want to reinstate cabinet inspection uncomment section below and change transition above
# smach.StateMachine.add("NAV_TO_START",
# states.NavigateToSymbolic(robot,
# {cabinet: "in_front_of"},
# cabinet),
# transitions={'arrived': 'INSPECT_SHELVES',
# 'unreachable': 'INSPECT_SHELVES',
# 'goal_not_defined': 'INSPECT_SHELVES'})
#
# smach.StateMachine.add("INSPECT_SHELVES",
# InspectShelves(robot, cabinet),
# transitions={'succeeded': 'WRITE_PDF_SHELVES',
# 'nothing_found': 'WRITE_PDF_SHELVES',
# 'failed': 'WRITE_PDF_SHELVES'})
#
# smach.StateMachine.add("WRITE_PDF_SHELVES", pdf_writer, transitions={"done": "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(5),
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'})
ds.analyse_designators(self, "manipulation")
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 __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, 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, rospy.Duration(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):
smach.StateMachine.__init__(self, outcomes=['Done','Aborted'])
# -----------------------------------------------------------------
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={'initialized': 'INSTRUCT_WAIT_FOR_DOOR',
'abort': 'Aborted'})
smach.StateMachine.add("INSTRUCT_WAIT_FOR_DOOR",
states.Say(robot, ["Hi there, I will now wait until you remove the cup",
"I'm waiting for you to remove the cup"], block=False),
transitions={"spoken": "WAIT_FOR_DOOR"})
smach.StateMachine.add("WAIT_FOR_DOOR",
states.WaitForDoorOpen(robot, timeout=10),
transitions={"closed": "DOOR_CLOSED",
"open": "AWAIT_START"})
smach.StateMachine.add("DOOR_CLOSED",
states.Say(robot, ["I am waiting for you to remove the cup",
"I'd start, if you remove the cup from my laser"]),
transitions={"spoken": "WAIT_FOR_DOOR"})
smach.StateMachine.add("AWAIT_START",
states.AskContinue(robot),
transitions={'continue': 'LEARN_OPERATOR_FACE',
'no_response': 'AWAIT_START'})
smach.StateMachine.add( 'LEARN_OPERATOR_FACE',
LearnOperatorFace(robot),
transitions={'succeeded': 'WAIT_FOR_OPERATOR_TO_JOIN',
'failed': 'LEARN_OPERATOR_FACE'})
@smach.cb_interface(outcomes=['done'])
def wait_a_sec(userdata=None):
robot.speech.speak("I will wait for 10 seconds for you to join the crowd", block=True)
start = rospy.Time.now()
stop = rospy.Duration(10) + start
last_spoken = start
while rospy.Time.now() < stop:
if (rospy.Time.now() - last_spoken).to_sec() > 1.0:
robot.speech.speak("%d" % (stop - rospy.Time.now()).to_sec())
last_spoken = rospy.Time.now()
return 'done'
smach.StateMachine.add('WAIT_FOR_OPERATOR_TO_JOIN',
smach.CBState(wait_a_sec),
transitions={'done': 'FORCE_DRIVE'})
@smach.cb_interface(outcomes=['done'])
def force_drive(userdata=None):
vth = 0.5
th = 3.1415
robot.head.cancel_goal()
robot.base.force_drive(0, 0, vth, th / vth)
return 'done'
smach.StateMachine.add('FORCE_DRIVE',
smach.CBState(force_drive),
transitions={'done': 'DETECT'})
smach.StateMachine.add( 'DETECT',
Detect(robot),
transitions={'succeeded': 'END_CHALLENGE',
'failed': 'DETECT'})
smach.StateMachine.add('END_CHALLENGE',
states.Say(robot, "My work here is done, goodbye!"),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "person_recognition")
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
self.target_destination = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.default_place)
self.car_waypoint = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.waypoint_car['id'])
self.place_position = ds.LockingDesignator(ds.EmptySpotDesignator(
robot,
self.target_destination,
name="placement",
area=challenge_knowledge.default_area),
name="place_position")
self.empty_arm_designator = ds.UnoccupiedArmDesignator(
robot, {}, 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()
# 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': 'FOLLOW_OPERATOR'})
# Use NEW:
smach.StateMachine.add('FOLLOW_OPERATOR',
states.FollowOperator2(robot),
transitions={
'Done': 'ASK_FOR_TASK',
'Failed': 'ASK_FOR_TASK',
'Aborted': 'FOLLOW_OPERATOR'
})
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',
states.HearOptions(robot, ['yes', 'no']),
transitions={
'no': 'FOLLOW_OPERATOR',
'yes': 'CONFIRM_CAR_LOCATION',
'no_result': 'ASK_FOR_TASK'
})
smach.StateMachine.add(
'CONFIRM_CAR_LOCATION',
states.Say(
robot,
["OK, I will remember this location as the car location."],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'REMEMBER_CAR_LOCATION'})
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'
})
#
# smach.StateMachine.add('CONFIRM_DESTINATION',
# states.Say(robot, [
# "I will deliver the groceries to the %s" % ds.EntityByIdDesignator(self.target_destination)],
# block=True,
# look_at_standing_person=True),
# transitions={'spoken': 'GRAB_ITEM'})
# 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'})
states.Say(robot, [
"I can't pick up the groceries since I don't have arms. Please place them in my basket."
],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'WAIT_FOR_GRAB_ITEM'})
smach.StateMachine.add('WAIT_FOR_GRAB_ITEM',
states.WaitTime(robot),
transitions={
'waited': 'SAY_GOING_TO_ROOM',
'preempted': 'Aborted'
})
# 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.NavigateToSymbolic(
robot, {self.target_destination: "in_front_of"},
self.target_destination),
transitions={
'arrived': 'PUTDOWN_ITEM',
'unreachable': 'TURN_180_TO_REPLAN',
# implement avoid obstacle behaviour later
'goal_not_defined': 'Aborted'
})
smach.StateMachine.add(
'TURN_180_TO_REPLAN',
hmc_states.TurnToReplan(robot),
transitions={
'success': 'GOTO_DESTINATION_BACKUP',
'abort': 'GOTO_DESTINATION_BACKUP',
# implement avoid obstacle behaviour later
#'goal_not_defined': 'Aborted'})
})
smach.StateMachine.add(
'GOTO_DESTINATION_BACKUP',
states.NavigateToSymbolic(
robot, {self.target_destination: "in_front_of"},
self.target_destination),
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),
states.Say(robot, [
"I can't put the groceries down since I have no arms. Please take them from my basket and put it down."
],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'WAIT_FOR_PUTDOWN_ITEM'})
smach.StateMachine.add('WAIT_FOR_PUTDOWN_ITEM',
states.WaitTime(robot),
transitions={
'waited': 'ASKING_FOR_HELP',
'preempted': 'Aborted'
})
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'}) #'LEARN_OPERATOR'})
# smach.StateMachine.add('LEARN_OPERATOR',
# hmc_states.LearnOperator(robot),
# transitions={'learned': 'GOTO_CAR',
# 'failed': '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 __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# start_waypoint = ds.EntityByIdDesignator(robot, id="manipulation_init_pose", name="start_waypoint")
pdf_writer = WritePdf(robot=robot)
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={'initialized': 'AWAIT_START',
'abort': 'Aborted'})
smach.StateMachine.add("AWAIT_START",
states.AskContinue(robot),
transitions={'continue': "MOVE_TABLE",
'no_response': 'AWAIT_START'})
@smach.cb_interface(outcomes=["done"])
def move_table(userdata=None):
""" 'Locks' a locking designator """
# For now, don't do anything
return "done"
# Move away the cabinet
robot.ed.update_entity(id="cabinet",
frame_stamped=FrameStamped(frame=kdl.Frame(kdl.Rotation(),
kdl.Vector(12.0, 0, 0)),
frame_id="map"))
# Determine where to perform the challenge
robot_pose = robot.base.get_location()
ENTITY_POSES.sort(key=lambda tup: (tup[0].frame.p - robot_pose.frame.p).Norm())
# Update the world model
robot.ed.update_entity(id=CABINET, frame_stamped=ENTITY_POSES[0][0])
robot.ed.update_entity(id=TABLE, frame_stamped=ENTITY_POSES[0][1])
return "done"
smach.StateMachine.add("MOVE_TABLE",
smach.CBState(move_table),
transitions={'done': 'NAV_TO_START'})
cabinet = ds.EntityByIdDesignator(robot, id=CABINET)
room = ds.EntityByIdDesignator(robot, id=ROOM)
smach.StateMachine.add("NAV_TO_START",
states.NavigateToSymbolic(robot,
{cabinet: "in_front_of"},
cabinet),
transitions={'arrived': 'INSPECT_SHELVES',
'unreachable': 'INSPECT_SHELVES',
'goal_not_defined': 'INSPECT_SHELVES'})
smach.StateMachine.add("INSPECT_SHELVES",
InspectShelves(robot),
transitions={'succeeded': 'WRITE_PDF_SHELVES',
'nothing_found': 'WRITE_PDF_SHELVES',
'failed': 'WRITE_PDF_SHELVES'})
smach.StateMachine.add("WRITE_PDF_SHELVES", pdf_writer, transitions={"done": "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(5),
it_label='index',
exhausted_outcome='succeeded')
with range_iterator:
single_item = ManipulateMachine(robot, pdf_writer=pdf_writer) # ToDo: add more pdf stuff
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'})
ds.analyse_designators(self, "manipulation")
