def __init__(self, robot, order_type):
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"])
beverage_dest_desig = EdEntityDesignator(robot) #.id is overwritten by instruct_barman
with self:
@smach.cb_interface(outcomes=['spoken'])
def instruct_barman(userdata=None):
try:
order = ORDERS[order_type]
beverage_dest_desig.id = order['location']
robot.speech.speak("Barman, please put a {name} in my basket for table {location}".format(**order))
except KeyError:
rospy.logerr("No beverage in ORDERS")
return 'spoken'
smach.StateMachine.add( 'INSTRUCT_BARMAN',
smach.CBState(instruct_barman),
transitions={'spoken' :'AWAIT_PUT_ORDER_CONFIRMATION'})
smach.StateMachine.add( 'AWAIT_PUT_ORDER_CONFIRMATION',
states.WaitTime(robot, 8),
transitions={ 'waited' :'GOTO_ORDER_DESTINATION_1',
'preempted' :'failed'})
smach.StateMachine.add( 'GOTO_ORDER_DESTINATION_1', states.NavigateToWaypoint(robot, beverage_dest_desig, radius = WAYPOINT_RADIUS),
transitions={ 'arrived' :'SAY_TAKE_ORDER',
'unreachable' :'GOTO_ORDER_DESTINATION_2',
'goal_not_defined' :'GOTO_ORDER_DESTINATION_2'})
smach.StateMachine.add( 'GOTO_ORDER_DESTINATION_2', states.NavigateToWaypoint(robot, beverage_dest_desig, radius = WAYPOINT_RADIUS),
transitions={ 'arrived' :'SAY_TAKE_ORDER',
'unreachable' :'failed',
'goal_not_defined' :'failed'})
@smach.cb_interface(outcomes=['spoken'])
def instruct_guest(userdata=None):
try:
order = ORDERS[order_type]
robot.speech.speak("Dear guest at table {location}, you can get your {name} from my basket.".format(**order))
except KeyError:
rospy.logerr("No beverage in ORDERS")
return 'spoken'
smach.StateMachine.add( 'SAY_TAKE_ORDER',
smach.CBState(instruct_guest),
transitions={'spoken' :'AWAIT_TAKE_ORDER_CONFIRMATION'})
smach.StateMachine.add( 'AWAIT_TAKE_ORDER_CONFIRMATION',
states.WaitTime(robot, 5),
transitions={ 'waited' :'SAY_ENJOY_ORDER',
'preempted' :'failed'})
smach.StateMachine.add( 'SAY_ENJOY_ORDER',
states.Say(robot, ["Enjoy your {}".format(order_type)], block=False),
transitions={ 'spoken' :'succeeded'})
def __init__(self, robot, drop_zone_id):
"""
:param robot: robot object
:param drop_designator: EdEntityDesignator designating the collection zone
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"])
arm_designator = ds.OccupiedArmDesignator(robot=robot, arm_properties={})
with self:
smach.StateMachine.add("GO_TO_COLLECTION_ZONE",
states.NavigateToWaypoint(robot, ds.EntityByIdDesignator(robot, id=drop_zone_id),
radius=0.5),
transitions={"arrived": "DROP_TRASH",
"goal_not_defined": "aborted",
"unreachable": "OPEN_DOOR_PLEASE"})
smach.StateMachine.add("OPEN_DOOR_PLEASE",
states.Say(robot, "Can you please open the door for me? It seems blocked!"),
transitions={"spoken": "WAIT_FOR_DOOR_OPEN"})
smach.StateMachine.add("WAIT_FOR_DOOR_OPEN",
states.WaitTime(robot=robot, waittime=5),
transitions={"waited": "GO_TO_COLLECTION_ZONE2",
"preempted": "GO_TO_COLLECTION_ZONE2"})
smach.StateMachine.add("GO_TO_COLLECTION_ZONE2",
states.NavigateToWaypoint(robot, ds.EntityByIdDesignator(robot, id=drop_zone_id),
radius=0.5),
transitions={"arrived": "DROP_TRASH",
"goal_not_defined": "aborted",
"unreachable": "failed"})
smach.StateMachine.add("DROP_TRASH", DropTrash(robot=robot, arm_designator=arm_designator),
transitions={"succeeded": "succeeded",
"failed": "HANDOVER"})
smach.StateMachine.add("HANDOVER",
states.HandoverToHuman(robot=robot, arm_designator=arm_designator),
transitions={"succeeded": "succeeded",
"failed": "failed"})
def __init__(self, robot, object_category_des, room_des):
smach.StateMachine.__init__(self, outcomes=["done", "failed"])
room_id_des = ds.AttrDesignator(room_des, "id", resolve_type=str)
with self:
smach.StateMachine.add("LOOK_INTO_ROOM", robot_smach_states.NavigateToRoom(robot, room_des, room_des),
transitions={"arrived": "SAY_COME_TO_ME",
"unreachable": "SAY_COME_TO_ME",
"goal_not_defined": "SAY_COME_TO_ME"})
smach.StateMachine.add("SAY_COME_TO_ME", robot_smach_states.Say(robot,
"Operator, please come to me in the {room}",
room=room_id_des, block=True),
transitions={"spoken": "WAIT_FOR_OPERATOR"})
smach.StateMachine.add("WAIT_FOR_OPERATOR", robot_smach_states.WaitTime(4),
transitions={"waited": "ASK_WHICH_CATERGORY",
"preempted": "ASK_WHICH_CATERGORY"})
smach.StateMachine.add("ASK_WHICH_CATERGORY", AskWhichCategory(robot,
ds.Designator(challenge_knowledge.category_grammar),
object_category_des),
transitions={"done": "done"})
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'])
self.robot = robot
self.position_constraint = PositionConstraint()
self.orientation_constraint = OrientationConstraint()
self.position_constraint.constraint = "x^2+y^2 < 1"
self.requested_location = None
rospy.Subscriber("/location_request", std_msgs.msg.String,
self.requestedLocationcallback)
self.random_nav_designator = RandomNavDesignator(self.robot)
with self:
smach.StateMachine.add("WAIT_A_SEC",
states.WaitTime(robot, waittime=1.0),
transitions={
'waited': "SELECT_ACTION",
'preempted': "Aborted"
})
smach.StateMachine.add("SELECT_ACTION",
SelectAction(),
transitions={
'continue': "DETERMINE_TARGET",
'pause': "SELECT_ACTION",
'stop': "SAY_DONE"
})
@smach.cb_interface(
outcomes=['target_determined', 'no_targets_available'],
input_keys=[],
output_keys=[])
def determine_target(userdata, designator):
entity_id = designator.getRandomGoal()
sentences = [
"Lets go look at the %s", "Lets have a look at the %s",
"Lets go to the %s", "Lets move to the %s",
"I will go to the %s", "I will now move to the %s",
"I will now drive to the %s", "I will look the %s",
"The %s will be my next location", "The %s it is",
"New goal, the %s", "Going to look at the %s",
"Moving to the %s", "Driving to the %s", "On to the %s",
"On the move to the %s", "Going to the %s"
]
robot.speech.speak(random.choice(sentences) % entity_id,
block=False)
return 'target_determined'
smach.StateMachine.add('DETERMINE_TARGET',
smach.CBState(determine_target,
cb_kwargs={
'designator':
self.random_nav_designator
}),
transitions={
'target_determined': 'DRIVE',
'no_targets_available': 'SELECT_ACTION'
})
smach.StateMachine.add('DRIVE',
states.NavigateToObserve(
robot, self.random_nav_designator),
transitions={
"arrived": "SAY_SUCCEEDED",
"unreachable": 'SAY_UNREACHABLE',
"goal_not_defined": 'SELECT_ACTION'
})
smach.StateMachine.add("SAY_SUCCEEDED",
states.Say(robot, [
"I am here",
"Goal succeeded",
"Another goal succeeded",
"Goal reached",
"Another goal reached",
"Target reached",
"Another target reached",
"Destination reached",
"Another destination reached",
"I have arrived",
"I have arrived at my goal",
"I have arrived at my target",
"I have arrived at my destination",
"I am at my goal",
"I am at my target",
"I am at my destination",
"Here I am",
]),
transitions={'spoken': 'SELECT_ACTION'})
smach.StateMachine.add(
"SAY_UNREACHABLE",
states.Say(robot, [
"I can't find a way to my goal, better try something else",
"This goal is unreachable, I better find somewhere else to go",
"I am having a hard time getting there so I will look for a new target"
]),
transitions={'spoken': 'SELECT_ACTION'})
smach.StateMachine.add(
"SAY_DONE",
states.Say(robot, [
"That's all folks", "I'll stay here for a while", "Goodbye"
]),
transitions={'spoken': 'Done'})
def __init__(self, robot):
""" Constructor
:param robot: robot object
"""
smach.StateMachine.__init__(self, outcomes=['STOP'])
start_pose = robot.base.get_location()
start_x = start_pose.frame.p.x()
start_y = start_pose.frame.p.y()
start_rz = start_pose.frame.M.GetRPY()[2]
kitchen_id = "kitchen"
kitchen_designator = states.util.designators.ed_designators.EdEntityDesignator(
robot=robot, id=kitchen_id)
customer_id = 'current_customer'
customer_designator = states.util.designators.VariableDesignator(
resolve_type=Entity, name=customer_id)
orders = []
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'SAY_WAVING',
'abort': 'STOP'
})
smach.StateMachine.add(
'SAY_WAVING',
states.Say(robot,
"Mr. Barman, please make sure that the people wave "
"slowly and put their arm up high. Like is shown "
"on my screen",
block=True),
transitions={'spoken': 'SHOW_IMAGE'})
smach.StateMachine.add(
'SHOW_IMAGE',
states.ShowImageState(
robot, "~/ros/kinetic/system/src/challenge_restaurant/"
"images/waving.jpg",
seconds=10),
transitions={
'succeeded': 'STORE_KITCHEN',
'failed': 'STORE_KITCHEN'
})
smach.StateMachine.add('STORE_KITCHEN',
StoreWaypoint(robot=robot,
location_id=kitchen_id),
transitions={'done': 'WAIT_FOR_CUSTOMER'})
# smach.StateMachine.add('WAIT_FOR_CUSTOMER',
# WaitForCustomer(robot, caller_id, kitchen_designator),
# transitions={'succeeded': 'SAY_I_HAVE_SEEN',
# 'aborted': 'STOP'})
# Implement new find state to detect nearest waving person
smach.StateMachine.add(
'WAIT_FOR_CUSTOMER',
states.FindFirstPerson(robot,
customer_designator.writeable,
properties={'tags': ['LWave', 'RWave']},
strict=False,
nearest=True,
speak=True,
look_range=(-np.pi / 4, np.pi / 4),
look_steps=4,
search_timeout=600), # 10 minutes
transitions={
'found': 'SAY_I_HAVE_SEEN',
'failed': 'WAIT_FOR_CUSTOMER'
})
# No Asking
# smach.StateMachine.add('SAY_I_HAVE_SEEN',
# states.Say(robot, 'I have seen a waving person, I will take the order, I will be there shortly! Coming your way my amigo!'),
# transitions={"spoken": 'NAVIGATE_TO_CUSTOMER'})
# End No Asking
# Asking for confirmation
smach.StateMachine.add(
'SAY_I_HAVE_SEEN',
states.Say(
robot,
'I have seen a waving person, should I take the order? '
'Please say "{0} take the order" or "{0} wait"'.format(
robot.robot_name)),
transitions={"spoken": 'WAIT_FOR_START'})
smach.StateMachine.add('WAIT_FOR_START',
AskTakeTheOrder(robot),
transitions={
'yes': 'SAY_NAVIGATE_TO_CUSTOMER',
'wait': 'WAIT_FOR_CUSTOMER',
'timeout': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add(
'SAY_NAVIGATE_TO_CUSTOMER',
states.Say(
robot,
"I am at your service, I will be there shortly! Coming your way my amigo!",
block=True),
transitions={'spoken': 'NAVIGATE_TO_CUSTOMER'})
# End Asking for confirmation
smach.StateMachine.add(
'NAVIGATE_TO_CUSTOMER',
states.NavigateToObserve(robot=robot,
entity_designator=customer_designator,
radius=0.8),
transitions={
'arrived': 'TAKE_ORDER',
'unreachable': 'SAY_NAVIGATE_TO_CUSTOMER_FALLBACK',
'goal_not_defined': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add('SAY_NAVIGATE_TO_CUSTOMER_FALLBACK',
states.Say(robot,
"Help, lets try it another way"),
transitions={'spoken': 'TURN_AROUND'})
smach.StateMachine.add(
'TURN_AROUND',
states.Turn(robot, radians=2 * math.pi),
transitions={'turned': 'NAVIGATE_TO_CUSTOMER_FALLBACK'})
smach.StateMachine.add('NAVIGATE_TO_CUSTOMER_FALLBACK',
states.NavigateToObserve(
robot=robot,
entity_designator=customer_designator,
radius=1.1),
transitions={
'arrived': 'TAKE_ORDER',
'unreachable': 'RETURN_TO_START',
'goal_not_defined': 'RETURN_TO_START'
})
smach.StateMachine.add('TAKE_ORDER',
TakeOrder(
robot=robot,
entity_designator=customer_designator,
orders=orders),
transitions={
'succeeded': 'NAVIGATE_TO_KITCHEN',
'failed': 'RETURN_TO_START'
})
smach.StateMachine.add('NAVIGATE_TO_KITCHEN',
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=kitchen_designator,
radius=0.15),
transitions={
'arrived':
'RECITE_ORDER',
'unreachable':
'SAY_NAVIGATE_TO_KITCHEN_FALLBACK',
'goal_not_defined':
'SAY_NAVIGATE_TO_KITCHEN_FALLBACK'
})
smach.StateMachine.add(
'SAY_NAVIGATE_TO_KITCHEN_FALLBACK',
states.Say(robot, "Help, how do I get there?", block=False),
transitions={'spoken': 'TURN_AROUND_KITCHEN_FALLBACK'})
smach.StateMachine.add(
'TURN_AROUND_KITCHEN_FALLBACK',
states.Turn(robot, radians=math.pi),
transitions={'turned': 'NAVIGATE_TO_KITCHEN_FALLBACK'})
smach.StateMachine.add('NAVIGATE_TO_KITCHEN_FALLBACK',
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=kitchen_designator,
radius=0.20),
transitions={
'arrived':
'RECITE_ORDER',
'unreachable':
'SAY_NAVIGATE_TO_KITCHEN_FALLBACK',
'goal_not_defined':
'SAY_NAVIGATE_TO_KITCHEN_FALLBACK'
})
smach.StateMachine.add('RECITE_ORDER',
ReciteOrders(robot=robot, orders=orders),
transitions={'spoken': 'CLEAR_ORDER'})
smach.StateMachine.add(
'CLEAR_ORDER',
ClearOrders(orders=orders),
transitions={'succeeded': 'SAY_CANNOT_GRASP'})
smach.StateMachine.add('SAY_CANNOT_GRASP',
states.Say(
robot,
"I am unable to grasp my own order, "
"could you please put it in my basket"),
transitions={'spoken': 'WAIT_FOR_OBJECTS'})
smach.StateMachine.add('WAIT_FOR_OBJECTS',
states.WaitTime(robot=robot, waittime=10.0),
transitions={
'waited': 'BRING_OBJECTS',
'preempted': 'STOP'
})
smach.StateMachine.add(
'BRING_OBJECTS',
states.NavigateToObserve(robot=robot,
entity_designator=customer_designator,
radius=1.1),
transitions={
'arrived': 'SAY_OBJECTS',
'unreachable': 'SAY_BRING_OBJECTS_FALLBACK',
'goal_not_defined': 'RETURN_TO_START'
})
smach.StateMachine.add(
'SAY_BRING_OBJECTS_FALLBACK',
states.Say(robot, "Help, how do I get there?"),
transitions={'spoken': 'TURN_AROUND_BRING_OBJECTS_FALLBACK'})
smach.StateMachine.add(
'TURN_AROUND_BRING_OBJECTS_FALLBACK',
states.Turn(robot, radians=2 * math.pi),
transitions={'turned': 'BRING_OBJECTS_FALLBACK'})
smach.StateMachine.add('BRING_OBJECTS_FALLBACK',
states.NavigateToObserve(
robot=robot,
entity_designator=customer_designator,
radius=1.1),
transitions={
'arrived': 'SAY_OBJECTS',
'unreachable': 'SAY_OBJECTS',
'goal_not_defined': 'RETURN_TO_START'
})
smach.StateMachine.add(
'SAY_OBJECTS',
states.Say(
robot, "Hi there handsome, here are your objects, "
"please take them from my basket"),
transitions={'spoken': 'WAIT_TO_TAKE_OBJECTS'})
smach.StateMachine.add('WAIT_TO_TAKE_OBJECTS',
states.WaitTime(robot=robot, waittime=10.0),
transitions={
'waited': 'RETURN_TO_START',
'preempted': 'STOP'
})
smach.StateMachine.add('RETURN_TO_START',
states.NavigateToPose(robot=robot,
x=start_x,
y=start_y,
rz=start_rz,
radius=0.3),
transitions={
'arrived': 'WAIT_FOR_CUSTOMER',
'unreachable':
'SAY_RETURN_TO_START_FALLBACK',
'goal_not_defined': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add(
'SAY_RETURN_TO_START_FALLBACK',
states.Say(robot, "Help, how do I get back?"),
transitions={'spoken': 'RETURN_TO_START_TURN_AROUND'})
smach.StateMachine.add(
'RETURN_TO_START_TURN_AROUND',
states.Turn(robot, radians=math.pi),
transitions={'turned': 'RETURN_TO_START_FALLBACK'})
smach.StateMachine.add('RETURN_TO_START_FALLBACK',
states.NavigateToObserve(
robot=robot,
entity_designator=customer_designator,
radius=0.7),
transitions={
'arrived': 'WAIT_FOR_CUSTOMER',
'unreachable': 'WAIT_FOR_CUSTOMER',
'goal_not_defined': 'WAIT_FOR_CUSTOMER'
})
def __init__(self, robot):
""" Constructor
:param robot: robot object
"""
smach.StateMachine.__init__(self, outcomes=['STOP'])
start_pose = robot.base.get_location()
start_x = start_pose.frame.p.x()
start_y = start_pose.frame.p.y()
start_rz = start_pose.frame.M.GetRPY()[2]
kitchen_id = "kitchen"
kitchen_designator = states.util.designators.ed_designators.EdEntityDesignator(
robot=robot, id=kitchen_id)
caller_id = "customer"
caller_designator = states.util.designators.ed_designators.EdEntityDesignator(
robot=robot, id=caller_id)
orders = {}
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'STORE_KITCHEN',
'abort': 'STOP'
})
smach.StateMachine.add('STORE_KITCHEN',
StoreWaypoint(robot=robot,
location_id=kitchen_id),
transitions={'done': 'WAIT_FOR_CUSTOMER'})
smach.StateMachine.add('WAIT_FOR_CUSTOMER',
WaitForCustomer(robot, caller_id,
kitchen_designator),
transitions={
'succeeded': 'NAVIGATE_TO_CUSTOMER',
'aborted': 'STOP',
'rejected': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add(
'NAVIGATE_TO_CUSTOMER',
states.NavigateToObserve(robot=robot,
entity_designator=caller_designator,
radius=0.85),
transitions={
'arrived': 'TAKE_ORDER',
'unreachable': 'SAY_NAVIGATE_TO_CUSTOMER_FALLBACK',
'goal_not_defined': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add('SAY_NAVIGATE_TO_CUSTOMER_FALLBACK',
states.Say(robot,
"Help, how do I get there?"),
transitions={'spoken': 'TURN_AROUND'})
smach.StateMachine.add(
'TURN_AROUND',
states.Turn(robot, radians=2 * math.pi),
transitions={'turned': 'NAVIGATE_TO_CUSTOMER_FALLBACK'})
smach.StateMachine.add('NAVIGATE_TO_CUSTOMER_FALLBACK',
states.NavigateToObserve(
robot=robot,
entity_designator=caller_designator,
radius=0.85),
transitions={
'arrived': 'TAKE_ORDER',
'unreachable': 'RETURN_TO_START',
'goal_not_defined': 'RETURN_TO_START'
})
smach.StateMachine.add('TAKE_ORDER',
TakeOrder(robot=robot,
location=caller_id,
orders=orders),
transitions={
'succeeded': 'NAVIGATE_TO_KITCHEN',
'failed': 'RETURN_TO_START'
})
smach.StateMachine.add('NAVIGATE_TO_KITCHEN',
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=kitchen_designator,
radius=0.15),
transitions={
'arrived':
'RECITE_ORDER',
'unreachable':
'SAY_NAVIGATE_TO_KITCHEN_FALLBACK',
'goal_not_defined':
'SAY_NAVIGATE_TO_KITCHEN_FALLBACK'
})
smach.StateMachine.add(
'SAY_NAVIGATE_TO_KITCHEN_FALLBACK',
states.Say(robot, "Help, how do I get there?"),
transitions={'spoken': 'TURN_AROUND_KITCHEN_FALLBACK'})
smach.StateMachine.add(
'TURN_AROUND_KITCHEN_FALLBACK',
states.Turn(robot, radians=math.pi),
transitions={'turned': 'NAVIGATE_TO_KITCHEN_FALLBACK'})
smach.StateMachine.add('NAVIGATE_TO_KITCHEN_FALLBACK',
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=kitchen_designator,
radius=0.20),
transitions={
'arrived': 'RECITE_ORDER',
'unreachable': 'RECITE_ORDER',
'goal_not_defined': 'RECITE_ORDER'
})
smach.StateMachine.add('RECITE_ORDER',
ReciteOrders(robot=robot, orders=orders),
transitions={'spoken': 'SAY_CANNOT_GRASP'})
smach.StateMachine.add('SAY_CANNOT_GRASP',
states.Say(
robot,
"I am unable to grasp my own order,"
"could you please put it in my basket"),
transitions={'spoken': 'WAIT_FOR_OBJECTS'})
smach.StateMachine.add('WAIT_FOR_OBJECTS',
states.WaitTime(robot=robot, waittime=5.0),
transitions={
'waited': 'BRING_OBJECTS',
'preempted': 'STOP'
})
smach.StateMachine.add(
'BRING_OBJECTS',
states.NavigateToObserve(robot=robot,
entity_designator=caller_designator,
radius=0.85),
transitions={
'arrived': 'SAY_OBJECTS',
'unreachable': 'SAY_BRING_OBJECTS_FALLBACK',
'goal_not_defined': 'RETURN_TO_START'
})
smach.StateMachine.add(
'SAY_BRING_OBJECTS_FALLBACK',
states.Say(robot, "Help, how do I get there?"),
transitions={'spoken': 'TURN_AROUND_BRING_OBJECTS_FALLBACK'})
smach.StateMachine.add(
'TURN_AROUND_BRING_OBJECTS_FALLBACK',
states.Turn(robot, radians=2 * math.pi),
transitions={'turned': 'BRING_OBJECTS_FALLBACK'})
smach.StateMachine.add('BRING_OBJECTS_FALLBACK',
states.NavigateToObserve(
robot=robot,
entity_designator=caller_designator,
radius=0.85),
transitions={
'arrived': 'SAY_OBJECTS',
'unreachable': 'SAY_OBJECTS',
'goal_not_defined': 'RETURN_TO_START'
})
smach.StateMachine.add(
'SAY_OBJECTS',
states.Say(
robot, "Dear mister, here are your objects, "
"please take them from my basket"),
transitions={'spoken': 'WAIT_TO_TAKE_OBJECTS'})
smach.StateMachine.add('WAIT_TO_TAKE_OBJECTS',
states.WaitTime(robot=robot, waittime=5.0),
transitions={
'waited': 'RETURN_TO_START',
'preempted': 'STOP'
})
smach.StateMachine.add('RETURN_TO_START',
states.NavigateToPose(robot=robot,
x=start_x,
y=start_y,
rz=start_rz,
radius=0.3),
transitions={
'arrived': 'WAIT_FOR_CUSTOMER',
'unreachable':
'SAY_RETURN_TO_START_FALLBACK',
'goal_not_defined': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add(
'SAY_RETURN_TO_START_FALLBACK',
states.Say(robot, "Help, how do I get back?"),
transitions={'spoken': 'RETURN_TO_START_TURN_AROUND'})
smach.StateMachine.add(
'RETURN_TO_START_TURN_AROUND',
states.Turn(robot, radians=math.pi),
transitions={'turned': 'RETURN_TO_START_FALLBACK'})
smach.StateMachine.add('RETURN_TO_START_FALLBACK',
states.NavigateToObserve(
robot=robot,
entity_designator=caller_designator,
radius=0.7),
transitions={
'arrived': 'WAIT_FOR_CUSTOMER',
'unreachable': 'WAIT_FOR_CUSTOMER',
'goal_not_defined': 'WAIT_FOR_CUSTOMER'
})
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=["Done", "Aborted"])
hmi_result_des = ds.VariableDesignator(resolve_type=HMIResult)
information_point_id_designator = ds.FuncDesignator(ds.AttrDesignator(
hmi_result_des, "semantics", resolve_type=unicode),
str,
resolve_type=str)
information_point_designator = ds.EdEntityDesignator(
robot, id_designator=information_point_id_designator)
with self:
single_item = InformMachine(robot)
if START_ROBUST:
smach.StateMachine.add("START_CHALLENGE",
states.StartChallengeRobust(
robot, INITIAL_POSE_ID),
transitions={
"Done": "ASK_WHERE_TO_GO",
"Aborted": "Aborted",
"Failed": "Aborted"
})
smach.StateMachine.add(
"ASK_WHERE_TO_GO",
states.Say(
robot,
"Near which furniture object should I go to start guiding people?"
),
transitions={"spoken": "WAIT_WHERE_TO_GO"})
smach.StateMachine.add(
"WAIT_WHERE_TO_GO",
states.HearOptionsExtra(
robot=robot,
spec_designator=ds.Designator(
initial_value=START_GRAMMAR),
speech_result_designator=hmi_result_des.writeable),
transitions={
"heard": "ASK_CONFIRMATION",
"no_result": "ASK_WHERE_TO_GO"
}) # ToDo: add fallbacks #option: STORE_STARTING_POSE
smach.StateMachine.add(
"ASK_CONFIRMATION",
states.Say(robot, [
"I hear that you would like me to start the tours at"
" the {place}, is this correct?"
],
place=information_point_id_designator,
block=True),
transitions={"spoken": "CONFIRM_LOCATION"})
smach.StateMachine.add("CONFIRM_LOCATION",
states.HearOptions(
robot=robot, options=["yes", "no"]),
transitions={
"yes": "MOVE_OUT_OF_MY_WAY",
"no": "ASK_WHERE_TO_GO",
"no_result": "ASK_WHERE_TO_GO"
})
smach.StateMachine.add(
"MOVE_OUT_OF_MY_WAY",
states.Say(robot,
"Please move your ass so I can get going!"),
transitions={"spoken": "TC_MOVE_TIME"})
smach.StateMachine.add("TC_MOVE_TIME",
states.WaitTime(robot=robot,
waittime=3),
transitions={
"waited": "NAV_TO_START",
"preempted": "Aborted"
})
smach.StateMachine.add(
"NAV_TO_START",
states.NavigateToSymbolic(
robot=robot,
entity_designator_area_name_map={
information_point_designator: "in_front_of"
},
entity_lookat_designator=information_point_designator),
transitions={
"arrived": "TURN_AROUND",
"unreachable": "WAIT_NAV_BACKUP",
"goal_not_defined": "Aborted"
}) # If this happens: never mind
smach.StateMachine.add("WAIT_NAV_BACKUP",
states.WaitTime(robot, 3.0),
transitions={
"waited": "NAV_TO_START_BACKUP",
"preempted": "Aborted"
})
smach.StateMachine.add(
"NAV_TO_START_BACKUP",
states.NavigateToSymbolic(
robot=robot,
entity_designator_area_name_map={
information_point_designator: "near"
},
entity_lookat_designator=information_point_designator),
transitions={
"arrived": "TURN_AROUND",
"unreachable":
"SAY_CANNOT_REACH_WAYPOINT", # Current pose backup
"goal_not_defined": "Aborted"
}) # If this happens: never mind
@smach.cb_interface(outcomes=["done"])
def _turn_around(userdata=None):
""" Turns the robot approximately 180 degrees around """
v_th = 0.5
robot.base.force_drive(vx=0.0,
vy=0.0,
vth=v_th,
timeout=math.pi / v_th)
return "done"
smach.StateMachine.add(
"TURN_AROUND",
smach.CBState(_turn_around),
transitions={"done": "STORE_STARTING_POSE"})
smach.StateMachine.add(
"SAY_CANNOT_REACH_WAYPOINT",
states.Say(
robot, "I am not able to reach the starting point."
"I'll use this as starting point"),
transitions={"spoken": "STORE_STARTING_POSE"})
else:
smach.StateMachine.add("INITIALIZE",
states.Initialize(robot),
transitions={
"initialized":
"STORE_STARTING_POSE",
"abort": "Aborted"
})
## This is purely for a back up scenario until the range iterator
@smach.cb_interface(outcomes=["succeeded"])
def store_pose(userdata=None):
base_loc = robot.base.get_location()
base_pose = base_loc.frame
location_id = INFORMATION_POINT_ID
robot.ed.update_entity(id=location_id,
frame_stamped=FrameStamped(
base_pose, "/map"),
type="waypoint")
return "succeeded"
smach.StateMachine.add("STORE_STARTING_POSE",
smach.CBState(store_pose),
transitions={"succeeded": "RANGE_ITERATOR"})
# Begin setup iterator
# The exhausted argument should be set to the prefered state machine outcome
range_iterator = smach.Iterator(
outcomes=["succeeded",
"failed"], # Outcomes of the iterator state
input_keys=[],
output_keys=[],
it=lambda: range(1000),
it_label="index",
exhausted_outcome="succeeded")
with range_iterator:
smach.Iterator.set_contained_state(
"SINGLE_ITEM",
single_item,
loop_outcomes=["succeeded", "failed"])
smach.StateMachine.add("RANGE_ITERATOR", range_iterator, {
"succeeded": "AT_END",
"failed": "Aborted"
})
# End setup iterator
smach.StateMachine.add("AT_END",
states.Say(robot, "Goodbye"),
transitions={"spoken": "Done"})
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=["Done", "Aborted", "Failed"])
self.robot = robot
with self:
smach.StateMachine.add( "RESET_ALL",
states.ResetArmsTorsoHead(robot, timeout=5.0),
transitions={"done":"WAIT_A_SEC"})
smach.StateMachine.add( "WAIT_A_SEC",
states.WaitTime(robot, waittime=1),
transitions={'waited' :"SAY_SHOW_QR_MARKER",
'preempted':"Aborted"})
smach.StateMachine.add( "SAY_SHOW_QR_MARKER",
states.Say(robot, [ "If you show me a QR marker, I will dance with you! ",
"Show me one of those paddles with a QR code and I'll do your bidding.",
"Please show me a QR marker",
"Show me one of those paddles, please."]),
transitions={"spoken":"CHECK_QR_MARKER"})
smach.StateMachine.add( "CHECK_QR_MARKER",
CheckQRMarker(robot, 100),
transitions=qr_transitions)
smach.StateMachine.add( "SELECT_RANDOM",
RandomOutcome(robot, demos),
transitions= random_transitions)
smach.StateMachine.add( "SAY_HI",
states.Say(robot, ["Hello, , , My name is AMIGO. . ., I am the care robot of Eindhoven university of technology. Have a nice day!",
"Hi There! , . What a beautiful day it is. I am happy to show my autonomous care robot skills to the people of Magdenburg!"]),
transitions={"spoken":"RESET_ALL"})
smach.StateMachine.add( "MAKE_JOKES",
MakeJokes(robot),
transitions={"Done":"RESET_ALL"})
smach.StateMachine.add( "TURN_AROUND",
TurnAround(robot),
transitions={"Done":"RESET_ALL", "Aborted":"RESET_ALL", "Failed":"RESET_ALL"})
# smach.StateMachine.add( "LOOK_AT_PERSON",
# LookAtPerson(robot),
# transitions={"Done":"RESET_ALL", "Aborted":"RESET_ALL", "Failed":"RESET_ALL"})
# @smach.cb_interface(outcomes=['done'])
# def wave_lights_wrapped(*args, **kwargs):
# wave_lights(robot)
# return 'done'
# smach.StateMachine.add( "WAVE_LIGHTS",
# smach.CBState(wave_lights_wrapped),
# transitions={"done":"RESET_ALL"})
smach.StateMachine.add( "FLASH_LIGHTS",
FlashLights(robot),
transitions={"Done":"RESET_ALL"})
smach.StateMachine.add( "WALK_EGYPTIAN",
WalkLikeAnEgyptian(robot),
transitions={"Done":"RESET_ALL"})
# smach.StateMachine.add( "BOO",
# Boo(robot),
# transitions={"Done":"RESET_ALL", "Aborted":"RESET_ALL", "Failed":"RESET_ALL"})
# smach.StateMachine.add( "PICKUP_LINES",
# Pickup(robot),
# transitions={"Done":"RESET_ALL"})
smach.StateMachine.add( "R2D2",
R2D2(robot),
transitions={"Done":"RESET_ALL"})
smach.StateMachine.add( "TOETER",
Toeter(robot),
transitions={"Done":"RESET_ALL"})
smach.StateMachine.add( "MACARENA",
Macarena(robot),
transitions={"Done":"RESET_ALL"})
smach.StateMachine.add( "GANGNAM",
GangNamStyle(robot),
transitions={"Done":"RESET_ALL"})
smach.StateMachine.add( "HOOFD_SCHOUDERS_KNIE_TEEN",
HoofdSchouderKnieTeen(robot),
transitions={"Done":"RESET_ALL"})
def __init__(self,
robot,
entity_des,
operator_distance=1.5,
operator_radius=0.5):
""" Constructor
:param robot: robot object
:param entity_des: designator resolving to a room or a piece of furniture
:param operator_distance: (float) check for the operator to be within this range of the robot
:param operator_radius: (float) from the point behind the robot defined by `distance`, the person must be within this radius
"""
smach.StateMachine.__init__(self,
outcomes=[
"arrived", "unreachable",
"goal_not_defined", "lost_operator",
"preempted"
])
self.operator_distance = operator_distance
self.operator_radius = operator_radius
with self:
@smach.cb_interface(outcomes=["room", "object"])
def determine_type(userdata=None):
entity = entity_des.resolve()
entity_type = entity.type
if entity_type == "room":
return "room"
else:
return "object"
smach.StateMachine.add("DETERMINE_TYPE",
smach.CBState(determine_type),
transitions={
"room": "GUIDE_TO_ROOM",
"object": "GUIDE_TO_FURNITURE"
})
smach.StateMachine.add(
"GUIDE_TO_ROOM",
guidance.GuideToSymbolic(
robot, {entity_des: "in"},
entity_des,
operator_distance=self.operator_distance,
operator_radius=self.operator_radius),
transitions={
"arrived": "arrived",
"unreachable": "WAIT_ROOM_BACKUP",
"goal_not_defined": "goal_not_defined",
"lost_operator": "ROOM_NAV_BACKUP",
"preempted": "preempted"
})
smach.StateMachine.add("WAIT_ROOM_BACKUP",
states.WaitTime(robot, 3.0),
transitions={
"waited": "GUIDE_TO_ROOM_BACKUP",
"preempted": "preempted"
})
smach.StateMachine.add(
"GUIDE_TO_ROOM_BACKUP",
guidance.GuideToSymbolic(
robot, {entity_des: "in"},
entity_des,
operator_distance=self.operator_distance,
operator_radius=self.operator_radius),
transitions={
"arrived": "arrived",
"unreachable": "unreachable",
"goal_not_defined": "goal_not_defined",
"lost_operator": "ROOM_NAV_BACKUP",
"preempted": "preempted"
})
smach.StateMachine.add(
"GUIDE_TO_FURNITURE",
guidance.GuideToSymbolic(
robot, {entity_des: "in_front_of"},
entity_des,
operator_distance=self.operator_distance,
operator_radius=self.operator_radius),
transitions={
"arrived": "arrived",
"unreachable":
"WAIT_FURNITURE_BACKUP", # Something is blocking
"goal_not_defined":
"GUIDE_NEAR_FURNITURE", # in_front_of not defined
"lost_operator": "FURNITURE_NAV_BACKUP",
"preempted": "preempted"
})
smach.StateMachine.add(
"GUIDE_NEAR_FURNITURE",
guidance.GuideToSymbolic(
robot, {entity_des: "near"},
entity_des,
operator_distance=self.operator_distance,
operator_radius=self.operator_radius),
transitions={
"arrived": "arrived",
"unreachable": "WAIT_FURNITURE_BACKUP",
"goal_not_defined": "goal_not_defined",
"lost_operator": "FURNITURE_NAV_BACKUP",
"preempted": "preempted"
})
smach.StateMachine.add("WAIT_FURNITURE_BACKUP",
states.WaitTime(robot, 3.0),
transitions={
"waited": "GUIDE_NEAR_FURNITURE_BACKUP",
"preempted": "preempted"
})
smach.StateMachine.add(
"GUIDE_NEAR_FURNITURE_BACKUP",
guidance.GuideToSymbolic(
robot, {entity_des: "near"},
entity_des,
operator_distance=self.operator_distance,
operator_radius=self.operator_radius),
transitions={
"arrived": "arrived",
"unreachable": "unreachable",
"goal_not_defined": "goal_not_defined",
"lost_operator": "FURNITURE_NAV_BACKUP",
"preempted": "preempted"
})
smach.StateMachine.add("ROOM_NAV_BACKUP",
states.NavigateToSymbolic(
robot, {entity_des: "in"}, entity_des),
transitions={
"arrived": "SAY_ARRIVED",
"unreachable": "unreachable",
"goal_not_defined": "goal_not_defined",
})
smach.StateMachine.add("FURNITURE_NAV_BACKUP",
states.NavigateToSymbolic(
robot, {entity_des: "near"},
entity_des),
transitions={
"arrived": "SAY_ARRIVED",
"unreachable": "unreachable",
"goal_not_defined": "goal_not_defined",
})
smach.StateMachine.add(
"SAY_ARRIVED",
states.Say(
robot,
"We have arrived. I'll go back to the meeting point"),
transitions={"spoken": "arrived"})
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['done', 'failed', 'aborted'])
with sm:
smach.StateMachine.add('START_CHALLENGE_ROBUST',
states.StartChallengeRobust(
robot, STARTING_POINT),
transitions={
'Done': 'GO_TO_SEARCH_POSE',
'Aborted': 'aborted',
'Failed': 'GO_TO_SEARCH_POSE'
})
smach.StateMachine.add('SAY_START',
states.Say(robot,
"Finding your mates, here we go!",
block=False),
transitions={'spoken': 'GO_TO_SEARCH_POSE'})
smach.StateMachine.add('GO_TO_SEARCH_POSE',
states.NavigateToWaypoint(
robot,
ds.EntityByIdDesignator(robot,
id=SEARCH_POINT),
radius=0.375),
transitions={
'arrived': 'RISE_FOR_THE_PEOPLE',
'goal_not_defined': 'failed',
'unreachable': 'WAIT_SEARCH_POSE'
})
smach.StateMachine.add('WAIT_SEARCH_POSE',
states.WaitTime(robot, 5),
transitions={
'preempted': 'aborted',
'waited': 'GO_TO_SEARCH_POSE'
})
# noinspection PyUnusedLocal
@smach.cb_interface(outcomes=["done"])
def _rise_for_the_people(userdata=None):
""" Resets the location hmi attempt so that each operator gets three attempts """
robot.arms.values()[0]._send_joint_trajectory(
[[0.70, -1.9, 0.0, -1.57, 0.0]])
robot.speech.speak(
"Hi there. My Name is Hero. I'm looking for the mates of my operator",
block=False)
return "done"
smach.StateMachine.add("RISE_FOR_THE_PEOPLE",
smach.CBState(_rise_for_the_people),
transitions={"done": "LOCATE_PEOPLE"})
# locate all four people
smach.StateMachine.add('LOCATE_PEOPLE',
LocatePeople(robot, room_id=ROOM_ID),
transitions={
'done': 'RESET_FOR_DRIVING',
})
# noinspection PyUnusedLocal
@smach.cb_interface(outcomes=["done"])
def _reset_for_driving(userdata=None):
""" Resets the location hmi attempt so that each operator gets three attempts """
robot.speech.speak("Thank you for your attention", block=False)
robot.arms.values()[0]._send_joint_trajectory([
[0.01, -1.9, 0.0, -1.57, 0.0], # Inspect with q0 low
[0.01, 0.0, -1.57, -1.57, 0.0]
]) # Reset
return "done"
smach.StateMachine.add("RESET_FOR_DRIVING",
smach.CBState(_reset_for_driving),
transitions={"done": "GO_BACK_TO_OPERATOR"})
# drive back to the operator to describe the mates
smach.StateMachine.add('GO_BACK_TO_OPERATOR',
states.NavigateToWaypoint(
robot,
ds.EntityByIdDesignator(robot,
id=OPERATOR_POINT),
radius=0.7,
look_at_designator=ds.EntityByIdDesignator(
robot, id=OPERATOR_POINT)),
transitions={
'arrived': 'REPORT_PEOPLE',
'goal_not_defined': 'REPORT_PEOPLE',
'unreachable': 'WAIT_GO_BACK'
})
smach.StateMachine.add('WAIT_GO_BACK',
states.WaitTime(robot, 5),
transitions={
'preempted': 'aborted',
'waited': 'GO_BACK_TO_OPERATOR'
})
# check how to uniquely define them # ToDo: make this more interesting
smach.StateMachine.add('REPORT_PEOPLE',
ReportPeople(robot),
transitions={'done': 'done'})
return sm
def __init__(self, robot):
""" Constructor
:param robot: robot object
"""
smach.StateMachine.__init__(self,
outcomes=['Done', 'WAIT_FOR_CUSTOMER'])
kitchen_id = "kitchen"
kitchen_designator = states.util.designators.ed_designators.EdEntityDesignator(
robot=robot, id=kitchen_id)
caller_id = "customer"
caller_designator = states.util.designators.ed_designators.EdEntityDesignator(
robot=robot, id=caller_id)
orders = {}
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'STORE_KITCHEN',
'abort': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add('STORE_KITCHEN',
StoreWaypoint(robot=robot,
location_id=kitchen_id),
transitions={'done': 'WAIT_FOR_CUSTOMER'})
smach.StateMachine.add('WAIT_FOR_CUSTOMER',
WaitForClickedCustomer(robot, caller_id),
transitions={
'succeeded': 'NAVIGATE_TO_CUSTOMER',
'failed': 'WAIT_FOR_CUSTOMER',
'aborted': 'WAIT_FOR_CUSTOMER',
'rejected': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add('NAVIGATE_TO_CUSTOMER',
states.NavigateToObserve(
robot=robot,
entity_designator=caller_designator,
radius=0.7),
transitions={
'arrived': 'TAKE_ORDER',
'unreachable': 'TAKE_ORDER',
'goal_not_defined': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add('TAKE_ORDER',
TakeOrder(robot=robot,
location=caller_id,
orders=orders),
transitions={
'succeeded': 'NAVIGATE_TO_KITCHEN',
'failed': 'WAIT_FOR_CUSTOMER',
'misunderstood': 'TAKE_ORDER'
})
smach.StateMachine.add('NAVIGATE_TO_KITCHEN',
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=kitchen_designator,
radius=0.15),
transitions={
'arrived': 'RECITE_ORDER',
'unreachable': 'RECITE_ORDER',
'goal_not_defined': 'RECITE_ORDER'
})
smach.StateMachine.add('RECITE_ORDER',
ReciteOrders(robot=robot, orders=orders),
transitions={'spoken': 'SAY_CANNOT_GRASP'})
smach.StateMachine.add('SAY_CANNOT_GRASP',
states.Say(
robot,
"I am unable to grasp my own order,"
"could you please put it in my basket"),
transitions={'spoken': 'WAIT_FOR_OBJECTS'})
smach.StateMachine.add('WAIT_FOR_OBJECTS',
states.WaitTime(robot=robot, waittime=5.0),
transitions={
'waited': 'BRING_OBJECTS',
'preempted': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add('BRING_OBJECTS',
states.NavigateToObserve(
robot=robot,
entity_designator=caller_designator,
radius=0.7),
transitions={
'arrived': 'SAY_OBJECTS',
'unreachable': 'SAY_OBJECTS',
'goal_not_defined': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add(
'SAY_OBJECTS',
states.Say(
robot, "Dear mister, here are your objects, "
"please take them from my basket"),
transitions={'spoken': 'WAIT_TO_TAKE_OBJECTS'})
smach.StateMachine.add('WAIT_TO_TAKE_OBJECTS',
states.WaitTime(robot=robot, waittime=5.0),
transitions={
'waited': 'RETURN_TO_KITCHEN',
'preempted': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add('RETURN_TO_KITCHEN',
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=kitchen_designator,
radius=0.15),
transitions={
'arrived': 'WAIT_FOR_CUSTOMER',
'unreachable': 'WAIT_FOR_CUSTOMER',
'goal_not_defined': 'WAIT_FOR_CUSTOMER'
})
smach.StateMachine.add('SAY_DONE',
states.Say(robot,
"That's it for today, I'm done"),
transitions={'spoken': 'Done'})
