def __init__(self, robot, furniture_designator):
""" Constructor
:param robot: robot object
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"])
with self:
smach.StateMachine.add("INSPECT_FURNITURE",
states.Inspect(robot, furniture_designator),
transitions={
"done": "PICKUP_FROM_FURNITURE",
"failed": "PICKUP_FROM_FURNITURE"
})
# ToDo: move GrabSingleItem to robot smach states
smach.StateMachine.add(
"PICKUP_FROM_FURNITURE", # Pickup the thing standing on the furniture
GrabSingleItem(
robot,
DefaultGrabDesignator(robot, furniture_designator,
"on_top_of")),
transitions={
"succeeded": "succeeded",
"failed": "failed"
})
def __init__(self, robot, supporting_entity_designator, arm_designator):
smach.StateMachine.__init__(
self, outcomes=['succeeded', 'inspect_failed', 'grasp_failed'])
self._robot = robot
self._supporting_entity_designator = supporting_entity_designator
self._arm_designator = arm_designator
self._classification_result_designator = VariableDesignator(
[], resolve_type=[ClassificationResult])
self._grasp_entity_designator = VariableDesignator(resolve_type=Entity)
with self:
smach.StateMachine.add(
'INSPECT',
states.Inspect(
robot=self._robot,
entityDes=self._supporting_entity_designator,
objectIDsDes=self._classification_result_designator,
searchArea="on_top_of",
navigation_area="in_front_of"),
transitions={
'done': 'SELECT_EASIEST_GRASP_ENTITY',
'failed': 'inspect_failed'
})
smach.StateMachine.add(
'SELECT_EASIEST_GRASP_ENTITY',
SelectEasiestGraspEntity(robot=self._robot,
classification_result_designator=self.
_classification_result_designator,
grasp_entity_designator=self.
_grasp_entity_designator.writeable),
transitions={
'selected': 'GRAB',
'failed': 'inspect_failed'
})
smach.StateMachine.add('GRAB',
states.Grab(
robot=self._robot,
item=self._grasp_entity_designator,
arm=self._arm_designator),
transitions={
'done': 'succeeded',
'failed': 'grasp_failed'
})
def __init__(self, robot, where_to_count_designator,
type_to_count_designator, count_designator):
"""
:param robot: robot object
:param where_to_count_designator: Where to look for objects?
:param type_to_count_designator: a VariableDesignator (resolve_type=string) that stores the type of the objects
that should be counted
:param count_designator: VariableDesignator(resolve_type=int).writeable() that will store the number
of objects
"""
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
entities = ds.VariableDesignator([],
resolve_type=[ClassificationResult])
with self:
smach.StateMachine.add("INSPECT_TABLE",
states.Inspect(
robot=robot,
entityDes=where_to_count_designator,
objectIDsDes=entities,
searchArea="on_top_of",
navigation_area="in_front_of"),
transitions={
"done": "COUNT",
"failed": "Aborted"
})
smach.StateMachine.add(
"COUNT",
CountObjectsOnLocation(
robot,
location=where_to_count_designator,
segmented_objects_designator=entities,
object_type=type_to_count_designator,
num_objects_designator=count_designator.writeable),
transitions={
'done': 'Done',
'failed': 'Aborted'
})
def __init__(self,
robot,
grab_designator_1=None,
grab_designator_2=None,
place_designator=None,
pdf_writer=None):
""" Constructor
:param robot: robot object
:param grab_designator_1: EdEntityDesignator designating the item to grab
:param grab_designator_2: EdEntityDesignator designating the item to grab
:param pdf_writer: WritePDF object to save images of recognized objects to pdf files
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"])
# Create designators
self._table_designator = ds.EntityByIdDesignator(robot, id=TABLE)
if grab_designator_1 is None:
grab_designator_1 = DefaultGrabDesignator(
robot=robot,
surface_designator=self._table_designator,
area_description=GRAB_SURFACE)
if grab_designator_2 is None:
grab_designator_2 = DefaultGrabDesignator(
robot=robot,
surface_designator=self._table_designator,
area_description=GRAB_SURFACE)
with self:
smach.StateMachine.add("MOVE_TO_TABLE1",
states.NavigateToSymbolic(
robot,
{self._table_designator: "in_front_of"},
self._table_designator),
transitions={
'arrived': 'INSPECT_TABLE',
'unreachable': 'MOVE_TO_TABLE2',
'goal_not_defined': 'INSPECT_TABLE'
})
smach.StateMachine.add(
"MOVE_TO_TABLE2",
states.NavigateToSymbolic(
robot, {self._table_designator: "large_in_front_of"},
self._table_designator),
transitions={
'arrived': 'INSPECT_TABLE',
'unreachable': 'INSPECT_TABLE',
'goal_not_defined': 'INSPECT_TABLE'
})
if pdf_writer:
# Designator to store the classificationresults
class_designator = ds.VariableDesignator(
[],
resolve_type=[
robot_skills.classification_result.ClassificationResult
])
# Add the designator to the pdf writer state
pdf_writer.set_designator(class_designator)
smach.StateMachine.add("INSPECT_TABLE",
states.Inspect(
robot=robot,
entityDes=self._table_designator,
objectIDsDes=class_designator,
searchArea=GRAB_SURFACE,
navigation_area="in_front_of"),
transitions={
"done": "WRITE_PDF",
"failed": "failed"
})
smach.StateMachine.add("WRITE_PDF",
pdf_writer,
transitions={"done": "GRAB_ITEM_1"})
else:
smach.StateMachine.add("INSPECT_TABLE",
states.Inspect(
robot=robot,
entityDes=self._table_designator,
objectIDsDes=None,
searchArea=GRAB_SURFACE,
inspection_area="in_front_of"),
transitions={
"done": "GRAB_ITEM_1",
"failed": "failed"
})
smach.StateMachine.add("GRAB_ITEM_1",
GrabSingleItem(
robot=robot,
grab_designator=grab_designator_1),
transitions={
"succeeded": "GRAB_ITEM_2",
"failed": "GRAB_ITEM_2"
})
smach.StateMachine.add("GRAB_ITEM_2",
GrabSingleItem(
robot=robot,
grab_designator=grab_designator_2),
transitions={
"succeeded": "MOVE_TO_PLACE",
"failed": "MOVE_TO_PLACE"
})
cabinet = ds.EntityByIdDesignator(robot, id=CABINET)
smach.StateMachine.add("MOVE_TO_PLACE",
states.NavigateToSymbolic(
robot, {cabinet: "in_front_of"},
cabinet),
transitions={
'arrived': 'PLACE_ITEM_1',
'unreachable': 'PLACE_ITEM_1',
'goal_not_defined': 'PLACE_ITEM_1'
})
smach.StateMachine.add("PLACE_ITEM_1",
PlaceSingleItem(
robot=robot,
place_designator=place_designator),
transitions={
"succeeded": "PLACE_ITEM_2",
"failed": "PLACE_ITEM_2"
})
smach.StateMachine.add("PLACE_ITEM_2",
PlaceSingleItem(
robot=robot,
place_designator=place_designator),
transitions={
"succeeded": "succeeded",
"failed": "failed"
})
def __init__(self, robot, grasp_designator1, grasp_designator2,
grasp_designator3, grasp_furniture_id1, grasp_furniture_id3,
place_furniture_id):
""" Constructor
:param robot: robot object
:param grasp_designator1: EdEntityDesignator designating the first item to grab.
:param grasp_designator2: EdEntityDesignator designating the second item to grab.
:param grasp_designator3: EdEntityDesignator designating the third item to grab.
:param grasp_furniture_id1: string identifying the location where to grasp objects 1 and 2
:param grasp_furniture_id3: string identifying the location where to grasp object 3
:param place_furniture_id: string identifying the location where to place the objects
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"])
# Create designators
grasp_furniture_designator1 = ds.EntityByIdDesignator(
robot, id=grasp_furniture_id1)
grasp_furniture_designator3 = ds.EntityByIdDesignator(
robot, id=grasp_furniture_id3)
place_furniture_designator = ds.EntityByIdDesignator(
robot, id=place_furniture_id)
place_designator = ds.EmptySpotDesignator(
robot=robot,
place_location_designator=place_furniture_designator,
area="on_top_of")
with self:
# Move to the inspect location
smach.StateMachine.add(
"MOVE_TO_GRASP_SURFACE1",
states.NavigateToSymbolic(
robot, {grasp_furniture_designator1: "in_front_of"},
grasp_furniture_designator1),
transitions={
'arrived': 'INSPECT_GRASP_SURFACE',
'unreachable': 'MOVE_TO_GRASP_SURFACE2',
'goal_not_defined': 'INSPECT_GRASP_SURFACE'
})
# Backup for moving to inspect location
smach.StateMachine.add(
"MOVE_TO_GRASP_SURFACE2",
states.NavigateToSymbolic(
robot, {grasp_furniture_designator1: "large_in_front_of"},
grasp_furniture_designator1),
transitions={
'arrived': 'INSPECT_GRASP_SURFACE',
'unreachable': 'INSPECT_GRASP_SURFACE',
'goal_not_defined': 'INSPECT_GRASP_SURFACE'
})
# Inspect grasp furniture
smach.StateMachine.add("INSPECT_GRASP_SURFACE",
states.Inspect(
robot=robot,
entityDes=grasp_furniture_designator1,
objectIDsDes=None,
searchArea="on_top_of",
navigation_area="in_front_of"),
transitions={
"done": "GRAB_ITEM_1",
"failed": "failed"
})
# Grasp the first item
smach.StateMachine.add("GRAB_ITEM_1",
GrabSingleItem(
robot=robot,
grab_designator=grasp_designator1),
transitions={
"succeeded": "GRAB_ITEM_2",
"failed": "GRAB_ITEM_2"
})
# Grasp the second item
smach.StateMachine.add("GRAB_ITEM_2",
GrabSingleItem(
robot=robot,
grab_designator=grasp_designator2),
transitions={
"succeeded": "MOVE_TO_PLACE",
"failed": "MOVE_TO_PLACE"
})
# Move to the place location
smach.StateMachine.add(
"MOVE_TO_PLACE",
states.NavigateToSymbolic(
robot, {place_furniture_designator: "in_front_of"},
place_furniture_designator),
transitions={
'arrived': 'PLACE_ITEM_1',
'unreachable': 'PLACE_ITEM_1',
'goal_not_defined': 'PLACE_ITEM_1'
})
# Place the first item
smach.StateMachine.add("PLACE_ITEM_1",
PlaceSingleItem(
robot=robot,
place_designator=place_designator),
transitions={
"succeeded": "PLACE_ITEM_2",
"failed": "PLACE_ITEM_2"
})
# Place the second item
smach.StateMachine.add("PLACE_ITEM_2",
PlaceSingleItem(
robot=robot,
place_designator=place_designator),
transitions={
"succeeded": "MOVE_TO_GRASP_SURFACE3",
"failed": "MOVE_TO_GRASP_SURFACE3"
})
# Move back to the grasp surface to grasp the third item
smach.StateMachine.add(
"MOVE_TO_GRASP_SURFACE3",
states.NavigateToSymbolic(
robot, {grasp_furniture_designator3: "in_front_of"},
grasp_furniture_designator3),
transitions={
'arrived': 'INSPECT_GRASP_SURFACE2',
'unreachable': 'MOVE_TO_GRASP_SURFACE4',
'goal_not_defined': 'INSPECT_GRASP_SURFACE2'
})
# Backup for moving back to the grasp location
smach.StateMachine.add(
"MOVE_TO_GRASP_SURFACE4",
states.NavigateToSymbolic(
robot, {grasp_furniture_designator3: "large_in_front_of"},
grasp_furniture_designator3),
transitions={
'arrived': 'INSPECT_GRASP_SURFACE2',
'unreachable': 'INSPECT_GRASP_SURFACE2',
'goal_not_defined': 'INSPECT_GRASP_SURFACE2'
})
# Inspect grasp furniture
smach.StateMachine.add("INSPECT_GRASP_SURFACE2",
states.Inspect(
robot=robot,
entityDes=grasp_furniture_designator3,
objectIDsDes=None,
searchArea="shelf2",
navigation_area="in_front_of"),
transitions={
"done": "GRAB_ITEM_3",
"failed": "failed"
})
# Grasp the third item
smach.StateMachine.add("GRAB_ITEM_3",
GrabSingleItem(
robot=robot,
grab_designator=grasp_designator3),
transitions={
"succeeded": "MOVE_TO_PLACE_3",
"failed": "MOVE_TO_PLACE_3"
})
# Move to the place location
smach.StateMachine.add(
"MOVE_TO_PLACE_3",
states.NavigateToSymbolic(
robot, {place_furniture_designator: "in_front_of"},
place_furniture_designator),
transitions={
'arrived': 'PLACE_ITEM_3',
'unreachable': 'PLACE_ITEM_3',
'goal_not_defined': 'PLACE_ITEM_3'
})
# Place the first item
smach.StateMachine.add("PLACE_ITEM_3",
PlaceSingleItem(
robot=robot,
place_designator=place_designator),
transitions={
"succeeded": "succeeded",
"failed": "succeeded"
})
def __init__(self, robot):
# type: (Robot) -> str
"""
Initialization method
:param robot: robot api object
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"])
# Designators
bar_designator = ds.EdEntityDesignator(robot=robot, id=challenge_knowledge.bar_id, name='bar_des')
room_designator = ds.EdEntityDesignator(robot=robot, id=challenge_knowledge.room_id, name='room_des')
objects_list_des = ds.VariableDesignator(resolve_type=[ClassificationResult], name='objects_list_des')
unav_drink_des = ds.VariableDesignator(resolve_type=str, name='unav_drink_str_des')
hacky_arm_des = ds.VariableDesignator(initial_value=robot.get_arm(), name='hacky_arm')
with self:
smach.StateMachine.add("INITIALIZE",
states.Initialize(robot=robot),
transitions={"initialized": "INITIAL_POSE",
"abort": "aborted"})
smach.StateMachine.add("INITIAL_POSE",
states.SetInitialPose(robot,
challenge_knowledge.starting_point),
transitions={"done": "INSPECT_BAR",
"preempted": "aborted",
"error": "INSPECT_BAR"})
# Inspect bar and store the list of available drinks
smach.StateMachine.add("INSPECT_BAR",
states.Inspect(robot=robot,
entityDes=bar_designator,
navigation_area="in_front_of",
objectIDsDes=objects_list_des),
transitions={"done": "INSPECT_FALLBACK", #TODO: Change to CHECK_INSPECT_RESULT after RWC2019
"failed": "INSPECT_FALLBACK"})
smach.StateMachine.add("CHECK_INSPECT_RESULT",
CheckInspect(objects_list_des,
[ClassificationResult]),
transitions={"true": "IDENTIFY_UNAVAILABLE_DRINK",
"false": "INSPECT_FALLBACK"})
smach.StateMachine.add("IDENTIFY_UNAVAILABLE_DRINK",
IdentifyUnavailableDrinkFromRecognitions(objects=common_knowledge.objects,
classification_list_designator=objects_list_des,
unavailable_drink_designator=unav_drink_des.writeable,
max_unavailable_drinks=challenge_knowledge.MAX_UNAVAILABLE_DRINKS),
transitions={"done": "NAVIGATE_TO_ROOM",
"failed": "INSPECT_FALLBACK"})
# Inspect fallback - ask the bartender which drink is unavailable and store the unavailable drink
smach.StateMachine.add("INSPECT_FALLBACK",
AskAvailability(robot=robot,
unavailable_drink_designator=unav_drink_des.writeable,
objects=common_knowledge.objects),
transitions={"succeeded": "RESET_ROBOT",
"failed": "RESET_ROBOT"})
smach.StateMachine.add("RESET_ROBOT",
states.ArmToJointConfig(robot=robot,
arm_designator=hacky_arm_des,
configuration="reset"),
transitions={'succeeded': "NAVIGATE_TO_ROOM",
'failed': "NAVIGATE_TO_ROOM"})
# Navigate to the predefined room
smach.StateMachine.add("NAVIGATE_TO_ROOM",
states.NavigateToRoom(robot=robot, entity_designator_room=room_designator),
transitions={"arrived": "SAY_HI",
"unreachable": "SAY_HI",
"goal_not_defined": "aborted"})
smach.StateMachine.add("SAY_HI",
states.Say(robot, "Hi, I am {}. I'll be your waiter today".format(robot.robot_name)),
transitions={"spoken": "SERVE_DRINK_1"})
# Explicitly add a new state for each drink, i.e., don't use a range iterator to make sure a new state
# is constructed every time
for idx in range(1, challenge_knowledge.NR_DRINKS + 1):
next_state = "SERVE_DRINK_{}".format(idx + 1) if idx < challenge_knowledge.NR_DRINKS else "SAY_DONE"
smach.StateMachine.add("SERVE_DRINK_{}".format(idx),
ServeOneDrink(robot=robot,
bar_designator=bar_designator,
room_id=challenge_knowledge.room_id,
room_designator=room_designator,
objects_list_des=objects_list_des,
unav_drink_des=unav_drink_des,
name_options=common_knowledge.names,
objects=common_knowledge.objects),
transitions={"succeeded": next_state,
"failed": next_state,
"aborted": next_state})
smach.StateMachine.add("SAY_DONE",
states.Say(robot, "My job here is done. Enjoy your day and see you next time"),
transitions={"spoken": "succeeded"})
def __init__(self, robot, bar_designator, room_id, room_designator,
objects_list_des, unav_drink_des, name_options, objects):
"""
Initialization method
:param robot: robot api object
:param bar_designator: (EntityDesignator) in which the bar location is stored
:param room_id: room ID from challenge knowledge
:param room_designator: (EntityDesignator) in which the room location is stored
:param objects_list_des: (VariableDesignator) in which the available drinks are stored
:param unav_drink_des: (VariableDesignator) in which the unavailable drink is stored
:param name_options: Names from common knowledge
:param objects: Objects from common knowledge
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"])
# Designators
arm_designator = ds.UnoccupiedArmDesignator(robot=robot,
arm_properties={},
name='arm_des').lockable()
drink_str_designator = ds.VariableDesignator(resolve_type=str, name='drink_str_des')
drink_designator = ds.EdEntityDesignator(robot=robot, type_designator=drink_str_designator, name='drink_des')
operator_name = ds.VariableDesignator(resolve_type=str, name='name_des')
operator_designator = ds.VariableDesignator(resolve_type=Entity, name='operator_des')
learn_check_designator = ds.VariableDesignator(initial_value=True, resolve_type=bool, name='learn_check_des')
hacky_arm_des = ds.VariableDesignator(initial_value=robot.get_arm(), name='hacky_arm_2')
with self:
# Lock the arm_designator
smach.StateMachine.add("LOCK_ARM",
states.LockDesignator(arm_designator),
transitions={'locked': "GET_ORDER"})
# Get order
smach.StateMachine.add("GET_ORDER",
GetOrder(robot=robot,
operator_name=operator_name,
drink_designator=drink_str_designator,
available_drinks_designator=objects_list_des,
unavailable_drink_designator=unav_drink_des,
name_options=name_options,
objects=objects,
learn_check_designator=learn_check_designator.writeable,
target_room_designator=room_designator),
transitions={"succeeded": "INSPECT_BAR",
"failed": "failed",
"aborted": "aborted"})
# Inspect bar
smach.StateMachine.add("INSPECT_BAR",
states.Inspect(robot=robot, entityDes=bar_designator, navigation_area="in_front_of"),
transitions={"done": "GRASP_DRINK",
"failed": "FALLBACK_BAR"})
# Grasp drink
smach.StateMachine.add("GRASP_DRINK",
states.Grab(robot=robot, item=drink_designator, arm=arm_designator),
transitions={"done": "FIND_OPERATOR",
"failed": "FALLBACK_BAR"})
# Inspect or grasp fallback - ask for assistance
smach.StateMachine.add("FALLBACK_BAR",
states.Say(robot=robot,
sentence=DescriptionStrDesignator("fallback_bar", drink_str_designator,
operator_name),
look_at_standing_person=True),
transitions={"spoken": "HANDOVER_FROM_HUMAN"})
# Handover from human fallback
smach.StateMachine.add("HANDOVER_FROM_HUMAN",
states.HandoverFromHuman(robot=robot, arm_designator=arm_designator,
grabbed_entity_designator=drink_designator),
transitions={"succeeded": "RESET_ROBOT_2",
"failed": "RESET_ROBOT_2",
"timeout": "RESET_ROBOT_2"})
smach.StateMachine.add("RESET_ROBOT_2",
states.ArmToJointConfig(robot=robot,
arm_designator=hacky_arm_des,
configuration="reset"),
transitions={'succeeded': "CHECK_LEARN_OPERATOR",
'failed': "CHECK_LEARN_OPERATOR"})
smach.StateMachine.add("CHECK_LEARN_OPERATOR",
states.CheckBool(learn_check_designator),
transitions={"true": "FIND_OPERATOR",
"false": "GO_TO_ROOM"})
smach.StateMachine.add("GO_TO_ROOM",
states.NavigateToRoom(robot=robot,
entity_designator_room=room_designator),
transitions={"arrived": "SAY_NOT_FOUND",
"unreachable": "failed",
"goal_not_defined": "aborted"})
# Find operator
smach.StateMachine.add("FIND_OPERATOR",
states.FindPersonInRoom(robot=robot,
area=room_id,
name=operator_name,
discard_other_labels=True,
found_entity_designator=operator_designator.writeable),
transitions={"found": "GOTO_OPERATOR",
"not_found": "SAY_NOT_FOUND"})
# Move to this person
smach.StateMachine.add("GOTO_OPERATOR",
states.NavigateToObserve(robot=robot,
entity_designator=operator_designator),
transitions={"arrived": "SAY_THE_NAME",
"unreachable": "SAY_NOT_FOUND",
"goal_not_defined": "SAY_NOT_FOUND"})
# Say not found
smach.StateMachine.add("SAY_NOT_FOUND",
states.Say(robot=robot,
sentence=DescriptionStrDesignator("not_found_operator",
drink_str_designator,
operator_name),
look_at_standing_person=True),
transitions={"spoken": "RISE_FOR_HMI_2"})
# Say the name
smach.StateMachine.add("SAY_THE_NAME",
states.Say(robot=robot,
sentence=DescriptionStrDesignator("found_operator",
drink_str_designator,
operator_name),
look_at_standing_person=True),
transitions={"spoken": "RISE_FOR_HMI_2"})
smach.StateMachine.add("RISE_FOR_HMI_2",
states.RiseForHMI(robot=robot),
transitions={"succeeded": "HAND_OVER",
"failed": "HAND_OVER"})
# Hand over the drink to the operator
smach.StateMachine.add("HAND_OVER",
states.HandoverToHuman(robot=robot,
arm_designator=arm_designator),
transitions={"succeeded": "UNLOCK_ARM",
"failed": "UNLOCK_ARM"})
smach.StateMachine.add("UNLOCK_ARM",
states.UnlockDesignator(arm_designator),
transitions={'unlocked': "RESET_ROBOT_3"})
smach.StateMachine.add("RESET_ROBOT_3",
states.ArmToJointConfig(robot=robot,
arm_designator=hacky_arm_des,
configuration="reset"),
transitions={'succeeded': "RETURN_TO_ROOM",
'failed': "RETURN_TO_ROOM"})
smach.StateMachine.add("RETURN_TO_ROOM",
states.NavigateToRoom(robot=robot,
entity_designator_room=room_designator),
transitions={"arrived": "succeeded",
"unreachable": "failed",
"goal_not_defined": "aborted"})
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, source_location, source_navArea, target_location, target_navArea, target_placeArea="on_top_of", source_searchArea="on_top_of"):
"""
Let the given robot move to a location and remove all entities from that table one at a time
:param robot: Robot to use
:param source_location: Location which will be cleared
:param target_location: Location where the objects will be placed
:return:
"""
smach.StateMachine.__init__(self, outcomes=['done', 'failed'])
# Check types or designator resolve types
#check_type(source_location, Entity)
#check_type(target_location, Entity)
segmented_entities_designator = VariableDesignator([], resolve_type=[ClassificationResult])
selected_entity_designator = EntityByIdDesignator(robot, "TBD", name='selected_entity_designator', )
arm_des = UnoccupiedArmDesignator(robot, {}).lockable()
arm_des.lock()
place_position = states.util.designators.EmptySpotDesignator(robot, EdEntityDesignator(
robot, id=target_location.id),
area="on_top_of"
)
with self:
smach.StateMachine.add('INSPECT_SOURCE_ENTITY',
states.Inspect(robot, source_location, objectIDsDes=segmented_entities_designator,
searchArea=source_searchArea, navigation_area=source_navArea),
transitions={'done': 'DETERMINE_IF_CLEAR',
'failed': 'failed'}
)
#smach.StateMachine.add('DETERMINE_IF_CLEAR',
# isitclear(robot=robot,
# objectIDsDes=segmented_entities_designator),
# transitions={'clear': 'done',
# 'not_clear': 'failed'})
smach.StateMachine.add('DETERMINE_IF_CLEAR',
SelectEntity(robot, segmented_entities_designator, selected_entity_designator),
transitions={'no_entities_left': 'done',
'entity_selected': 'GRAB'}
)
smach.StateMachine.add('GRAB',
states.Grab(robot, selected_entity_designator, arm_des),
transitions={'done': 'INSPECT_TARGET',
'failed': 'failed'}
)
smach.StateMachine.add('INSPECT_TARGET',
states.Inspect(robot, target_location, searchArea=target_placeArea,
navigation_area=target_navArea),
transitions={'done': 'PLACE',
'failed': 'failed'}
)
smach.StateMachine.add('PLACE',
states.Place(robot, selected_entity_designator, place_position, arm_des),
transitions={'done': 'INSPECT_SOURCE_ENTITY',
'failed': 'failed'}
)
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", robot_smach_states.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",
robot_smach_states.Grab(robot, selected_entity_designator,
ds.UnoccupiedArmDesignator(robot, {},
name="empty_arm_designator")),
transitions={"done": "SAY_GRAB_SUCCESS",
"failed": "ARM_RESET"})
smach.StateMachine.add("ARM_RESET", robot_smach_states.ArmToJointConfig(robot,
ds.UnoccupiedArmDesignator(robot,
{},
name="empty_arm_designator"),
"reset"),
transitions={"succeeded": "SAY_GRAB_FAILED",
"failed": "SAY_GRAB_FAILED"})
smach.StateMachine.add('SAY_GRAB_SUCCESS',
robot_smach_states.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',
robot_smach_states.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',
robot_smach_states.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',
robot_smach_states.Inspect(robot,
store_entity_des),
transitions={"done": "PLACE_IN_TRASH",
"failed": "SAY_PLACE_FAILED"})
smach.StateMachine.add('PLACE_IN_TRASH',
robot_smach_states.Place(robot,
selected_entity_designator,
trash_place_pose,
ds.OccupiedArmDesignator(robot, {},
name="occupied_arm_designator")),
transitions={"done": "SAY_PLACE_SUCCESS",
"failed": "SAY_PLACE_FAILED"})
smach.StateMachine.add('PLACE_TO_STORE', robot_smach_states.Place(robot, selected_entity_designator,
store_entity_des,
ds.OccupiedArmDesignator(robot, {}, name=
"occupied_arm_designator"),
"on_top_of"),
transitions={"done": "SAY_PLACE_SUCCESS",
"failed": "SAY_PLACE_FAILED"})
smach.StateMachine.add('SAY_PLACE_SUCCESS',
robot_smach_states.Say(robot, ["Bye bye!",
"Yeah!",
"Successfully disposed the item",
"Another score for HERO"], block=False),
transitions={"spoken": "CHECK_ARM_OCCUPIED"})
smach.StateMachine.add('SAY_PLACE_FAILED',
robot_smach_states.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"})
