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",
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",
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",
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, room_grammar, roomw, cleanup_locationsw):
smach.StateMachine.__init__(self, outcomes=["done"])
"""
Ask the operator which room has to be cleaned
"""
hmi_result_des = ds.VariableDesignator(resolve_type=hmi.HMIResult,
name="hmi_result_des")
room_name_des = ds.FuncDesignator(ds.AttrDesignator(
hmi_result_des, "semantics", resolve_type=unicode),
str,
resolve_type=str)
@smach.cb_interface(outcomes=['done'])
def write_room(ud, des_read, des_write):
# type: (object, ds.Designator, ds.Designator) -> str
assert (ds.is_writeable(des_write))
assert (des_write.resolve_type == des_read.resolve_type)
des_write.write(des_read.resolve())
return 'done'
with self:
smach.StateMachine.add("ASK_WHICH_ROOM",
Say(robot,
"Which room should I clean for you?",
block=True),
transitions={"spoken": "HEAR_ROOM"})
smach.StateMachine.add("HEAR_ROOM",
HearOptionsExtra(
robot, room_grammar,
ds.writeable(hmi_result_des)),
transitions={
"heard": "SAY_HEARD_CORRECT",
"no_result": "ASK_WHICH_ROOM"
})
smach.StateMachine.add(
"SAY_HEARD_CORRECT",
Say(robot,
"I understood that the {room} should be cleaned, is this correct?",
room=room_name_des,
block=True),
transitions={"spoken": "HEAR_CORRECT"})
smach.StateMachine.add("HEAR_CORRECT",
AskYesNo(robot),
transitions={
"yes": "FILL_LOCATIONS",
"no": "ASK_WHICH_ROOM",
"no_result": "ASK_WHICH_ROOM"
})
smach.StateMachine.add("FILL_LOCATIONS",
RoomToCleanUpLocations(
challenge_knowledge, room_name_des,
cleanup_locationsw),
transitions={"done": "WRITE_ROOM"})
smach.StateMachine.add('WRITE_ROOM',
smach.CBState(
write_room,
cb_args=[room_name_des, roomw]),
transitions={'done': 'done'})
def __init__(self, robot, category_grammar, categoryw):
smach.StateMachine.__init__(self, outcomes=["done"])
hmi_result_des = ds.VariableDesignator(resolve_type=hmi.HMIResult,
name="hmi_result_des")
category_des = ds.FuncDesignator(ds.AttrDesignator(
hmi_result_des, "semantics", resolve_type=unicode),
str,
resolve_type=str)
@smach.cb_interface(outcomes=['done'])
def write_category(ud, des_read, des_write):
# type: (object, ds.Designator, ds.Designator) -> str
assert (ds.is_writeable(des_write))
assert (des_write.resolve_type == des_read.resolve_type)
des_write.write(des_read.resolve())
return 'done'
with self:
smach.StateMachine.add(
"ASK_WHERE_TO_DROP",
Say(robot,
"Please look at the object in my gripper and tell me"
"which category it is. If it should be thrown away,"
"call it trash",
block=True),
transitions={"spoken": "HEAR_LOCATION"})
smach.StateMachine.add("HEAR_LOCATION",
HearOptionsExtra(
robot, category_grammar,
ds.writeable(hmi_result_des)),
transitions={
"heard": "SAY_HEARD_CORRECT",
"no_result": "ASK_WHERE_TO_DROP"
})
smach.StateMachine.add(
"SAY_HEARD_CORRECT",
Say(robot,
"I understood that the object is of category {category}, is this correct?",
category=category_des,
block=True),
transitions={"spoken": "HEAR_CORRECT"})
smach.StateMachine.add("HEAR_CORRECT",
AskYesNo(robot),
transitions={
"yes": "WRITE_CATEGORY",
"no": "ASK_WHERE_TO_DROP",
"no_result": "ASK_WHERE_TO_DROP"
})
smach.StateMachine.add('WRITE_CATEGORY',
smach.CBState(
write_category,
cb_args=[category_des, categoryw]),
transitions={'done': 'done'})
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, selected_entity_designator, room_des):
smach.StateMachine.__init__(self, outcomes=['done', 'failed'])
store_entity_id_des = ds.VariableDesignator(resolve_type=str,
name="store_entity_id")
store_entity_des = ds.EdEntityDesignator(
robot, id_designator=store_entity_id_des)
selected_entity_type_des = ds.AttrDesignator(
selected_entity_designator, "type", resolve_type=str)
store_area_name_des = ds.VariableDesignator(resolve_type=str,
name="store_entity_id")
trash_place_pose = DropPoseDesignator(robot, store_entity_des, 0.6,
"drop_pose")
category_des = ds.VariableDesignator(resolve_type=str,
name="category_des")
with self:
smach.StateMachine.add(
"SPEAK",
Say(robot,
["I will pick-up the {object}", "Let's move the {object}"],
object=selected_entity_type_des,
block=True),
transitions={"spoken": "GRAB"})
smach.StateMachine.add("GRAB",
Grab(
robot, selected_entity_designator,
ds.UnoccupiedArmDesignator(
robot,
arm_properties={
"required_trajectories":
["prepare_grasp"],
"required_goals":
["carrying_pose"],
"required_gripper_types":
[arms.GripperTypes.GRASPING]
},
name="empty_arm_designator")),
transitions={
"done": "SAY_GRAB_SUCCESS",
"failed": "ARM_RESET"
})
smach.StateMachine.add(
"ARM_RESET",
ArmToJointConfig(
robot,
ds.UnoccupiedArmDesignator(
robot,
arm_properties={"required_goals": ["reset"]},
name="empty_arm_designator"), "reset"),
transitions={
"succeeded": "SAY_GRAB_FAILED",
"failed": "SAY_GRAB_FAILED"
})
smach.StateMachine.add(
'SAY_GRAB_SUCCESS',
Say(robot, [
"Now I am going to move this item",
"Let's clean up this object", "Away with this one",
"Everything will be cleaned"
],
block=False),
transitions={"spoken": "GET_CATEGORY"})
smach.StateMachine.add(
'SAY_GRAB_FAILED',
Say(robot, [
"I could not grab the item.", "I failed to grasp the item",
"I cannot reach the item", "Item grab failed"
],
block=False),
transitions={"spoken": "failed"})
smach.StateMachine.add('CHECK_ARM_FREE',
ArmFree(robot),
transitions={
"yes": "done",
"no": "CHECK_ARM_OCCUPIED"
})
smach.StateMachine.add('CHECK_ARM_OCCUPIED',
ArmOccupied(robot),
transitions={
"yes": "GET_CATEGORY",
"no": "done"
})
# # ROBOT
# smach.StateMachine.add('GET_CATEGORY',
# EntityToCategory(robot, selected_entity_designator, category_des.writeable),
# transitions={"done": "DETERMINE_PLACE_LOCATION",
# "failed": "NAVIGATE_TO_TRASH"})
# OPERATOR
smach.StateMachine.add('GET_CATEGORY',
OperatorToCategory(robot,
category_des.writeable,
room_des),
transitions={
"done": "DETERMINE_PLACE_LOCATION",
"failed": "NAVIGATE_TO_TRASH"
})
smach.StateMachine.add(
'DETERMINE_PLACE_LOCATION',
CategoryToLocation(category_des, store_entity_id_des.writeable,
store_area_name_des.writeable),
transitions={
"trashbin": "INSPECT_TRASH",
"other": "PLACE_TO_STORE",
"failed": "NAVIGATE_TO_TRASH"
})
smach.StateMachine.add('NAVIGATE_TO_TRASH',
NavigateToPlace(
robot, trash_place_pose,
ds.OccupiedArmDesignator(
robot, {},
name="occupied_arm_designator")),
transitions={
"arrived": "PLACE_IN_TRASH",
"unreachable": "SAY_PLACE_FAILED",
"goal_not_defined": "SAY_PLACE_FAILED"
})
smach.StateMachine.add('INSPECT_TRASH',
Inspect(robot, store_entity_des),
transitions={
"done": "PLACE_IN_TRASH",
"failed": "SAY_PLACE_FAILED"
})
arm_properties_place = {
"required_trajectories": ["prepare_place"],
"required_gripper_types": [arms.GripperTypes.GRASPING]
}
arm_designator_place = ds.OccupiedArmDesignator(
robot, arm_properties_place, name="occupied_arm_designator")
smach.StateMachine.add('PLACE_IN_TRASH',
Place(robot, selected_entity_designator,
trash_place_pose,
arm_designator_place),
transitions={
"done": "SAY_PLACE_SUCCESS",
"failed": "SAY_PLACE_FAILED"
})
arm_designator_place_store = ds.OccupiedArmDesignator(
robot, arm_properties_place, name="occupied_arm_designator")
smach.StateMachine.add('PLACE_TO_STORE',
Place(robot, selected_entity_designator,
store_entity_des,
arm_designator_place_store,
"on_top_of"),
transitions={
"done": "SAY_PLACE_SUCCESS",
"failed": "SAY_PLACE_FAILED"
})
smach.StateMachine.add(
'SAY_PLACE_SUCCESS',
Say(robot, [
"Bye bye!", "Yeah!", "Successfully disposed the item",
"Another score for {}".format(robot.robot_name)
],
block=False),
transitions={"spoken": "CHECK_ARM_OCCUPIED"})
smach.StateMachine.add(
'SAY_PLACE_FAILED',
Say(robot, [
"I could not cleanup the item.",
"I cannot put the item in the trashbin",
"Item cleanup failed"
],
block=False),
transitions={"spoken": "CHECK_ARM_OCCUPIED"})
def __init__(self, robot, furniture_designator, entity_designator):
# type: (Robot, object) -> None
"""
Drives to the designated furniture object, inspects this and selects the entity that will be pointed to
:param robot: (Robot) robot API object
:param furniture_designator: (EdEntityDesignator) designates the furniture object that was pointed to.
:param entity_designator: (EdEntityDesignator) writeable EdEntityDesignator
"""
# ToDo: we need to add userdata
smach.StateMachine.__init__(self,
outcomes=["succeeded", "failed"],
input_keys=["laser_dot"])
assert ds.is_writeable(entity_designator), "Entity designator must be writeable for this purpose"
object_ids_des = ds.VariableDesignator([], resolve_type=[states.ClassificationResult])
with self:
smach.StateMachine.add("SAY_GO",
states.Say(robot, "Let's go to the {furniture_object}",
furniture_object=ds.AttrDesignator(furniture_designator, "id",
resolve_type=str)),
transitions={"spoken": "INSPECT_FURNITURE"})
smach.StateMachine.add("INSPECT_FURNITURE",
states.Inspect(robot=robot,
entityDes=furniture_designator,
objectIDsDes=object_ids_des,
navigation_area="in_front_of",
),
transitions={"done": "SELECT_ENTITY",
"failed": "SAY_INSPECTION_FAILED"}) # ToDo: fallback?
smach.StateMachine.add("SAY_INSPECTION_FAILED",
states.Say(robot, "I am sorry but I was not able to reach the {furniture_object}",
furniture_object=ds.AttrDesignator(furniture_designator, "id",
resolve_type=str)),
transitions={"spoken": "failed"})
@smach.cb_interface(outcomes=["succeeded", "no_entities"],
input_keys=["laser_dot"])
def select_entity(userdata):
"""
Selects the entity that the robot believes the operator has pointed to and that the robot will
identify later on.
Userdata contains key 'laser_dot' with value geometry_msgs.msg.PointStamped where the operator pointed
at.
:param userdata: (dict)
:return: (str) outcome
"""
assert userdata.laser_dot.header.frame_id.endswith("map"), "Provide your laser dot in map frame"
# Extract classification results
entity_ids = [cr.id for cr in object_ids_des.resolve()]
rospy.loginfo("Segmented entities: {}".format(entity_ids))
# Obtain all corresponding entities
all_entities = robot.ed.get_entities()
segmented_entities = [e for e in all_entities if e.id in entity_ids]
# Filter out 'unprobable' entities
candidates = []
for entity in segmented_entities: # type: Entity
# The following filtering has been 'copied' from the cleanup challenge
# It can be considered a first step but does not take the orientation of the convex hull into
# account
shape = entity.shape
size_x = max(shape.x_max - shape.x_min, 0.001)
size_y = max(shape.y_max - shape.y_min, 0.001)
if size_x > SIZE_LIMIT or size_y > SIZE_LIMIT:
continue
if not 1 / min(RATIO_LIMIT, 1000) <= size_x / size_y <= min(RATIO_LIMIT, 1000):
continue
candidates.append(entity)
# If no entities left: don't bother continuing
if not candidates:
rospy.logwarn("No 'probable' entities left")
return "no_entities"
# Select entity closest to the point where the operator pointed at (i.e., closest in 2D)
closest_tuple = (None, None)
x_ref = userdata.laser_dot.point.x
y_ref = userdata.laser_dot.point.y
# ToDo: use sorting for this...
for e in candidates: # type: Entity
x_e = e.pose.frame.p.x()
y_e = e.pose.frame.p.y()
distance_2d = math.hypot(x_ref - x_e, y_ref - y_e)
rospy.loginfo("Entity {} at {}, {}: distance = {}".format(e.id, x_e, y_e, distance_2d))
if closest_tuple[0] is None or distance_2d < closest_tuple[1]:
closest_tuple = (e, distance_2d)
rospy.loginfo("Best entity: {} at {}".format(closest_tuple[0].id, closest_tuple[1]))
entity_designator.write(closest_tuple[0])
return "succeeded"
smach.StateMachine.add("SELECT_ENTITY",
smach.CBState(select_entity),
transitions={"succeeded": "succeeded",
"no_entities": "failed"})
def __init__(self, robot):
"""
Constructor
:param robot: robot object
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"])
with self:
self.spec_des = ds.Designator(knowledge.location_grammar)
self.answer_des = ds.VariableDesignator(resolve_type=HMIResult)
self.entity_des = EntityFromHmiResults(robot, self.answer_des)
self._location_hmi_attempt = 0
self._max_hmi_attempts = 3 # ToDo: parameterize?
@smach.cb_interface(outcomes=["reset"])
def _reset_location_hmi_attempt(userdata=None):
""" Resets the location hmi attempt so that each operator gets three attempts """
self._location_hmi_attempt = 0
return "reset"
smach.StateMachine.add("RESET_HMI_ATTEMPT",
smach.CBState(_reset_location_hmi_attempt),
transitions={"reset": "ANNOUNCE_ITEM"})
if WAIT_MODE == WaitMode.SPEECH:
smach.StateMachine.add(
"ANNOUNCE_ITEM",
Say(robot,
"Hello, my name is {}. Please call me by my name. "
"Talk loudly into my microphone and wait for the ping".
format(robot.robot_name),
block=True),
transitions={"spoken": "WAIT_TO_BE_CALLED"})
smach.StateMachine.add(
"WAIT_TO_BE_CALLED",
HearOptions(robot, ["{}".format(robot.robot_name)],
timeout=10),
transitions={
"{}".format(robot.robot_name): "INSTRUCT",
"no_result": "ANNOUNCE_ITEM"
})
elif WAIT_MODE == WaitMode.VISUAL:
smach.StateMachine.add(
"ANNOUNCE_ITEM",
Say(robot,
"Hello, my name is {}. Please step in front of me.".
format(robot.robot_name),
block=True),
transitions={"spoken": "WAIT_TO_BE_CALLED"})
smach.StateMachine.add("WAIT_TO_BE_CALLED",
WaitForPersonInFront(
robot,
attempts=10,
sleep_interval=1.0),
transitions={
"success": "INSTRUCT",
"failed": "SAY_NOT_DETECTED"
})
smach.StateMachine.add(
"SAY_NOT_DETECTED",
Say(robot,
"I did not see you but will try to continue anyway.".
format(robot.robot_name),
block=True),
transitions={"spoken": "INSTRUCT"})
smach.StateMachine.add(
"INSTRUCT",
Say(robot, [
"Please tell me where you would like to go. "
"Talk loudly into my microphone and wait for the ping"
],
block=True),
transitions={"spoken": "LISTEN_FOR_LOCATION"})
smach.StateMachine.add("LISTEN_FOR_LOCATION",
HearOptionsExtra(robot, self.spec_des,
self.answer_des.writeable,
6),
transitions={
"heard": "ASK_CONFIRMATION",
"no_result": "HANDLE_FAILED_HMI"
})
smach.StateMachine.add(
"ASK_CONFIRMATION",
Say(robot, [
"I hear that you would like to go to the {place}, is this correct?"
],
place=ds.AttrDesignator(self.entity_des,
"id",
resolve_type=str)),
transitions={"spoken": "CONFIRM_LOCATION"})
smach.StateMachine.add("CONFIRM_LOCATION",
HearOptions(robot=robot,
options=["yes", "no"]),
transitions={
"yes": "INSTRUCT_FOR_WAIT",
"no": "INSTRUCT",
"no_result": "HANDLE_FAILED_HMI"
})
@smach.cb_interface(outcomes=["retry", "fallback", "failed"])
def _handle_failed_hmi(userdata=None):
""" Handle failed HMI queries so we can try up to x times """
self._location_hmi_attempt += 1 # Increment
if self._location_hmi_attempt == self._max_hmi_attempts:
rospy.logwarn(
"HMI failed for the {} time, returning 'failed'".
format(self._max_hmi_attempts))
if not BACKUP_SCENARIOS:
rospy.logwarn(
"No fallback scenario's available anymore")
return "failed"
backup = BACKUP_SCENARIOS.pop(0)
robot.speech.speak("I am sorry but I did not hear you",
mood="sad",
block=False)
robot.speech.speak(backup.sentence, block=False)
self.answer_des.writeable.write(
HMIResult("", backup.entity_id))
return "fallback"
rospy.loginfo(
"HMI failed for the {} time out of {}, retrying".format(
self._location_hmi_attempt, self._max_hmi_attempts))
return "retry"
smach.StateMachine.add("HANDLE_FAILED_HMI",
smach.CBState(_handle_failed_hmi),
transitions={
"retry": "INSTRUCT",
"fallback": "INSTRUCT_FOR_WAIT",
"failed": "failed"
})
smach.StateMachine.add(
"INSTRUCT_FOR_WAIT",
Say(robot, [
"Let me think how to get to the {entity_id}",
"I will now determine the best route to the {entity_id}"
],
entity_id=ds.AttrDesignator(self.entity_des,
"id",
resolve_type=str)),
transitions={"spoken": "GIVE_DIRECTIONS"})
smach.StateMachine.add("GIVE_DIRECTIONS",
GiveDirections(robot, self.entity_des),
transitions={
"succeeded": "INSTRUCT_FOLLOW",
"failed": "failed"
})
smach.StateMachine.add("INSTRUCT_FOLLOW",
Say(robot, ["Please follow me"],
block=True),
transitions={"spoken": "GUIDE_OPERATOR"})
smach.StateMachine.add("GUIDE_OPERATOR",
GuideToRoomOrObject(robot, self.entity_des),
transitions={
"arrived": "SUCCESS",
"unreachable": "SAY_CANNOT_REACH",
"goal_not_defined": "SAY_CANNOT_REACH",
"lost_operator": "SAY_LOST_OPERATOR",
"preempted": "failed"
})
smach.StateMachine.add(
"SUCCESS",
Say(robot, ["We have arrived"], block=True),
transitions={"spoken": "RETURN_TO_INFORMATION_POINT"})
smach.StateMachine.add(
"SAY_CANNOT_REACH",
Say(robot, ["I am sorry but I cannot reach the destination."],
block=True),
transitions={"spoken": "RETURN_TO_INFORMATION_POINT"})
smach.StateMachine.add(
"SAY_LOST_OPERATOR",
Say(robot, ["Oops I have lost you completely."], block=True),
transitions={"spoken": "RETURN_TO_INFORMATION_POINT"})
smach.StateMachine.add("RETURN_TO_INFORMATION_POINT",
NavigateToWaypoint(
robot,
ds.EntityByIdDesignator(
robot, INFORMATION_POINT_ID)),
transitions={
"arrived": "succeeded",
"unreachable": "failed",
"goal_not_defined": "failed"
})
def __init__(self, robot, seats_to_inspect, room, seat_is_for=None):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
seats = SeatsInRoomDesignator(robot, seats_to_inspect, room,
"seats_in_room")
seat_ent_des = ds.VariableDesignator(resolve_type=Entity)
if seat_is_for:
ds.check_type(seat_is_for, str)
else:
seat_is_for = ds.Designator(' ')
with self:
smach.StateMachine.add(
'SAY_LETS_FIND_SEAT',
states.SayFormatted(robot, [
"Let me find a place for {name} to sit. Please be patient while I check out where there's place to sit"
],
name=seat_is_for,
block=False),
transitions={'spoken': 'ITERATE_NEXT_SEAT'})
smach.StateMachine.add('ITERATE_NEXT_SEAT',
states.IterateDesignator(
seats, seat_ent_des.writeable),
transitions={
'next': 'CHECK_SEAT_EMPTY',
'stop_iteration': 'SAY_NO_EMPTY_SEATS'
})
smach.StateMachine.add('CHECK_SEAT_EMPTY',
states.CheckVolumeEmpty(
robot, seat_ent_des, 'on_top_of', 0.2),
transitions={
'occupied': 'ITERATE_NEXT_SEAT',
'empty': 'POINT_AT_EMPTY_SEAT',
'partially_occupied':
'POINT_AT_PARTIALLY_OCCUPIED_SEAT',
'failed': 'ITERATE_NEXT_SEAT'
})
smach.StateMachine.add(
'POINT_AT_EMPTY_SEAT',
states.PointAt(robot=robot,
arm_designator=ds.UnoccupiedArmDesignator(
robot, {'required_goals': ['point_at']}),
point_at_designator=seat_ent_des,
look_at_designator=seat_ent_des),
transitions={
"succeeded": "SAY_SEAT_EMPTY",
"failed": "SAY_SEAT_EMPTY"
})
smach.StateMachine.add(
'SAY_SEAT_EMPTY',
states.SayFormatted(robot,
["Please sit on the {seat}, {name}"],
name=seat_is_for,
seat=ds.AttrDesignator(seat_ent_des,
'id',
resolve_type=str),
block=True),
transitions={'spoken': 'RESET_SUCCESS'})
smach.StateMachine.add(
'POINT_AT_PARTIALLY_OCCUPIED_SEAT',
states.PointAt(robot=robot,
arm_designator=ds.UnoccupiedArmDesignator(
robot, {'required_goals': ['point_at']}),
point_at_designator=seat_ent_des,
look_at_designator=seat_ent_des),
transitions={
"succeeded": "SAY_SEAT_PARTIALLY_OCCUPIED",
"failed": "SAY_SEAT_PARTIALLY_OCCUPIED"
})
smach.StateMachine.add(
'SAY_SEAT_PARTIALLY_OCCUPIED',
states.SayFormatted(robot, [
"I think there's some space left here where you can sit {name}"
],
name=seat_is_for,
block=True),
transitions={'spoken': 'RESET_SUCCESS'})
smach.StateMachine.add(
'SAY_NO_EMPTY_SEATS',
states.SayFormatted(robot, [
"Sorry, there are no empty seats. I guess you just have to stand {name}"
],
name=seat_is_for,
block=True),
transitions={'spoken': 'RESET_FAIL'})
smach.StateMachine.add('RESET_FAIL',
states.ResetArms(robot),
transitions={'done': 'failed'})
smach.StateMachine.add('RESET_SUCCESS',
states.ResetArms(robot),
transitions={'done': 'succeeded'})