def __init__(self, robot, grab_designator=None):
"""
Constructor
:param robot: robot object
:param grab_designator: EdEntityDesignator designating the item to grab. If not provided, a default one is
constructed (grabs the closest object in the volume of the surface)
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"])
# Create designators
self.empty_arm_designator = ds.UnoccupiedArmDesignator(
robot, {
"required_trajectories": ["prepare_grasp"],
"required_goals": ["carrying_pose"],
"required_gripper_types": [arms.GripperTypes.GRASPING]
},
name="empty_arm_designator")
self.grab_designator = ds.LockToId(robot=robot,
to_be_locked=grab_designator)
with self:
@smach.cb_interface(outcomes=["locked"])
def lock(userdata=None):
""" 'Locks' a locking designator """
# This determines that self.current_item cannot not resolve to a new value until it is unlocked again.
self.grab_designator.lock()
if self.grab_designator.resolve():
rospy.loginfo("Current_item is now locked to {0}".format(
self.grab_designator.resolve().id))
return "locked"
smach.StateMachine.add("LOCK_ITEM",
smach.CBState(lock),
transitions={'locked': 'GRAB_ITEM'})
smach.StateMachine.add("GRAB_ITEM",
states.Grab(robot, self.grab_designator,
self.empty_arm_designator),
transitions={
'done': 'UNLOCK_ITEM_SUCCEED',
'failed': 'UNLOCK_ITEM_FAIL'
})
@smach.cb_interface(outcomes=["unlocked"])
def unlock(userdata=None):
""" 'Unlocks' a locking designator """
self.grab_designator.unlock()
return "unlocked"
smach.StateMachine.add("UNLOCK_ITEM_SUCCEED",
smach.CBState(unlock),
transitions={'unlocked': 'succeeded'})
smach.StateMachine.add("UNLOCK_ITEM_FAIL",
smach.CBState(unlock),
transitions={'unlocked': 'failed'})
def __init__(self, robot, grab_designator=None):
""" Constructor
:param robot: robot object
:param grab_designator: EdEntityDesignator designating the item to grab. If not provided, a default one is
constructed (grabs the closest object in the volume of the surface)
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"])
# Create designators
self.empty_arm_designator = ds.UnoccupiedArmDesignator(robot.arms, robot.leftArm, name="empty_arm_designator")
self.grab_designator = ds.LockToId(robot=robot, to_be_locked=grab_designator)
with self:
@smach.cb_interface(outcomes=["locked"])
def lock(userdata=None):
""" 'Locks' a locking designator """
# This determines that self.current_item cannot not resolve to a new value until it is unlocked again.
self.grab_designator.lock()
if self.grab_designator.resolve():
rospy.loginfo("Current_item is now locked to {0}".format(self.grab_designator.resolve().id))
return "locked"
smach.StateMachine.add("LOCK_ITEM",
smach.CBState(lock),
transitions={'locked': 'ANNOUNCE_ITEM'})
smach.StateMachine.add("ANNOUNCE_ITEM",
states.Say(robot, EntityDescriptionDesignator(self.grab_designator,
name="current_item_desc"),
block=False),
transitions={'spoken': 'GRAB_ITEM'})
smach.StateMachine.add("GRAB_ITEM",
states.Grab(robot, self.grab_designator, self.empty_arm_designator),
transitions={'done': 'UNLOCK_ITEM_SUCCEED',
'failed': 'UNLOCK_ITEM_FAIL'})
@smach.cb_interface(outcomes=["unlocked"])
def lock(userdata=None):
""" 'Locks' a locking designator """
# This determines that self.current_item cannot not resolve to a new value until it is unlocked again.
self.grab_designator.unlock()
return "unlocked"
smach.StateMachine.add("UNLOCK_ITEM_SUCCEED",
smach.CBState(lock),
transitions={'unlocked': 'succeeded'})
smach.StateMachine.add("UNLOCK_ITEM_FAIL",
smach.CBState(lock),
transitions={'unlocked': 'failed'})