def with_area():
esd = ds.EmptySpotDesignator(
robot=robot,
place_location_designator=furniture_designator,
name="with_area",
area="on_top_of")
print(esd.resolve())
def __init__(self, robot, place_designator=None):
""" Constructor
:param robot: robot object
:param place_designator: Designator that resolves to the pose to place at. E.g. an EmptySpotDesignator
"""
smach.State.__init__(self, outcomes=["succeeded", "failed"])
self._robot = robot
if place_designator is not None:
self._place_designator = place_designator
else:
place_entity_designator = ds.EdEntityDesignator(
robot=robot, id=DEFAULT_PLACE_ENTITY)
self._place_designator = ds.EmptySpotDesignator(
robot=robot,
place_location_designator=place_entity_designator,
area=DEFAULT_PLACE_AREA)
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'])
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 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
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.place_designator = ds.EmptySpotDesignator(
robot=robot,
place_location_designator=self.place_entity_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: "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
# 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"
})
def without_area():
esd = ds.EmptySpotDesignator(
robot=robot,
place_location_designator=furniture_designator,
name=None)
print esd.resolve()
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"})
