def execute(self, userdata=None):
# Try to place the object
item = ds.EdEntityDesignator(robot=self._robot, id=arm.occupied_by.id)
arm_designator = ds.OccupiedArmDesignator(
self._robot,
arm_properties={
"required_trajectories": ["prepare_place"],
"required_goals": ["reset", "handover_to_human"],
"required_gripper_types": [arms.GripperTypes.GRASPING]
})
resolved_arm = arm_designator.resolve()
if resolved_arm is None:
rospy.logwarn("No arm holding an entity")
return "failed"
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 execute(self, userdata=None):
# Try to place the object
item = ds.EdEntityDesignator(robot=self._robot, id=arm.occupied_by.id)
arm_designator = ds.OccupiedArmDesignator(
self._robot, {
"required_goals": ["reset", "handover_to_human"],
"required_gripper_types": [arms.GripperTypes.GRASPING]
})
resolved_arm = arm_designator.resolve()
if resolved_arm is None:
rospy.logwarn("No arm holding an entity")
return "failed"
place = states.Place(robot=self._robot,
item_to_place=item,
place_pose=self.place_designator,
arm=arm_designator)
result = place.execute()
# If failed, do handover to human in order to continue
if result != "done":
rospy.loginfo("{place} resulted in {out}".format(place=place,
out=result))
handover = states.HandoverToHuman(robot=self._robot,
arm_designator=arm_designator)
handover.execute()
return "succeeded" if result == "done" else "failed"
def setUp(self):
entity = Entity("12345", "dummy", "/map", None, None, {}, None, 0)
self.robot.arms["leftArm"].occupied_by = entity
self.arm_ds = ds.OccupiedArmDesignator(
self.robot, {
"required_goals": ['handover_to_human', 'reset'],
"required_gripper_types": [arms.GripperTypes.GRASPING]
})
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=None):
# Try to place the object
item = ds.EdEntityDesignator(robot=self._robot, id=arm.occupied_by.id)
arm_designator = ds.OccupiedArmDesignator(self._robot)
resolved_arm = arm_designator.resolve()
if resolved_arm is None:
rospy.logwarn("No arm holding an entity")
return "failed"
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 __init__(self, dummy_robot):
smach.StateMachine.__init__(self, outcomes=[
"succeeded",
"failed",
])
# Create designators
dummy_trashbin_designator = ds.EdEntityDesignator(
dummy_robot,
id=CHALLENGE_KNOWLEDGE.trashbin_id2,
name='trashbin_designator')
dummy_arm_designator_un = ds.UnoccupiedArmDesignator(dummy_robot, {})
dummy_arm_designator_oc = ds.OccupiedArmDesignator(dummy_robot, {})
with self:
smach.StateMachine.add("PICK_UP_TRASH",
PickUpTrash(dummy_robot,
dummy_trashbin_designator,
dummy_arm_designator_un),
transitions={
"succeeded": "TURN_BASE",
"failed": "TURN_BASE",
"aborted": "failed"
})
smach.StateMachine.add("TURN_BASE",
states.ForceDrive(dummy_robot, 0, 0, 0.5,
3),
transitions={"done": "DROP_TRASH"})
smach.StateMachine.add("DROP_TRASH",
DropTrash(dummy_robot,
dummy_arm_designator_oc),
transitions={
"succeeded": "succeeded",
"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",
Say(robot,
["I will pick-up the {object}", "Let's move the {object}"],
object=selected_entity_type_des,
block=True),
transitions={"spoken": "GRAB"})
smach.StateMachine.add("GRAB",
Grab(
robot, selected_entity_designator,
ds.UnoccupiedArmDesignator(
robot,
arm_properties={
"required_trajectories":
["prepare_grasp"],
"required_goals":
["carrying_pose"],
"required_gripper_types":
[arms.GripperTypes.GRASPING]
},
name="empty_arm_designator")),
transitions={
"done": "SAY_GRAB_SUCCESS",
"failed": "ARM_RESET"
})
smach.StateMachine.add(
"ARM_RESET",
ArmToJointConfig(
robot,
ds.UnoccupiedArmDesignator(
robot,
arm_properties={"required_goals": ["reset"]},
name="empty_arm_designator"), "reset"),
transitions={
"succeeded": "SAY_GRAB_FAILED",
"failed": "SAY_GRAB_FAILED"
})
smach.StateMachine.add(
'SAY_GRAB_SUCCESS',
Say(robot, [
"Now I am going to move this item",
"Let's clean up this object", "Away with this one",
"Everything will be cleaned"
],
block=False),
transitions={"spoken": "GET_CATEGORY"})
smach.StateMachine.add(
'SAY_GRAB_FAILED',
Say(robot, [
"I could not grab the item.", "I failed to grasp the item",
"I cannot reach the item", "Item grab failed"
],
block=False),
transitions={"spoken": "failed"})
smach.StateMachine.add('CHECK_ARM_FREE',
ArmFree(robot),
transitions={
"yes": "done",
"no": "CHECK_ARM_OCCUPIED"
})
smach.StateMachine.add('CHECK_ARM_OCCUPIED',
ArmOccupied(robot),
transitions={
"yes": "GET_CATEGORY",
"no": "done"
})
# # ROBOT
# smach.StateMachine.add('GET_CATEGORY',
# EntityToCategory(robot, selected_entity_designator, category_des.writeable),
# transitions={"done": "DETERMINE_PLACE_LOCATION",
# "failed": "NAVIGATE_TO_TRASH"})
# OPERATOR
smach.StateMachine.add('GET_CATEGORY',
OperatorToCategory(robot,
category_des.writeable,
room_des),
transitions={
"done": "DETERMINE_PLACE_LOCATION",
"failed": "NAVIGATE_TO_TRASH"
})
smach.StateMachine.add(
'DETERMINE_PLACE_LOCATION',
CategoryToLocation(category_des, store_entity_id_des.writeable,
store_area_name_des.writeable),
transitions={
"trashbin": "INSPECT_TRASH",
"other": "PLACE_TO_STORE",
"failed": "NAVIGATE_TO_TRASH"
})
smach.StateMachine.add('NAVIGATE_TO_TRASH',
NavigateToPlace(
robot, trash_place_pose,
ds.OccupiedArmDesignator(
robot, {},
name="occupied_arm_designator")),
transitions={
"arrived": "PLACE_IN_TRASH",
"unreachable": "SAY_PLACE_FAILED",
"goal_not_defined": "SAY_PLACE_FAILED"
})
smach.StateMachine.add('INSPECT_TRASH',
Inspect(robot, store_entity_des),
transitions={
"done": "PLACE_IN_TRASH",
"failed": "SAY_PLACE_FAILED"
})
arm_properties_place = {
"required_trajectories": ["prepare_place"],
"required_gripper_types": [arms.GripperTypes.GRASPING]
}
arm_designator_place = ds.OccupiedArmDesignator(
robot, arm_properties_place, name="occupied_arm_designator")
smach.StateMachine.add('PLACE_IN_TRASH',
Place(robot, selected_entity_designator,
trash_place_pose,
arm_designator_place),
transitions={
"done": "SAY_PLACE_SUCCESS",
"failed": "SAY_PLACE_FAILED"
})
arm_designator_place_store = ds.OccupiedArmDesignator(
robot, arm_properties_place, name="occupied_arm_designator")
smach.StateMachine.add('PLACE_TO_STORE',
Place(robot, selected_entity_designator,
store_entity_des,
arm_designator_place_store,
"on_top_of"),
transitions={
"done": "SAY_PLACE_SUCCESS",
"failed": "SAY_PLACE_FAILED"
})
smach.StateMachine.add(
'SAY_PLACE_SUCCESS',
Say(robot, [
"Bye bye!", "Yeah!", "Successfully disposed the item",
"Another score for {}".format(robot.robot_name)
],
block=False),
transitions={"spoken": "CHECK_ARM_OCCUPIED"})
smach.StateMachine.add(
'SAY_PLACE_FAILED',
Say(robot, [
"I could not cleanup the item.",
"I cannot put the item in the trashbin",
"Item cleanup failed"
],
block=False),
transitions={"spoken": "CHECK_ARM_OCCUPIED"})
def execute(self, userdata=None):
d = ds.OccupiedArmDesignator(self._robot, {},
name="empty_arm_designator2")
if d.resolve():
return "yes"
return "no"
def __init__(self, robot, trashbin_designator, arm_designator):
"""
:param robot: robot object
:param trashbin_designator: EdEntityDesignator designating the trashbin
:param arm_designator: arm designator resolving to the arm with which to grab
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"])
place_pose_designator = ds.EmptySpotDesignator(robot, trashbin_designator)
with self:
# @cb_interface(outcomes=['done'])
# def _joint_goal(_):
# arm = arm_designator.resolve()
# if not arm:
# rospy.logerr("Could not resolve arm")
# return "failed"
# # Send to grab trash pose
# arm.send_joint_goal('grab_trash_bag')
# arm.wait_for_motion_done()
# return 'done'
#
# smach.StateMachine.add("JOINT_GOAL",
# CBState(_joint_goal),
# transitions={'done': 'GO_BIN'})
smach.StateMachine.add("GO_BIN",
states.NavigateToPlace(robot=robot, place_pose_designator=place_pose_designator,
arm_designator=arm_designator),
transitions={"arrived": "GET_BIN_POSITION",
"goal_not_defined": "aborted",
"unreachable": "failed"})
smach.StateMachine.add("GET_BIN_POSITION", GetTrashBin(robot=robot, trashbin=trashbin_designator),
transitions={"succeeded": "JOINT_PATH",
"failed": "failed"})
@cb_interface(outcomes=['done'])
def _joint_path(_):
robot.head.cancel_goal()
arm = arm_designator.resolve()
if not arm:
rospy.logerr("Could not resolve arm")
return "failed" # ToDo: fix
# Send to grab trash pose
arm._arm._send_joint_trajectory(
[[0.01, 0.0, -1.57, -1.57, 0.0],
[0.69, 0.0, -1.57, -1.57, 0.0],
[0.65, -2.2, -1.57, -1.57, 0.],
[0.65, -2.2, 0.0, -0.85, 0.]
]
)
arm.wait_for_motion_done()
return 'done'
smach.StateMachine.add("JOINT_PATH",
CBState(_joint_path),
transitions={'done': 'GO_TO_NEW_BIN'})
smach.StateMachine.add("GO_TO_NEW_BIN",
ControlToTrashBin(robot=robot, trashbin_id=trashbin_designator.id, radius=0.4,
yaw_offset=-0.2),
transitions={"done": "PREPARE_AND_GRAB"})
smach.StateMachine.add("PREPARE_AND_GRAB", GrabTrash(robot=robot, arm_designator=arm_designator),
transitions={"succeeded": "succeeded",
"failed": "ANNOUNCE_PICKUP_FAIL"})
smach.StateMachine.add("ANNOUNCE_PICKUP_FAIL",
states.Say(robot, "Unfortunately I could not pick up the trash myself, let's go to"
"plan B!",
block=False),
transitions={'spoken': 'ASK_HANDOVER'})
# Ask human to handover the trash bag
smach.StateMachine.add("ASK_HANDOVER", HandoverFromHumanFigure(robot=robot, arm_designator=arm_designator,
grabbed_entity_label='thrash'),
transitions={"succeeded": "LOWER_ARM",
"failed": "failed",
"timeout": "TIMEOUT"})
arm_occupied_designator = ds.OccupiedArmDesignator(robot=robot, arm_properties={})
smach.StateMachine.add("LOWER_ARM", states.ArmToJointConfig(robot=robot,
arm_designator=arm_occupied_designator,
configuration="reset"),
transitions={"succeeded": "RECEIVED_TRASH_BAG",
"failed": "RECEIVED_TRASH_BAG"})
smach.StateMachine.add("RECEIVED_TRASH_BAG", states.Say(robot, "I received the thrash bag. I will throw"
" it away, please move away.", block=True),
transitions={'spoken': 'succeeded'})
smach.StateMachine.add("TIMEOUT", states.Say(robot, "I have not received anything, so I will just continue",
block=False),
transitions={'spoken': "failed"})
