def __init__(self, robot):
smach.StateMachine.__init__(
self, outcomes=['succeeded', 'failed', 'aborted'])
self.door_waypoint = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.waypoint_door['id'])
self.livingroom_waypoint = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.waypoint_livingroom['id'])
self.operator_designator = ds.VariableDesignator(
resolve_type=Entity)
self.guest1_entity_des = ds.VariableDesignator(
resolve_type=Entity, name='guest1_entity')
self.guest1_name_des = ds.VariableDesignator('guest 1',
name='guest1_name')
self.guest1_drink_des = ds.VariableDesignator(
resolve_type=HMIResult, name='guest1_drink')
self.guest1_drinkname_des = ds.FieldOfHMIResult(
self.guest1_drink_des,
semantics_field='drink',
name='guest1_drinkname')
with self:
smach.StateMachine.add('LEARN_GUEST',
LearnGuest(robot, self.door_waypoint,
self.guest1_entity_des,
self.guest1_name_des,
self.guest1_drink_des),
transitions={
'succeeded': 'succeeded',
'failed': 'failed',
'aborted': 'aborted'
})
def __init__(self, robot, area, name):
""" Constructor
:param robot: robot object
:param area: (str) if a waypoint "<area>_waypoint" is present in the world model, the robot will navigate
to this waypoint. Else, it will navigate to the room called "<area>"
:param name: (str) Name of the person to look for
"""
smach.StateMachine.__init__(self, outcomes=["found", "not_found"])
waypoint_designator = ds.EntityByIdDesignator(robot=robot,
id=area + "_waypoint")
room_designator = ds.EntityByIdDesignator(robot=robot, id=area)
with self:
smach.StateMachine.add("DECIDE_NAVIGATE_STATE",
_DecideNavigateState(
robot=robot,
waypoint_designator=waypoint_designator,
room_designator=room_designator),
transitions={
"waypoint": "NAVIGATE_TO_WAYPOINT",
"room": "NAVIGATE_TO_ROOM",
"none": "not_found"
})
smach.StateMachine.add("NAVIGATE_TO_WAYPOINT",
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=waypoint_designator,
radius=0.15),
transitions={
"arrived": "FIND_PERSON",
"unreachable": "not_found",
"goal_not_defined": "not_found"
})
smach.StateMachine.add("NAVIGATE_TO_ROOM",
states.NavigateToRoom(
robot=robot,
entity_designator_room=room_designator),
transitions={
"arrived": "FIND_PERSON",
"unreachable": "not_found",
"goal_not_defined": "not_found"
})
# Wait for the operator to appear and detect what he's pointing at
smach.StateMachine.add("FIND_PERSON",
FindPerson(robot=robot, person_label=name),
transitions={
"found": "found",
"failed": "not_found"
})
def __init__(self, robot, room, found_people_designator,
look_range=(-np.pi/2, np.pi/2),
look_steps=8):
"""
Constructor
:param robot: robot object
:param area: (str) if a waypoint "<area>_waypoint" is present in the world model, the robot will navigate
to this waypoint. Else, it will navigate to the room called "<area>"
:param name: (str) Name of the person to look for
:param discard_other_labels: (bool) Whether or not to discard faces based on label
:param found_person_designator: (Designator) A designator that will resolve to the found object
"""
smach.StateMachine.__init__(self, outcomes=["found", "not_found"])
waypoint_designator = ds.EntityByIdDesignator(robot=robot, id=room + "_waypoint")
room_designator = ds.EntityByIdDesignator(robot=robot, id=room)
ds.check_type(found_people_designator, [Entity])
ds.is_writeable(found_people_designator)
with self:
smach.StateMachine.add("DECIDE_NAVIGATE_STATE",
_DecideNavigateState(robot=robot, waypoint_designator=waypoint_designator,
room_designator=room_designator),
transitions={"waypoint": "NAVIGATE_TO_WAYPOINT",
"room": "NAVIGATE_TO_ROOM",
"none": "not_found"})
smach.StateMachine.add("NAVIGATE_TO_WAYPOINT",
states.NavigateToWaypoint(robot=robot,
waypoint_designator=waypoint_designator, radius=0.15),
transitions={"arrived": "FIND_PEOPLE",
"unreachable": "not_found",
"goal_not_defined": "not_found"})
smach.StateMachine.add("NAVIGATE_TO_ROOM", states.NavigateToRoom(robot=robot,
entity_designator_room=room_designator),
transitions={"arrived": "FIND_PEOPLE",
"unreachable": "not_found",
"goal_not_defined": "not_found"})
# Wait for the operator to appear and detect what he's pointing at
smach.StateMachine.add("FIND_PEOPLE",
FindPeople(robot=robot,
query_entity_designator=room_designator,
found_people_designator=found_people_designator,
speak=True,
look_range=look_range,
look_steps=look_steps),
transitions={"found": "found",
"failed": "not_found"})
def __init__(self, robot, cabinet_id, cabinet_navigate_area,
cabinet_inspect_area):
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"])
self.cabinet = ds.EntityByIdDesignator(robot=robot, id=cabinet_id)
with self:
smach.StateMachine.add("NAVIGATE_TO_CABINET",
states.NavigateToSymbolic(
robot,
{self.cabinet: cabinet_navigate_area},
self.cabinet),
transitions={
'arrived': 'UPDATE_CABINET_POSE',
'unreachable': 'failed',
'goal_not_defined': 'failed'
})
smach.StateMachine.add("UPDATE_CABINET_POSE",
UpdateCabinetPose(robot, self.cabinet,
cabinet_inspect_area),
transitions={
'succeeded': 'OPEN_DOOR',
'failed': 'failed'
})
smach.StateMachine.add("OPEN_DOOR",
OpenDoor(robot, self.cabinet),
transitions={
'succeeded': 'succeeded',
'failed': 'failed'
})
def grab(p, robot):
entities = parse_object(p, robot)
if not entities:
say("No such entities!")
return
# Only filter to entities that do not have a shape but do have a convex hull
entities = [
e for e in entities if not e.has_shape and len(e.convex_hull) > 0
]
if not entities:
say("I am sorry, but I cannot grab that object")
return
arm = robot.leftArm
machine = Grab(robot,
arm=ds.UnoccupiedArmDesignator(robot.arms, arm),
item=ds.EntityByIdDesignator(robot, id=entities[0].id))
stop()
global ACTION
ACTION = SmachAction(machine)
ACTION.start()
def arm_left_grab(pre):
global arm_left_object_in_arm
global task_object_grabbed
global left_grab_exec_object_in_WS
classification_result_designator = ds.VariableDesignator([], resolve_type=[ClassificationResult])
# Inspect the waypoint and classify objects on it
segmSM = SegmentObjects(robot, classification_result_designator.writeable, wp)
status = segmSM.execute()
grab_obj = ds.VariableDesignator(resolve_type=Entity)
# take the first seen coke
for obj in classification_result_designator.resolve():
if obj.type == 'coke':
grab_obj = ds.EntityByIdDesignator(robot, type=obj.id)
break
grabSM = Grab(robot, grab_obj, leftarm)
status = grabSM.execute()
if status == 'done':
arm_left_object_in_arm = True
task_object_grabbed = True
# left_grab_exec_object_in_WS = 0
else:
arm_left_object_in_arm = False # RETRY
task_object_grabbed = False
Amigo_AssignInputVariables()
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted'])
runs = ds.Designator([0, 1])
run = ds.VariableDesignator(resolve_type=int)
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': 'HANDLE_GUEST_1',
"preempted": 'aborted',
'error': 'HANDLE_GUEST_1'
})
smach.StateMachine.add('HANDLE_GUEST_1',
HandleSingleGuest(robot, assume_john=True),
transitions={
'succeeded': 'HANDLE_GUEST_2',
'aborted': 'HANDLE_GUEST_2'
})
smach.StateMachine.add('HANDLE_GUEST_2',
HandleSingleGuest(robot, assume_john=False),
transitions={
'succeeded': 'SAY_DONE',
'aborted': 'SAY_DONE'
})
smach.StateMachine.add(
'SAY_DONE',
states.Say(robot,
["That's all folks, my job is done, bye bye!"],
block=False),
transitions={'spoken': 'GO_BACK'})
smach.StateMachine.add(
'GO_BACK',
states.NavigateToWaypoint(
robot,
ds.EntityByIdDesignator(
robot, id=challenge_knowledge.waypoint_door['id']),
challenge_knowledge.waypoint_door['radius']),
transitions={
'arrived': 'succeeded',
'unreachable': 'succeeded',
'goal_not_defined': 'succeeded'
})
def setup_statemachine(robot):
entity = ds.EntityByIdDesignator(robot, id='hallway_couch')
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
with sm:
smach.StateMachine.add('TEST',
LookAtEntity(robot, entity),
transitions={'succeeded': 'Done'})
return sm
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 execute(self, userdata):
target_waypoint = challenge_knowledge.waypoints[userdata.target_room]['id']
target_radius = challenge_knowledge.waypoints[userdata.target_room]['radius']
navigateToWaypoint = states.NavigateToWaypoint(self._robot,
ds.EntityByIdDesignator(self._robot,
id=target_waypoint),
target_radius)
return navigateToWaypoint.execute()
def move(p, robot):
entities = parse_object(p, robot)
if not entities:
say("No such entities!")
return
machine = NavigateToObserve(robot, entity_designator=ds.EntityByIdDesignator(robot, id=entities[0].id), radius=.5)
stop()
global ACTION
ACTION = SmachAction(machine)
ACTION.start()
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['Done'])
with sm:
# Start challenge via StartChallengeRobust, skipped atm
smach.StateMachine.add(
"RAYTRACEDEMO",
RayTraceSelector(robot,
waypoint="final_challenge",
furniture_designator=ds.EntityByIdDesignator(
robot, id="temp")),
transitions={
outcome: "Done"
for outcome in ["waypoint", "furniture", "grasp", "done"]
})
return sm
def __init__(self, robot):
"""
Drives to the lightsaber pose and starts the lightsaber state.
"""
smach.StateMachine.__init__(self, outcomes=["done"])
with self:
smach.StateMachine.add(
"NAVIGATE_TO_START",
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=ds.EntityByIdDesignator(
robot, id=LIGHTSABER_WAYPOINT_ID),
),
transitions={
"arrived": "SWORDFIGHT",
"unreachable": "SWORDFIGHT", # Just take it from here
"goal_not_defined": "SWORDFIGHT"
}) # Just take it from here
smach.StateMachine.add("SWORDFIGHT",
LightSaber(robot),
transitions={"done": "done"})
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# start_waypoint = ds.EntityByIdDesignator(robot, id="manipulation_init_pose", name="start_waypoint")
pdf_writer = WritePdf(robot=robot)
with self:
single_item = ManipulateMachine(robot, pdf_writer=pdf_writer)
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized':
'SAY_UNABLE_TO_OPEN_DOOR',
'abort': 'Aborted'
})
smach.StateMachine.add('SAY_UNABLE_TO_OPEN_DOOR',
states.Say(
robot,
"I am unable to open the shelf door, "
"can you please open it for me?"),
transitions={'spoken': 'AWAIT_START'})
smach.StateMachine.add("AWAIT_START",
states.AskContinue(robot),
transitions={
'continue': "MOVE_TABLE",
'no_response': 'AWAIT_START'
})
cabinet = ds.EntityByIdDesignator(robot, id=CABINET)
@smach.cb_interface(outcomes=["done"])
def move_table(userdata=None, manipulate_machine=None):
""" Moves the entities for this challenge to the correct poses"""
# Determine where to perform the challenge
# Apparently, the indices here come from:
# (cabinet_pose, table_pose, cabinet_amcl, grasp_surface, room, default_place_area)
robot_pose = robot.base.get_location()
WORKSPACES.sort(
key=lambda ws: (ws.place_entity_conf.pose_estimate.frame.p
- robot_pose.frame.p).Norm())
closest_workspace = WORKSPACES[0]
rospy.loginfo(
"Closest workspace: grasp from '{grasp}' and place on '{place}'"
.format(
grasp=closest_workspace.grasp_entity_conf.entity_id,
place=closest_workspace.place_entity_conf.entity_id))
cabinet_id = closest_workspace.place_entity_conf.entity_id
table_id = closest_workspace.grasp_entity_conf.entity_id
# Update the world model by fitting the entities to the frame_stamped's given below.
robot.ed.update_entity(id=cabinet_id,
frame_stamped=closest_workspace.
place_entity_conf.pose_estimate)
robot.ed.update_entity(id=table_id,
frame_stamped=closest_workspace.
grasp_entity_conf.pose_estimate)
# Update designators
cabinet.id_ = closest_workspace.place_entity_conf.entity_id
# Update manipulate machine
manipulate_machine.table_designator.id_ = closest_workspace.grasp_entity_conf.entity_id
manipulate_machine.place_entity_designator.id_ = closest_workspace.place_entity_conf.entity_id
manipulate_machine.place_designator._area = closest_workspace.place_entity_conf.manipulation_volumes[
0]
manipulate_machine.place_designator.place_location_designator.id = closest_workspace.place_entity_conf.entity_id
manipulate_machine.cabinet.id_ = closest_workspace.place_entity_conf.entity_id
return "done"
smach.StateMachine.add("MOVE_TABLE",
smach.CBState(move_table,
cb_args=[single_item]),
transitions={'done': 'NAV_TO_START'})
smach.StateMachine.add("NAV_TO_START",
states.NavigateToSymbolic(
robot, {cabinet: "in_front_of"},
cabinet),
transitions={
'arrived': 'INSPECT_SHELVES',
'unreachable': 'INSPECT_SHELVES',
'goal_not_defined': 'INSPECT_SHELVES'
})
smach.StateMachine.add("INSPECT_SHELVES",
InspectShelves(robot, cabinet),
transitions={
'succeeded': 'WRITE_PDF_SHELVES',
'nothing_found': 'WRITE_PDF_SHELVES',
'failed': 'WRITE_PDF_SHELVES'
})
smach.StateMachine.add("WRITE_PDF_SHELVES",
pdf_writer,
transitions={"done": "RANGE_ITERATOR"})
# Begin setup iterator
# The exhausted argument should be set to the prefered state machine outcome
range_iterator = smach.Iterator(
outcomes=['succeeded',
'failed'], # Outcomes of the iterator state
input_keys=[],
output_keys=[],
it=lambda: range(5),
it_label='index',
exhausted_outcome='succeeded')
with range_iterator:
smach.Iterator.set_contained_state(
'SINGLE_ITEM',
single_item,
loop_outcomes=['succeeded', 'failed'])
smach.StateMachine.add('RANGE_ITERATOR', range_iterator, {
'succeeded': 'AT_END',
'failed': 'Aborted'
})
# End setup iterator
smach.StateMachine.add('AT_END',
states.Say(robot, "Goodbye"),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "manipulation")
# TU/e Robotics
from robot_smach_states.human_interaction import give_directions
import robot_smach_states.util.designators as ds
from robot_skills.get_robot import get_robot
if __name__ == "__main__":
parser = argparse.ArgumentParser(
description=
"Starting from the current robot position and orientation give "
"directions to the desired target object")
parser.add_argument("--robot",
default="hero",
help="Robot name (amigo, hero, sergio)")
parser.add_argument("target_entity_id",
type=str,
help="Entity id of the target object")
args = parser.parse_args()
# Create node
rospy.init_node("test_give_directions")
robot = get_robot(args.robot)
e_id = args.target_entity_id
# Instantiate GuideToSymbolic machine
state = give_directions.GiveDirections(r,
ds.EntityByIdDesignator(r, id=e_id))
# Execute the state
state.execute()
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# start_waypoint = ds.EntityByIdDesignator(robot, id="manipulation_init_pose", name="start_waypoint")
pdf_writer = WritePdf(robot=robot)
with self:
single_item = ManipulateMachine(robot, pdf_writer=pdf_writer)
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized':
'SAY_UNABLE_TO_OPEN_DOOR',
'abort': 'Aborted'
})
smach.StateMachine.add('SAY_UNABLE_TO_OPEN_DOOR',
states.Say(
robot,
"I am unable to open the shelf door, "
"can you please open it for me?"),
transitions={'spoken': 'AWAIT_START'})
smach.StateMachine.add("AWAIT_START",
states.AskContinue(robot),
transitions={
'continue': "MOVE_TABLE",
'no_response': 'AWAIT_START'
})
cabinet = ds.EntityByIdDesignator(robot, id=CABINET)
room = ds.EntityByIdDesignator(robot, id=ROOM)
@smach.cb_interface(outcomes=["done"])
def move_table(userdata=None, manipulate_machine=None):
""" Moves the entities for this challenge to the correct poses"""
# Determine where to perform the challenge
robot_pose = robot.base.get_location()
ENTITY_POSES.sort(key=lambda tup:
(tup[0].frame.p - robot_pose.frame.p).Norm())
cabinet_id = ENTITY_POSES[0][2]
table_id = ENTITY_POSES[0][3]
# Update the world model
robot.ed.update_entity(id="balcony_shelf",
frame_stamped=FrameStamped(
kdl.Frame(kdl.Rotation(),
kdl.Vector(0.0, 3.0,
0.0)),
frame_id="map"))
robot.ed.update_entity(id=cabinet_id,
frame_stamped=ENTITY_POSES[0][0])
robot.ed.update_entity(id=table_id,
frame_stamped=ENTITY_POSES[0][1])
# Update designators
cabinet.id_ = ENTITY_POSES[0][2]
room.id_ = ENTITY_POSES[0][4]
# Update manipulate machine
manipulate_machine.place_entity_designator.id_ = cabinet_id
manipulate_machine.place_designator._area = ENTITY_POSES[0][5]
manipulate_machine.place_designator.place_location_designator.id = cabinet_id
manipulate_machine.table_designator.id_ = table_id
manipulate_machine.cabinet.id_ = ENTITY_POSES[0][2]
return "done"
smach.StateMachine.add("MOVE_TABLE",
smach.CBState(move_table,
cb_args=[single_item]),
transitions={'done': 'NAV_TO_START'})
smach.StateMachine.add("NAV_TO_START",
states.NavigateToSymbolic(
robot, {cabinet: "in_front_of"},
cabinet),
transitions={
'arrived': 'INSPECT_SHELVES',
'unreachable': 'INSPECT_SHELVES',
'goal_not_defined': 'INSPECT_SHELVES'
})
smach.StateMachine.add("INSPECT_SHELVES",
InspectShelves(robot, cabinet),
transitions={
'succeeded': 'WRITE_PDF_SHELVES',
'nothing_found': 'WRITE_PDF_SHELVES',
'failed': 'WRITE_PDF_SHELVES'
})
smach.StateMachine.add("WRITE_PDF_SHELVES",
pdf_writer,
transitions={"done": "RANGE_ITERATOR"})
# Begin setup iterator
# The exhausted argument should be set to the prefered state machine outcome
range_iterator = smach.Iterator(
outcomes=['succeeded',
'failed'], # Outcomes of the iterator state
input_keys=[],
output_keys=[],
it=lambda: range(5),
it_label='index',
exhausted_outcome='succeeded')
with range_iterator:
smach.Iterator.set_contained_state(
'SINGLE_ITEM',
single_item,
loop_outcomes=['succeeded', 'failed'])
smach.StateMachine.add('RANGE_ITERATOR', range_iterator, {
'succeeded': 'AT_END',
'failed': 'Aborted'
})
# End setup iterator
smach.StateMachine.add('AT_END',
states.Say(robot, "Goodbye"),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "manipulation")
def __init__(self, robot, manipulated_items):
"""@param manipulated_items is VariableDesignator that will be a list of items manipulated by the robot."""
self.manipulated_items = manipulated_items
smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
self.cabinet = ds.EntityByIdDesignator(robot,
id=CABINET,
name="pick_shelf")
# self.place_shelf = ds.EntityByIdDesignator(robot, id=PLACE_SHELF, name="place_shelf")
not_manipulated = lambda entity: not entity in self.manipulated_items.resolve(
)
def entity_z_pos(entity):
"""
Checks if the entity is between the minimum and maximum grasp height
:param entity:
:return:
"""
if not entity._pose:
return False
return MIN_GRASP_HEIGHT < entity._pose.p.z() < MAX_GRASP_HEIGHT
# select the entity closest in x direction to the robot in base_link frame
def weight_function(entity):
# TODO: return x coordinate of entity.center_point in base_link frame
epose = entity.pose.projectToFrame(
robot.robot_name + "/base_link",
robot.tf_listener) # Get position in base_link
p = epose.frame.p
return p.x()**2
self.current_item = ds.LockingDesignator(ds.EdEntityDesignator(
robot,
criteriafuncs=[
not_ignored, size, not_manipulated, min_entity_height,
entity_z_pos, max_width
],
weight_function=weight_function,
debug=False,
name="item"),
name="current_item")
#This makes that the empty spot is resolved only once, even when the robot moves. This is important because the sort is based on distance between robot and constraint-area
# self.place_position = ds.LockingDesignator(ds.EmptySpotDesignator(robot, self.cabinet, name="placement", area=PLACE_SHELF), name="place_position")
self.place_position = ds.LockingDesignator(EmptyShelfDesignator(
robot, self.cabinet, name="placement", area=PLACE_SHELF),
name="place_position")
self.empty_arm_designator = ds.UnoccupiedArmDesignator(
robot, {
'required_trajectories': ['prepare_grasp'],
'required_goals': ['carrying_pose'],
'required_gripper_types': [arms.GripperTypes.GRASPING],
'required_arm_name': PREFERRED_ARM
},
name="empty_arm_designator")
self.arm_with_item_designator = ds.ArmHoldingEntityDesignator(
robot, {'required_objects': [self.current_item]}, {
"required_trajectories": ["prepare_place"],
"required_goals": ["reset", "handover_to_human"],
'required_gripper_types': [arms.GripperTypes.GRASPING]
},
name="arm_with_item_designator")
# print "{0} = pick_shelf".format(self.pick_shelf)
# print "{0} = current_item".format(self.current_item)
# print "{0} = place_position".format(self.place_position)
# print "{0} = empty_arm_designator".format(self.empty_arm_designator)
# print "{0} = arm_with_item_designator".format(self.arm_with_item_designator)
with self:
# smach.StateMachine.add( "NAV_TO_OBSERVE_PICK_SHELF",
# #states.NavigateToObserve(robot, self.pick_shelf),
# states.NavigateToSymbolic(robot, {self.pick_shelf:"in_front_of", EntityByIdDesignator(robot, id=ROOM):"in"}, self.pick_shelf),
# transitions={ 'arrived' :'LOOKAT_PICK_SHELF',
# 'unreachable' :'LOOKAT_PICK_SHELF',
# 'goal_not_defined' :'LOOKAT_PICK_SHELF'})
smach.StateMachine.add("REMOVE_ENTITIES",
RemoveSegmentedEntities(robot=robot),
transitions={'done': 'LOOKAT_PICK_SHELF'})
smach.StateMachine.add("LOOKAT_PICK_SHELF",
states.LookAtArea(robot,
self.cabinet,
area=PICK_SHELF),
transitions={'succeeded': 'SEGMENT_SHELF'})
smach.StateMachine.add("SEGMENT_SHELF",
SegmentShelf(robot,
entity_id=CABINET,
area_id=PICK_SHELF),
transitions={'done': 'LOCK_ITEM'})
@smach.cb_interface(outcomes=['locked'])
def lock(userdata=None):
self.current_item.lock(
) #This determines that self.current_item cannot not resolve to a new value until it is unlocked again.
if self.current_item.resolve():
rospy.loginfo("Current_item is now locked to {0}".format(
self.current_item.resolve().id))
self.place_position.lock(
) #This determines that self.place_position will lock/cache its result after its resolved the first time.
return 'locked'
smach.StateMachine.add('LOCK_ITEM',
smach.CBState(lock),
transitions={'locked': 'ANNOUNCE_ITEM'})
smach.StateMachine.add("ANNOUNCE_ITEM",
states.Say(robot,
EntityDescriptionDesignator(
self.current_item,
name="current_item_desc"),
block=False),
transitions={'spoken': 'GRAB_ITEM'})
smach.StateMachine.add("GRAB_ITEM",
Grab(robot, self.current_item,
self.empty_arm_designator),
transitions={
'done': 'STORE_ITEM',
'failed': 'SAY_GRAB_FAILED'
})
smach.StateMachine.add(
"SAY_GRAB_FAILED",
states.Say(robot, ["I couldn't grab this thing"], mood="sad"),
transitions={'spoken': 'UNLOCK_ITEM_AFTER_FAILED_GRAB'}
) # Not sure whether to fail or keep looping with NAV_TO_OBSERVE_PICK_SHELF
@smach.cb_interface(outcomes=['unlocked'])
def unlock_and_ignore(userdata=None):
global ignore_ids
# import ipdb; ipdb.set_trace()
if self.current_item.resolve():
ignore_ids += [self.current_item.resolve().id]
rospy.loginfo("Current_item WAS now locked to {0}".format(
self.current_item.resolve().id))
self.current_item.unlock(
) #This determines that self.current_item can now resolve to a new value on the next call
self.place_position.unlock(
) #This determines that self.place_position can now resolve to a new position on the next call
return 'unlocked'
smach.StateMachine.add('UNLOCK_ITEM_AFTER_FAILED_GRAB',
smach.CBState(unlock_and_ignore),
transitions={'unlocked': 'failed'})
@smach.cb_interface(outcomes=['stored'])
def store_as_manipulated(userdata=None):
# manipulated_items.current += [self.current_item.current]
item_list = manipulated_items.resolve()
item_list += [self.current_item.resolve()]
w = ds.VariableWriter(manipulated_items)
w.write(item_list)
return 'stored'
smach.StateMachine.add(
'STORE_ITEM',
smach.CBState(store_as_manipulated),
transitions={'stored': 'LOOKAT_PLACE_SHELF'})
smach.StateMachine.add("LOOKAT_PLACE_SHELF",
states.LookAtArea(robot,
self.cabinet,
area=PLACE_SHELF),
transitions={'succeeded': 'PLACE_ITEM'})
smach.StateMachine.add("PLACE_ITEM",
Place(robot, self.current_item,
self.place_position,
self.arm_with_item_designator),
transitions={
'done': 'RESET_HEAD_PLACE',
'failed': 'RESET_HEAD_HUMAN'
})
smach.StateMachine.add(
"RESET_HEAD_PLACE",
states.CancelHead(robot),
transitions={'done': 'UNLOCK_ITEM_AFTER_SUCCESSFUL_PLACE'})
smach.StateMachine.add(
"RESET_HEAD_HUMAN",
states.CancelHead(robot),
transitions={'done': 'SAY_HANDOVER_TO_HUMAN'})
smach.StateMachine.add('UNLOCK_ITEM_AFTER_SUCCESSFUL_PLACE',
smach.CBState(unlock_and_ignore),
transitions={'unlocked': 'succeeded'})
smach.StateMachine.add(
"SAY_HANDOVER_TO_HUMAN",
states.Say(robot, [
"I'm can't get rid of this item myself, can somebody help me maybe?"
]),
transitions={'spoken': 'HANDOVER_TO_HUMAN'})
smach.StateMachine.add('HANDOVER_TO_HUMAN',
states.HandoverToHuman(
robot, self.arm_with_item_designator),
transitions={
'succeeded': 'UNLOCK_AFTER_HANDOVER',
'failed': 'UNLOCK_AFTER_HANDOVER'
})
smach.StateMachine.add('UNLOCK_AFTER_HANDOVER',
smach.CBState(unlock_and_ignore),
transitions={'unlocked': 'failed'})
def setup_statemachine(robot):
place_name = ds.EntityByIdDesignator(robot, id=challenge_knowledge.default_place, name="place_name")
place_position = ds.LockingDesignator(ds.EmptySpotDesignator(robot,
place_name,
name="placement",
area=challenge_knowledge.default_area),
name="place_position")
empty_arm_designator = ds.UnoccupiedArmDesignator(robot.arms,
robot.rightArm,
name="empty_arm_designator")
# With the empty_arm_designator locked, it will ALWAYS resolve to the same arm (first resolve is cached), unless it is unlocked
# For this challenge, unlocking is not needed
bag_arm_designator = empty_arm_designator.lockable()
bag_arm_designator.lock()
# We don't actually grab something, so there is no need for an actual thing to grab
current_item = ds.VariableDesignator(Entity("dummy",
"dummy",
"/{}/base_link".format(robot.robot_name),
kdl_conversions.kdlFrameFromXYZRPY(0.6, 0, 0.5),
None,
{},
[],
datetime.datetime.now()),
name="current_item")
sm = smach.StateMachine(outcomes=['Done','Aborted'])
with sm:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={'initialized': 'SET_INITIAL_POSE',
'abort': 'Aborted'})
smach.StateMachine.add('SET_INITIAL_POSE',
states.SetInitialPose(robot, challenge_knowledge.starting_point),
transitions={'done': 'FOLLOW_OPERATOR',
"preempted": 'Aborted',
'error': 'FOLLOW_OPERATOR'})
# TODO: learn operator state needs to be added before follow
# smach.StateMachine.add('WAIT_TO_FOLLOW',
# WaitForOperatorCommand(robot, possible_commands=['follow', 'follow me']),
# transitions={'success': 'FOLLOW_OPERATOR',
# 'abort': 'Aborted'})
smach.StateMachine.add('ASK_FOLLOW_OR_REMEMBER',
states.Say(robot, ["Are we at the car or should I follow you?"], block=True),
transitions={'spoken': 'WAIT_TO_FOLLOW_OR_REMEMBER'})
smach.StateMachine.add('WAIT_TO_FOLLOW_OR_REMEMBER',
WaitForOperatorCommand(robot,
possible_commands=[
"follow",
'follow me',
"here is the car",
"stop following",
"stop following me",
],
commands_as_outcomes=True),
transitions={'follow': 'FOLLOW_OPERATOR',
'follow me': 'FOLLOW_OPERATOR',
'here is the car': 'REMEMBER_CAR_LOCATION',
'stop following': 'REMEMBER_CAR_LOCATION',
'stop following me': 'REMEMBER_CAR_LOCATION',
'abort': 'Aborted'})
# Follow the operator until (s)he states that you have arrived at the "car".
smach.StateMachine.add('FOLLOW_OPERATOR',
states.FollowOperator(robot,
operator_timeout=30,
ask_follow=True,
learn_face=True,
replan=True),
transitions={'stopped': 'ASK_FOLLOW_OR_REMEMBER',
'lost_operator': 'ASK_FOLLOW_OR_REMEMBER',
'no_operator': 'ASK_FOLLOW_OR_REMEMBER'})
smach.StateMachine.add('REMEMBER_CAR_LOCATION',
StoreCarWaypoint(robot),
transitions={'success': 'ASK_DESTINATION',
'abort': 'Aborted'})
smach.StateMachine.add('ASK_DESTINATION',
states.Say(robot, ["Where should I bring the groceries?"], block=True),
transitions={'spoken': 'WAIT_FOR_DESTINATION'})
smach.StateMachine.add('WAIT_FOR_DESTINATION',
WaitForOperatorCommand(robot,
possible_commands=challenge_knowledge.waypoints.keys(),
commands_as_userdata=True),
transitions={'success': 'GRAB_ITEM',
'abort': 'Aborted'})
# Grab the item (bag) the operator hands to the robot, when they are at the "car".
smach.StateMachine.add('GRAB_ITEM',
GrabItem(robot, bag_arm_designator, current_item),
transitions={'succeeded': 'ARM_DRIVING_POSE',
'timeout': 'BACKUP_CLOSE_GRIPPER', # For now in simulation timeout is considered a succes.
'failed': 'BACKUP_CLOSE_GRIPPER'},
remapping={'target_room_in': 'command_recognized',
'target_room_out': 'target_room'})
smach.StateMachine.add('BACKUP_CLOSE_GRIPPER',
states.SetGripper(robot, bag_arm_designator, gripperstate=GripperState.CLOSE),
transitions={'succeeded': 'ARM_DRIVING_POSE',
'failed': 'ARM_DRIVING_POSE'})
smach.StateMachine.add('ARM_DRIVING_POSE',
states.ArmToJointConfig(robot, bag_arm_designator, 'driving_bag_pose'),
transitions={'succeeded': 'SAY_GOING_TO_ROOM',
'failed': 'SAY_GOING_TO_ROOM'})
smach.StateMachine.add('SAY_GOING_TO_ROOM',
states.Say(robot, ["Let me bring in your groceries",
"Helping you carry stuff",
"I'm going back inside"],
block=True),
transitions={'spoken': 'GOTO_DESTINATION'})
smach.StateMachine.add('GOTO_DESTINATION',
NavigateToRoom(robot),
transitions={'arrived': 'PUTDOWN_ITEM',
'unreachable': 'PUTDOWN_ITEM', # implement avoid obstacle behaviour later
'goal_not_defined': 'Aborted'})
# Put the item (bag) down when the robot has arrived at the "drop-off" location (house).
smach.StateMachine.add('PUTDOWN_ITEM',
DropBagOnGround(robot, bag_arm_designator),
transitions={'succeeded': 'ASKING_FOR_HELP',
'failed': 'ASKING_FOR_HELP'})
smach.StateMachine.add('ASKING_FOR_HELP',
#TODO: look and then face new operator
states.Say(robot, "Please follow me and help me carry groceries into the house"),
transitions={'spoken': 'GOTO_CAR'})
#transitions={'success': 'GOTO_CAR',
# 'abort': 'Aborted'})
smach.StateMachine.add('GOTO_CAR',
states.NavigateToWaypoint(robot,
ds.EntityByIdDesignator(robot,
id=challenge_knowledge.waypoint_car['id']),
challenge_knowledge.waypoint_car['radius']),
# TODO: detect closed door
transitions={'unreachable': 'OPEN_DOOR',
'arrived': 'AT_END',
'goal_not_defined': 'Aborted'})
smach.StateMachine.add('OPEN_DOOR',
#TODO: implement functionality
states.Say(robot, "Please open the door for me"),
transitions={'spoken': 'GOTO_CAR'})
#transitions={'success': 'GOTO_CAR',
# 'abort': 'Aborted'})
smach.StateMachine.add('AT_END',
states.Say(robot, ["We arrived at the car, goodbye",
"You have reached your destination, goodbye",
"The car is right here, see you later!"],
block=True),
transitions={'spoken': 'Done'})
ds.analyse_designators(sm, "help_me_carry")
return sm
from robot_smach_states.manipulation.place import EmptySpotDesignator
import robot_smach_states.util.designators as ds
from robot_skills import get_robot
if __name__ == "__main__":
rospy.init_node("testdesignator")
parser = argparse.ArgumentParser(
description="Test the empty spot designator")
parser.add_argument("--robot",
default="hero",
help="Robot name (amigo, hero, sergio)")
args = parser.parse_args()
robot = get_robot(args.robot)
furniture_designator = ds.EntityByIdDesignator(robot, id="dinner_table")
arm_designator = ds.UnoccupiedArmDesignator(robot, {})
def with_area():
esd = EmptySpotDesignator(
robot=robot,
place_location_designator=furniture_designator,
arm_designator=arm_designator,
name="with_area",
area="on_top_of")
print(esd.resolve())
def without_area():
esd = EmptySpotDesignator(
robot=robot,
place_location_designator=furniture_designator,
robot_name = sys.argv[1]
if robot_name == 'amigo':
from robot_skills.amigo import Amigo as Robot
elif robot_name == 'sergio':
from robot_skills.sergio import Sergio as Robot
elif robot_name == 'hero':
from robot_skills.hero import Hero as Robot
else:
rospy.logerr("unknown robot")
sys.exit()
robot = Robot()
if len(sys.argv) > 2:
furniture_id = sys.argv[2]
else:
furniture_id = "bed"
rospy.loginfo("Waiting for tf cache to be filled")
rospy.sleep(2) # wait for tf cache to be filled
rospy.loginfo("Testing the 'get_room' method")
_test_rooms(robot)
rospy.loginfo("Starting giving directions to {}".format(furniture_id))
state = GiveDirections(robot=robot,
entity_designator=ds.EntityByIdDesignator(
robot=robot, id=furniture_id))
state.execute()
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
start_waypoint = ds.EntityByIdDesignator(robot,
id="manipulation_init_pose",
name="start_waypoint")
placed_items = []
with sm:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'INIT_WM',
'abort': 'Aborted'
})
smach.StateMachine.add("INIT_WM",
InitializeWorldModel(robot),
transitions={'done': 'AWAIT_START'})
# smach.StateMachine.add("INSTRUCT_WAIT_FOR_DOOR",
# states.Say(robot, ["Hi there, I will now wait until you remove the cup",
# "I'm waiting for you to remove the cup"], block=False),
# transitions={"spoken": "WAIT_FOR_DOOR"})
#
# smach.StateMachine.add("WAIT_FOR_DOOR",
# states.WaitForDoorOpen(robot, timeout=10),
# transitions={"closed": "DOOR_CLOSED",
# "open": "AWAIT_START"})
#
# smach.StateMachine.add("DOOR_CLOSED",
# states.Say(robot, ["I am waiting for you to remove the cup",
# "I'd start, if you remove the cup from my laser"]),
# transitions={"spoken": "WAIT_FOR_DOOR"})
if USE_SLAM:
drive_state = "RESET_ED_SLAM"
else:
drive_state = "NAV_TO_START"
smach.StateMachine.add("AWAIT_START",
states.AskContinue(robot),
transitions={
'continue': drive_state,
'no_response': 'AWAIT_START'
})
cabinet = ds.EntityByIdDesignator(robot, id=CABINET)
room = ds.EntityByIdDesignator(robot, id=ROOM)
if USE_SLAM:
# vth = 1.0
# smach.StateMachine.add("NAV_TO_FIT_POSE",
# ForceDrive(robot, 0, 0, vth, 3.14/vth),
# transitions={'done': 'FIT_ENTITY'})
smach.StateMachine.add("RESET_ED_SLAM",
states.ResetED(robot),
transitions={'done': 'FIT_ENTITY'})
smach.StateMachine.add("FIT_ENTITY",
FitEntity(robot, CABINET),
transitions={
'succeeded': 'NAV_TO_START',
'failed': 'SAY_FITTING_FAILED'
})
smach.StateMachine.add(
"SAY_FITTING_FAILED",
states.Say(robot, [
"Fitting the {0} failed, I will stop now.".format(CABINET)
],
mood="sad"),
transitions={'spoken': 'Aborted'})
smach.StateMachine.add("NAV_TO_START",
states.NavigateToSymbolic(
robot, {cabinet: "in_front_of"}, cabinet),
transitions={
'arrived': 'INSPECT_SHELVES',
'unreachable': 'FORCE_ROTATE',
'goal_not_defined': 'INSPECT_SHELVES'
})
smach.StateMachine.add("FORCE_ROTATE",
ForceRotate(robot, 0.5, 2.0, 30.0),
transitions={
'done': "NAV_TO_START",
'timedout': "INSPECT_SHELVES"
})
# smach.StateMachine.add("RESET_ED",
# states.ResetED(robot),
# transitions={'done' :'INSPECT_SHELVES'})
smach.StateMachine.add("INSPECT_SHELVES",
InspectShelves(robot, OBJECT_SHELVES),
transitions={
'succeeded': 'EXPORT_PDF',
'nothing_found': 'EXPORT_PDF',
'failed': 'EXPORT_PDF'
})
@smach.cb_interface(outcomes=["exported"])
def export_to_pdf(userdata=None):
global DETECTED_OBJECTS_WITH_PROBS
entities = [e[0] for e in DETECTED_OBJECTS_WITH_PROBS]
# Export images (Only best MAX_NUM_ENTITIES_IN_PDF)
# pdf.entities_to_pdf(robot.ed, entities[:MAX_NUM_ENTITIES_IN_PDF], "tech_united_manipulation_challenge")
return "exported"
smach.StateMachine.add('EXPORT_PDF',
smach.CBState(export_to_pdf),
transitions={'exported': 'RANGE_ITERATOR'})
# Begin setup iterator
range_iterator = smach.Iterator(
outcomes=['succeeded', 'failed'], #Outcomes of the iterator state
input_keys=[],
output_keys=[],
it=lambda: range(5),
it_label='index',
exhausted_outcome='succeeded'
) #The exhausted argument should be set to the preffered state machine outcome
with range_iterator:
single_item = ManipRecogSingleItem(
robot,
ds.VariableDesignator(placed_items, [Entity],
name="placed_items"))
smach.Iterator.set_contained_state(
'SINGLE_ITEM',
single_item,
loop_outcomes=['succeeded', 'failed'])
smach.StateMachine.add('RANGE_ITERATOR', range_iterator, {
'succeeded': 'AT_END',
'failed': 'Aborted'
})
# End setup iterator
smach.StateMachine.add('AT_END',
states.Say(robot, "Goodbye"),
transitions={'spoken': 'Done'})
ds.analyse_designators(sm, "manipulation")
return sm
parser.add_argument("--robot",
default="hero",
help="Robot name (amigo, hero, sergio)")
parser.add_argument(
"entity",
help="Entity name of the object to guide to, for example dinner_table")
args = parser.parse_args()
# Create node, robot and toggle interface
rospy.init_node("test_guidance")
r = get_robot(args.robot)
e_id = args.entity
# Instantiate GuideToSymbolic machine
s = guidance.GuideToSymbolic(
r, {ds.EntityByIdDesignator(r, id=e_id): "in_front_of"},
ds.EntityByIdDesignator(r, id=e_id))
# In simulation, mock the _check_operator method
if os.getenv("ROBOT_REAL", "false").lower() != "true":
# noinspection PyUnusedLocal
def _mock_detect_operator(robot, distance=1.0, radius=0.5):
return OPERATOR_AVAILABLE
guidance._detect_operator_behind_robot = _mock_detect_operator
rospy.Service("toggle_operator", std_srvs.srv.Empty, toggle_operator)
rospy.loginfo(
'\n\nCall:\nrosservice call /toggle_operator "{}"\n\nto toggle operator availability\n'
.format('{}'))
# Execute the state
def __init__(self, robot, room_id=ROOM_ID):
"""
Finds the people in the living room and takes pictures of them. Put then in a data struct and put this in
output keys.
Output keys:
* detected_people: same datastruct as was used in find my mates. Ask Rein for a pickled example
:param robot: (Robot) api object
:param room_id: (str) identifies the room in which the people are detected
"""
smach.StateMachine.__init__(self, outcomes=["done"], output_keys=["detected_people"])
with self:
# Move to start location
smach.StateMachine.add("NAVIGATE_TO_START",
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=ds.EntityByIdDesignator(robot, WAYPOINT_ID), radius=0.3),
transitions={"arrived": "DETECT_PEOPLE",
"unreachable": "DETECT_PEOPLE",
"goal_not_defined": "DETECT_PEOPLE"})
@smach.cb_interface(outcomes=["done"], output_keys=["raw_detections"])
def detect_people(user_data):
person_detections = []
#look_angles = np.linspace(-np.pi / 2, np.pi / 2, 8) # From -pi/2 to +pi/2 to scan 180 degrees wide
look_angles = np.linspace(-np.pi / 4, np.pi / 4, 4) # From -pi/2 to +pi/2 to scan 180 degrees wide
head_goals = [kdl_conversions.VectorStamped(x=100 * math.cos(angle),
y=100 * math.sin(angle),
z=1.5,
frame_id="/%s/base_link" % robot.robot_name)
for angle in look_angles]
sentences = deque([
"Hi there mates, where are you, please look at me!",
"I am looking for my mates! Dippi dee doo! Pew pew!",
"You are all looking great today! Keep looking at my camera.",
"I like it when everybody is staring at me!"
])
look_at_me_sentences = deque([
"Please look at me",
"Let me check you out",
"Your eyes are beautiful",
"Try to look pretty, your face will pop up on the screen later!"
])
for _ in range(NUM_LOOKS):
sentences.rotate(1)
robot.speech.speak(sentences[0], block=False)
for head_goal in head_goals:
look_at_me_sentences.rotate(1)
robot.speech.speak(look_at_me_sentences[0], block=False)
robot.head.look_at_point(head_goal)
robot.head.wait_for_motion_done()
now = time.time()
rgb, depth, depth_info = robot.perception.get_rgb_depth_caminfo()
if rgb:
try:
persons = robot.perception.detect_person_3d(rgb, depth, depth_info)
except Exception as e:
rospy.logerr(e)
rospy.sleep(2.0)
else:
for person in persons:
if person.face.roi.width > 0 and person.face.roi.height > 0:
try:
person_detections.append({
"map_ps": robot.tf_listener.transformPoint("map", PointStamped(
header=rgb.header,
point=person.position
)),
"person_detection": person,
"rgb": rgb
})
except Exception as e:
rospy.logerr(
"Failed to transform valid person detection to map frame: {}".format(e))
rospy.loginfo("Took %.2f, we have %d person detections now", time.time() - now,
len(person_detections))
rospy.loginfo("Detected %d persons", len(person_detections))
user_data.raw_detections = person_detections
return 'done'
smach.StateMachine.add('DETECT_PEOPLE',
smach.CBState(detect_people),
transitions={"done": "FILTER_AND_CLUSTER"})
# Filter and cluster images
@smach.cb_interface(
outcomes=["done", "failed"],
input_keys=["raw_detections"],
output_keys=["detected_people"])
def filter_and_cluster_images(user_data):
"""
Filters the raw detections so that only people within the room maintain. Next, it clusters the images
so that only one image per person remains. This is stored in the user data
:param user_data: (smach.UserData)
:return: (str) Done
"""
try:
user_data.detected_people = _filter_and_cluster_images(
robot, user_data.raw_detections, room_id)
return "done"
except:
return "failed"
smach.StateMachine.add('FILTER_AND_CLUSTER',
smach.CBState(filter_and_cluster_images),
transitions={"done": "done",
"failed": "done"}) # ToDo: fallback
rospy.init_node('supervisor_wrapper', log_level=rospy.DEBUG)
robot = robot_constructor("amigo")
objects_to_grab = ["coke"]
objects_to_navigate = ["hallway_table", "dinner_table"]
objects_to_navigate_ds = []
robot.base.set_initial_pose(0.2, 5, -1.4) # not nescessary in demo extern mode
leftarm = Designator(robot.leftArm)
rightarm = Designator(robot.rightArm)
objects_to_grab.reverse() # Reverse here, because we are going to use list.pop()
objects_to_navigate.reverse() # Reverse here, because we are going to use list.pop()
for item in objects_to_navigate:
objects_to_navigate_ds.append(ds.EntityByIdDesignator(robot, id=item))
leftarm.resolve().reset()
rightarm.resolve().reset()
leftarm.resolve().send_gripper_goal('close')
rightarm.resolve().send_gripper_goal('close')
raw_input('Press enter to continue: ')
robot.base.set_initial_pose(0.2, 5, -1.4) # not nescessary in demo extern mode
lib.Amigo_EngineFirstStep()
while not rospy.is_shutdown():
lib.Amigo_EngineTimeStep(0.5)
rospy.sleep(0.5)
reset_variables()
description="Test if it can segment objects")
parser.add_argument("--robot",
default="hero",
help="Robot name (amigo, hero, sergio)")
parser.add_argument("entity",
default="dinner_table",
help="Name of the entity on which to segment objects")
args = parser.parse_args()
rospy.init_node("example_segment_objects")
robot = get_robot(args.robot)
location_name = args.entity
rospy.loginfo("Creating location designator")
location_designator = ds.EntityByIdDesignator(robot, location_name)
rospy.loginfo("Creating found entity designator")
segmented_entities_designator = ds.VariableDesignator(
[], resolve_type=[ClassificationResult])
rospy.loginfo("Creating segment objects state")
sm = SegmentObjects(robot, segmented_entities_designator.writeable,
location_designator, "on_top_of")
rospy.loginfo("Executing segment objects state")
sm.execute()
rospy.loginfo("Resulting classification result: \n {}".format(
segmented_entities_designator.resolve()))
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, 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):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
msg = "\n".join([
"==============================================",
"== CHALLENGE HELP ME CARRY ==",
"=============================================="
])
rospy.loginfo("\n" + msg)
self.target_destination = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.default_place)
self.car_waypoint = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.waypoint_car['id'])
self.empty_arm_designator = ds.UnoccupiedArmDesignator(
robot,
arm_properties={
"required_goals": [
"handover_to_human", "reset",
challenge_knowledge.driving_bag_pose,
challenge_knowledge.drop_bag_pose
],
"required_gripper_types": [GripperTypes.GRASPING]
},
name="empty_arm_designator")
# With the empty_arm_designator locked, it will ALWAYS resolve to the same arm, unless it is unlocked.
# For this challenge, unlocking is not needed.
self.bag_arm_designator = self.empty_arm_designator.lockable()
self.bag_arm_designator.lock()
self.place_position = ds.LockingDesignator(EmptySpotDesignator(
robot,
self.target_destination,
arm_designator=self.bag_arm_designator,
name="placement",
area=challenge_knowledge.default_area),
name="place_position")
# We don't actually grab something, so there is no need for an actual thing to grab
self.current_item = ds.VariableDesignator(Entity(
"dummy", "dummy", "/{}/base_link".format(robot.robot_name),
kdl_conversions.kdl_frame_from_XYZRPY(0.6, 0, 0.5), None, {}, [],
datetime.datetime.now()),
name="current_item")
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'SET_INITIAL_POSE',
'abort': 'Aborted'
})
smach.StateMachine.add('SET_INITIAL_POSE',
states.SetInitialPose(
robot,
challenge_knowledge.starting_point),
transitions={
'done': 'FOLLOW_OPERATOR',
"preempted": 'Aborted',
'error': 'FOLLOW_OPERATOR'
})
# Follow the operator until (s)he states that you have arrived at the "car".
smach.StateMachine.add('FOLLOW_OPERATOR',
states.FollowOperator(robot,
operator_timeout=30,
ask_follow=True,
learn_face=True,
replan=True),
transitions={
'stopped': 'ASK_FOR_TASK',
'lost_operator': 'ASK_FOR_TASK',
'no_operator': 'ASK_FOR_TASK'
})
smach.StateMachine.add('ASK_FOR_TASK',
states.Say(robot,
["Are we at the car already?"],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'WAIT_FOR_TASK'})
smach.StateMachine.add(
'WAIT_FOR_TASK',
hmc_states.WaitForOperatorCommand(
robot,
possible_commands=challenge_knowledge.commands.keys(),
commands_as_outcomes=True),
transitions={
'no': 'FOLLOW_OPERATOR',
'yes': 'REMEMBER_CAR_LOCATION',
'abort': 'Aborted'
})
smach.StateMachine.add('REMEMBER_CAR_LOCATION',
hmc_states.StoreCarWaypoint(robot),
transitions={
'success': 'ASK_FOR_DESTINATION',
'abort': 'Aborted'
})
smach.StateMachine.add(
'ASK_FOR_DESTINATION',
states.Say(robot, ["Where should I bring the groceries?"],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'RECEIVE_DESTINATION'})
smach.StateMachine.add(
'RECEIVE_DESTINATION',
hmc_states.WaitForOperatorCommand(
robot,
possible_commands=challenge_knowledge.destinations,
commands_as_userdata=True,
target=self.target_destination),
transitions={
'success': 'GRAB_ITEM',
'abort': 'Aborted'
})
# Grab the item (bag) the operator hands to the robot, when they are at the "car".
smach.StateMachine.add(
'GRAB_ITEM',
states.HandoverFromHuman(
robot,
self.bag_arm_designator,
"current_item",
self.current_item,
arm_configuration=challenge_knowledge.carrying_bag_pose),
transitions={
'succeeded': 'ARM_DRIVING_POSE',
'timeout': 'BACKUP_CLOSE_GRIPPER',
# For now in simulation timeout is considered a success.
'failed': 'BACKUP_CLOSE_GRIPPER'
})
smach.StateMachine.add('BACKUP_CLOSE_GRIPPER',
states.SetGripper(
robot,
self.bag_arm_designator,
gripperstate=GripperState.CLOSE),
transitions={
'succeeded': 'ARM_DRIVING_POSE',
'failed': 'ARM_DRIVING_POSE'
})
smach.StateMachine.add('ARM_DRIVING_POSE',
states.ArmToJointConfig(
robot, self.bag_arm_designator,
challenge_knowledge.driving_bag_pose),
transitions={
'succeeded': 'SAY_GOING_TO_ROOM',
'failed': 'SAY_GOING_TO_ROOM'
})
smach.StateMachine.add(
'SAY_GOING_TO_ROOM',
states.Say(robot, [
"Let me bring in your groceries",
"Helping you carry stuff", "I'm going back inside"
],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'GOTO_DESTINATION'})
smach.StateMachine.add(
'GOTO_DESTINATION',
states.NavigateToWaypoint(
robot, self.target_destination,
challenge_knowledge.default_target_radius),
transitions={
'arrived': 'PUTDOWN_ITEM',
'unreachable': 'GOTO_DESTINATION_BACKUP',
# implement avoid obstacle behaviour later
'goal_not_defined': 'Aborted'
})
smach.StateMachine.add(
'GOTO_DESTINATION_BACKUP',
states.NavigateToWaypoint(
robot, self.target_destination,
challenge_knowledge.backup_target_radius),
transitions={
'arrived': 'PUTDOWN_ITEM',
'unreachable': 'PUTDOWN_ITEM',
# implement avoid obstacle behaviour later
'goal_not_defined': 'Aborted'
})
# Put the item (bag) down when the robot has arrived at the "drop-off" location (house).
smach.StateMachine.add('PUTDOWN_ITEM',
hmc_states.DropBagOnGround(
robot, self.bag_arm_designator,
challenge_knowledge.drop_bag_pose),
transitions={'done': 'ASKING_FOR_HELP'})
smach.StateMachine.add(
'ASKING_FOR_HELP',
# TODO: look and then face new operator
states.Say(
robot,
"Please follow me and help me carry groceries into the house",
block=True,
look_at_standing_person=True),
transitions={'spoken': 'GOTO_CAR'})
smach.StateMachine.add(
'GOTO_CAR',
states.NavigateToWaypoint(
robot, self.car_waypoint,
challenge_knowledge.waypoint_car['radius']),
# TODO: detect closed door
transitions={
'unreachable': 'OPEN_DOOR',
'arrived': 'AT_END',
'goal_not_defined': 'Aborted'
})
smach.StateMachine.add(
'OPEN_DOOR',
# TODO: implement functionality
states.Say(robot, "Please open the door for me"),
transitions={'spoken': 'GOTO_CAR'})
smach.StateMachine.add(
'AT_END',
states.Say(robot, [
"We arrived at the car, goodbye",
"You have reached your destination, goodbye",
"The car is right here, see you later!"
],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "help_me_carry")
def __init__(self,
robot,
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, assume_john):
"""
:param robot:
:param assume_john: bool indicating that John (the homeowner) is already there.
"""
smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted'])
door_waypoint = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.waypoint_door['id'])
livingroom_waypoint = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.waypoint_livingroom['id'])
guest_entity_des = ds.VariableDesignator(resolve_type=Entity,
name='guest_entity')
guest_name_des = ds.VariableDesignator('guest 1', name='guest_name')
guest_drink_des = ds.VariableDesignator(resolve_type=HMIResult,
name='guest_drink')
guest_drinkname_des = ds.FieldOfHMIResult(guest_drink_des,
semantics_field='drink',
name='guest_drinkname')
with self:
smach.StateMachine.add('LEARN_GUEST',
LearnGuest(robot, door_waypoint,
guest_entity_des, guest_name_des,
guest_drink_des),
transitions={
'succeeded': 'SAY_GOTO_OPERATOR',
'aborted': 'SAY_GOTO_OPERATOR',
'failed': 'SAY_GOTO_OPERATOR'
})
smach.StateMachine.add(
'SAY_GOTO_OPERATOR',
states.SayFormatted(robot, [
"Okidoki, you are {name} and you like {drink}, lets go inside. Please follow me"
],
name=guest_name_des,
drink=guest_drinkname_des,
block=True,
look_at_standing_person=True),
transitions={'spoken': 'GOTO_LIVINGROOM'})
smach.StateMachine.add(
'GOTO_LIVINGROOM',
states.NavigateToWaypoint(
robot, livingroom_waypoint,
challenge_knowledge.waypoint_livingroom['radius']),
transitions={
'arrived': 'INTRODUCE_GUEST',
'unreachable': 'INTRODUCE_GUEST',
'goal_not_defined': 'aborted'
})
smach.StateMachine.add('INTRODUCE_GUEST',
IntroduceGuest(robot,
guest_entity_des,
guest_name_des,
guest_drinkname_des,
assume_john=assume_john),
transitions={
'succeeded': 'FIND_SEAT_FOR_GUEST',
'abort': 'FIND_SEAT_FOR_GUEST'
})
smach.StateMachine.add(
'FIND_SEAT_FOR_GUEST',
FindEmptySeat(robot,
seats_to_inspect=challenge_knowledge.seats,
room=ds.EntityByIdDesignator(
robot, challenge_knowledge.sitting_room),
seat_is_for=guest_name_des),
transitions={
'succeeded': 'succeeded',
'failed': 'aborted'
})
