def execute(self, userdata=None):
arm = None
# See if there's an arm holding something
for k, v in self._robot.arms.iteritems():
if v.occupied_by is not None:
arm = v
break
if arm is None:
return "failed"
# Try to place the object
item = ds.EdEntityDesignator(robot=self._robot, id=arm.occupied_by.id)
arm_designator = ds.ArmDesignator(all_arms={arm.side: arm},
preferred_arm=arm)
sm = states.Place(robot=self._robot,
item_to_place=item,
place_pose=self._place_designator,
arm=arm_designator)
result = sm.execute()
# If failed, do handover to human in order to continue
if result != "done":
sm = states.HandoverToHuman(robot=self._robot,
arm_designator=arm_designator)
sm.execute()
return "succeeded" if result == "done" else "failed"
def setUp(self):
self.arm_ds = ds.ArmDesignator(
self.robot, {
'required_arm_name': 'leftArm',
"required_gripper_types": [arms.GripperTypes.GRASPING]
})
self.entity_label = "entity_label"
def setUp(self):
self.arm_ds = ds.ArmDesignator(
self.robot, {
'required_arm_name': 'leftArm',
"required_gripper_types": [arms.GripperTypes.GRASPING]
})
self.entity = Entity("12345", "dummy", "/map", None, None, {}, None, 0)
self.entity_ds = ds.VariableDesignator(self.entity)
def setUp(self):
self.arm_ds = ds.ArmDesignator(self.robot,
{'required_arm_name': 'leftArm'})
self.entity = Entity("12345", "dummy", "/map", None, None, {}, None, 0)
self.entity_ds = ds.VariableDesignator(self.entity)
def setUp(self):
self.arm_ds = ds.ArmDesignator(self.robot,
{'required_arm_name': 'leftArm'})
def test_invalid_arm_designator(self):
invalid_arm_ds = ds.ArmDesignator(
self.robot,
{'required_arm_name': 'there_is_no_arm_with_this_name'})
state = states.SetGripper(self.robot, invalid_arm_ds, 'close')
self.assertEqual(state.execute(), "failed")
parser = argparse.ArgumentParser(
description=
"Place an imaginary object at provided x, y, z coordinates using the "
"'Place' smach state")
parser.add_argument("x", type=float, help="x-coordinate (in map)")
parser.add_argument("y", type=float, help="y-coordinate (in map)")
parser.add_argument("z", type=float, help="z-coordinate (in map)")
parser.add_argument("--robot",
default="hero",
help="Robot name (amigo, hero, sergio)")
args = parser.parse_args()
rospy.init_node("test_placing")
robot = get_robot(args.robot)
pose = FrameStamped(frame=kdl.Frame(kdl.Rotation.RPY(0.0, 0.0, 0.0),
kdl.Vector(args.x, args.y, args.z)),
frame_id="/map")
item = Entity("dummy_id", "dummy_type", None, None, None, None, None, None)
arm = ds.ArmDesignator(robot,
arm_properties={
"required_trajectories": ["prepare_place"],
"required_gripper_types":
[arms.GripperTypes.GRASPING]
})
place_state = Place(robot, ds.Designator(item), ds.Designator(pose), arm)
place_state.execute()
def setUp(self):
self.arm_ds = ds.ArmDesignator(
self.robot, {
"required_gripper_types": [arms.GripperTypes.GRASPING],
'required_arm_name': 'leftArm'
})
def __init__(self, robot, grasp_designator1, grasp_designator2,
grasp_designator3, grasp_furniture_id1, grasp_furniture_id2,
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_designator2 = ds.EntityByIdDesignator(
robot, id=grasp_furniture_id2)
place_furniture_designator = ds.EntityByIdDesignator(
robot, id=place_furniture_id)
arm_designator = ds.ArmDesignator(robot, {})
place_designator = EmptySpotDesignator(
robot=robot,
place_location_designator=place_furniture_designator,
arm_designator=arm_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_designator2: "in_front_of"},
grasp_furniture_designator2),
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_designator2: "large_in_front_of"},
grasp_furniture_designator2),
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_designator2,
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,
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="temp") # will be updated later on
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)
self.cabinet = ds.EntityByIdDesignator(
robot, id="temp") # will be updated later on
self.place_entity_designator = ds.EdEntityDesignator(robot=robot,
id="temp")
self.arm_designator = ds.ArmDesignator(robot, {})
self.place_designator = EmptySpotDesignator(
robot=robot,
place_location_designator=self.place_entity_designator,
arm_designator=self.arm_designator,
area="temp")
self.placeaction1 = PlaceSingleItem(
robot=robot, place_designator=self.place_designator)
self.placeaction2 = PlaceSingleItem(
robot=robot, place_designator=self.place_designator)
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: "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
# The Inspect-state (INSPECT_TABLE) gathers a list of ClassificationResults for Entities on the table
# These are passed to the pdf_writer
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,
navigation_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"
})
smach.StateMachine.add("MOVE_TO_PLACE",
states.NavigateToSymbolic(
robot, {self.cabinet: "in_front_of"},
self.cabinet),
transitions={
'arrived': 'PLACE_ITEM_1',
'unreachable': 'PLACE_ITEM_1',
'goal_not_defined': 'PLACE_ITEM_1'
})
smach.StateMachine.add("PLACE_ITEM_1",
self.placeaction1,
transitions={
"succeeded": "PLACE_ITEM_2",
"failed": "PLACE_ITEM_2"
})
smach.StateMachine.add("PLACE_ITEM_2",
self.placeaction2,
transitions={
"succeeded": "succeeded",
"failed": "failed"
})
