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 __init__(self, robot):
smach.StateMachine.__init__(
self, outcomes=['container_success', 'container_failed'])
with self:
smach.StateMachine.add('SAY_STARTING_TEST',
states.Say(robot,
"Starting navigation test",
block=False),
transitions={'spoken': 'SAY_TEST_SYMBOLIC'})
smach.StateMachine.add('SAY_TEST_SYMBOLIC',
states.Say(robot,
"Testing Navigate To Symbolic",
block=False),
transitions={'spoken': 'NAV_TO_SYMB'})
smach.StateMachine.add(
'NAV_TO_SYMB',
states.NavigateToSymbolic(
robot,
{EntityByIdDesignator(robot, id="living_room"): "in"},
EntityByIdDesignator(robot, id="dinnertable")),
transitions={
'arrived': 'SAY_TEST_WAYPOINT',
'unreachable': 'SAY_FAILED_SYMBOLIC',
'goal_not_defined': 'SAY_FAILED_SYMBOLIC'
})
smach.StateMachine.add('SAY_FAILED_SYMBOLIC',
states.Say(robot,
"Failed Navigate To Symbolic.",
block=True),
transitions={'spoken': 'SAY_TEST_WAYPOINT'})
smach.StateMachine.add('SAY_TEST_WAYPOINT',
states.Say(robot,
"Testing Navigate To Waypoint.",
block=False),
transitions={'spoken': 'NAV_TO_WAYPOINT'})
smach.StateMachine.add(
'NAV_TO_WAYPOINT',
states.NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id=challenge_knowledge.wp_test_nav)),
transitions={
'arrived': 'container_success',
'unreachable': 'SAY_FAILED_WAYPOINT',
'goal_not_defined': 'SAY_FAILED_WAYPOINT'
})
smach.StateMachine.add('SAY_FAILED_WAYPOINT',
states.Say(robot,
"Failed reaching the waypoint.",
block=True),
transitions={'spoken': 'container_success'})
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, 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 __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, objectsIDsDesLocal):
smach.StateMachine.__init__(
self, outcomes=['container_success', 'container_failed'])
with self:
smach.StateMachine.add(
'SAY_STARTING_TEST',
states.Say(robot,
"Starting object pickup test test",
block=False),
transitions={'spoken': 'SAY_SEARCHING_OBJECTS'})
smach.StateMachine.add(
'SAY_SEARCHING_OBJECTS',
states.Say(
robot,
"I'm going to the dinning table to search for objects",
block=False),
transitions={'spoken': 'NAV_TO_TABLE'})
smach.StateMachine.add(
'NAV_TO_TABLE',
states.NavigateToSymbolic(
robot, {
EdEntityDesignator(robot,
id=challenge_knowledge.INSPECT_ROOM_ID):
"in"
},
EdEntityDesignator(
robot, id=challenge_knowledge.INSPECT_ENTITY_ID)),
transitions={
'arrived': 'SEGMENT_OBJECTS',
'unreachable': 'SAY_FAILED_NAV_TO_TABLE',
'goal_not_defined': 'SAY_FAILED_NAV_TO_TABLE'
})
smach.StateMachine.add(
'SAY_FAILED_NAV_TO_TABLE',
states.Say(
robot,
"I could not reach the table, but i will try to continue.",
block=True),
transitions={'spoken': 'SEGMENT_OBJECTS'})
smach.StateMachine.add(
"SEGMENT_OBJECTS",
states.SegmentObjects(
robot, objectsIDsDesLocal.writeable,
EdEntityDesignator(
robot, id=challenge_knowledge.INSPECT_ENTITY_ID),
"on_top_of"),
transitions={'done': 'PICKUP_OBJECT'})
smach.StateMachine.add('PICKUP_OBJECT',
test_states.PickUpRandomObj(
robot, objectsIDsDesLocal),
transitions={
'succeeded': 'SAY_I_HAVE_OBJ',
'failed': 'SAY_PICKUP_FAILED',
'no_objects': 'SAY_NO_OBJECTS'
})
smach.StateMachine.add('SAY_I_HAVE_OBJ',
states.Say(robot,
"I have the object!",
block=False),
transitions={'spoken': 'container_success'})
smach.StateMachine.add(
'SAY_PICKUP_FAILED',
states.Say(
robot,
"Something went wrong and i could not pick up the object!",
block=False),
transitions={'spoken': 'container_failed'})
smach.StateMachine.add('SAY_NO_OBJECTS',
states.Say(
robot,
"I don't see any objects to pick up!",
block=False),
transitions={'spoken': 'container_failed'})
def __init__(self, robot, location_des):
"""
Visit all selected locations from the list, and handle the found objects
:param location_des is a designator resolving to a dictionary with fields ... TODO
"""
smach.StateMachine.__init__(self, outcomes=['done'])
segment_areas = ValueByKeyDesignator(location_des,
"segment_areas", [str],
name='segment_areas')
segment_area = VariableDesignator(resolve_type=str,
name='segment_area')
navigation_area_des = ValueByKeyDesignator(location_des,
key='navigation_area',
resolve_type=str,
name='navigation_area')
location_id_des = ValueByKeyDesignator(location_des,
key='name',
resolve_type=str,
name='location_id')
room_des = EdEntityDesignator(robot,
id_designator=ValueByKeyDesignator(
location_des,
key="room",
resolve_type=str))
# Set up the designators for this machine
e_classifications_des = VariableDesignator(
[],
resolve_type=[ClassificationResult],
name='e_classifications_des')
e_des = EdEntityDesignator(robot,
id_designator=location_id_des,
name='e_des')
with self:
smach.StateMachine.add('ITERATE_NEXT_AREA',
states.IterateDesignator(
segment_areas, segment_area.writeable),
transitions={
"next": 'INSPECT',
"stop_iteration": "done"
})
# Segment the area and handle segmented objects for the specified navigation area
# for i, segment_area in enumerate(segment_areas):
smach.StateMachine.add('INSPECT',
states.NavigateToSymbolic(
robot, {e_des: navigation_area_des},
e_des),
transitions={
'arrived': 'SEGMENT_SAY',
'unreachable': "SAY_UNREACHABLE",
'goal_not_defined': "SAY_UNREACHABLE"
})
smach.StateMachine.add("SEGMENT_SAY",
states.Say(robot,
"Looking {area} the {entity}",
area=segment_area,
entity=location_id_des,
block=False),
transitions={"spoken": "RISE"})
smach.StateMachine.add('RISE',
states.RiseForInspect(
robot, e_des, segment_area),
transitions={
'succeeded': 'SEGMENT',
'failed': 'SEGMENT'
})
smach.StateMachine.add(
'SEGMENT',
states.SegmentObjects(robot, e_classifications_des.writeable,
e_des, segment_area),
transitions={'done': "HANDLE_DETECTED_ENTITIES"})
smach.StateMachine.add("SAY_UNREACHABLE",
states.Say(robot,
"I failed to inspect the {furn}",
furn=location_id_des,
block=True),
transitions={"spoken": "done"})
smach.StateMachine.add("HANDLE_DETECTED_ENTITIES",
HandleDetectedEntities(
robot, e_classifications_des,
location_id_des, segment_area,
room_des),
transitions={"done": "ITERATE_NEXT_AREA"})
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=["Done", "Aborted"])
hmi_result_des = ds.VariableDesignator(resolve_type=HMIResult)
information_point_id_designator = ds.FuncDesignator(ds.AttrDesignator(
hmi_result_des, "semantics", resolve_type=unicode),
str,
resolve_type=str)
information_point_designator = ds.EdEntityDesignator(
robot, id_designator=information_point_id_designator)
with self:
single_item = InformMachine(robot)
if START_ROBUST:
smach.StateMachine.add("START_CHALLENGE",
states.StartChallengeRobust(
robot, INITIAL_POSE_ID),
transitions={
"Done": "ASK_WHERE_TO_GO",
"Aborted": "Aborted",
"Failed": "Aborted"
})
smach.StateMachine.add(
"ASK_WHERE_TO_GO",
states.Say(
robot,
"Near which furniture object should I go to start guiding people?"
),
transitions={"spoken": "WAIT_WHERE_TO_GO"})
smach.StateMachine.add(
"WAIT_WHERE_TO_GO",
states.HearOptionsExtra(
robot=robot,
spec_designator=ds.Designator(
initial_value=START_GRAMMAR),
speech_result_designator=hmi_result_des.writeable),
transitions={
"heard": "ASK_CONFIRMATION",
"no_result": "ASK_WHERE_TO_GO"
}) # ToDo: add fallbacks #option: STORE_STARTING_POSE
smach.StateMachine.add(
"ASK_CONFIRMATION",
states.Say(robot, [
"I hear that you would like me to start the tours at"
" the {place}, is this correct?"
],
place=information_point_id_designator,
block=True),
transitions={"spoken": "CONFIRM_LOCATION"})
smach.StateMachine.add("CONFIRM_LOCATION",
states.HearOptions(
robot=robot, options=["yes", "no"]),
transitions={
"yes": "MOVE_OUT_OF_MY_WAY",
"no": "ASK_WHERE_TO_GO",
"no_result": "ASK_WHERE_TO_GO"
})
smach.StateMachine.add(
"MOVE_OUT_OF_MY_WAY",
states.Say(robot,
"Please move your ass so I can get going!"),
transitions={"spoken": "TC_MOVE_TIME"})
smach.StateMachine.add("TC_MOVE_TIME",
states.WaitTime(robot=robot,
waittime=3),
transitions={
"waited": "NAV_TO_START",
"preempted": "Aborted"
})
smach.StateMachine.add(
"NAV_TO_START",
states.NavigateToSymbolic(
robot=robot,
entity_designator_area_name_map={
information_point_designator: "in_front_of"
},
entity_lookat_designator=information_point_designator),
transitions={
"arrived": "TURN_AROUND",
"unreachable": "WAIT_NAV_BACKUP",
"goal_not_defined": "Aborted"
}) # If this happens: never mind
smach.StateMachine.add("WAIT_NAV_BACKUP",
states.WaitTime(robot, 3.0),
transitions={
"waited": "NAV_TO_START_BACKUP",
"preempted": "Aborted"
})
smach.StateMachine.add(
"NAV_TO_START_BACKUP",
states.NavigateToSymbolic(
robot=robot,
entity_designator_area_name_map={
information_point_designator: "near"
},
entity_lookat_designator=information_point_designator),
transitions={
"arrived": "TURN_AROUND",
"unreachable":
"SAY_CANNOT_REACH_WAYPOINT", # Current pose backup
"goal_not_defined": "Aborted"
}) # If this happens: never mind
@smach.cb_interface(outcomes=["done"])
def _turn_around(userdata=None):
""" Turns the robot approximately 180 degrees around """
v_th = 0.5
robot.base.force_drive(vx=0.0,
vy=0.0,
vth=v_th,
timeout=math.pi / v_th)
return "done"
smach.StateMachine.add(
"TURN_AROUND",
smach.CBState(_turn_around),
transitions={"done": "STORE_STARTING_POSE"})
smach.StateMachine.add(
"SAY_CANNOT_REACH_WAYPOINT",
states.Say(
robot, "I am not able to reach the starting point."
"I'll use this as starting point"),
transitions={"spoken": "STORE_STARTING_POSE"})
else:
smach.StateMachine.add("INITIALIZE",
states.Initialize(robot),
transitions={
"initialized":
"STORE_STARTING_POSE",
"abort": "Aborted"
})
## This is purely for a back up scenario until the range iterator
@smach.cb_interface(outcomes=["succeeded"])
def store_pose(userdata=None):
base_loc = robot.base.get_location()
base_pose = base_loc.frame
location_id = INFORMATION_POINT_ID
robot.ed.update_entity(id=location_id,
frame_stamped=FrameStamped(
base_pose, "/map"),
type="waypoint")
return "succeeded"
smach.StateMachine.add("STORE_STARTING_POSE",
smach.CBState(store_pose),
transitions={"succeeded": "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(1000),
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"})
def __init__(self, robot, location_id, room_id, navigate_area,
segment_areas, known_types):
smach.StateMachine.__init__(self, outcomes=['done'])
# Set up the designators for this machine
e_classifications_des = VariableDesignator(
[],
resolve_type=[ClassificationResult],
name='e_classifications_des')
e_des = EdEntityDesignator(robot, id=location_id, name='e_des')
room_des = EntityByIdDesignator(robot, id=room_id, name='room_des')
with self:
# Loop over the areas that we have to segment and handle segmented objects for the specified navigation area
for i, segment_area in enumerate(segment_areas):
smach.StateMachine.add("RESET_ED_%d" % i,
robot_smach_states.ResetED(robot),
transitions={'done': 'NAVIGATE_%d' % i})
smach.StateMachine.add("NAVIGATE_%d" % i,
robot_smach_states.NavigateToSymbolic(
robot, {
e_des: navigate_area,
room_des: "in"
}, e_des),
transitions={
'arrived':
'SEGMENT_SAY_%d' % i,
'unreachable':
"SAY_UNREACHABLE_%d" % i,
'goal_not_defined':
"SAY_UNREACHABLE_%d" % i
})
smach.StateMachine.add(
"SEGMENT_SAY_%d" % i,
robot_smach_states.Say(
robot,
["Looking %s the %s" % (segment_area, location_id)],
block=False),
transitions={"spoken": "SEGMENT_%d" % i})
smach.StateMachine.add(
'SEGMENT_%d' % i,
robot_smach_states.SegmentObjects(
robot, e_classifications_des.writeable, e_des,
segment_area),
transitions={'done': "HANDLE_DETECTED_ENTITIES_%d" % i})
# Determine the next state, either it is the next iter or done
next_state = "RESET_ED_%d" % (
i + 1) if i + 1 < len(segment_areas) else "done"
smach.StateMachine.add(
"SAY_UNREACHABLE_%d" % i,
robot_smach_states.Say(
robot, ["I failed to inspect the %s" % location_id],
block=True),
transitions={"spoken": next_state})
smach.StateMachine.add("HANDLE_DETECTED_ENTITIES_%d" % i,
HandleDetectedEntities(
robot, e_classifications_des,
known_types, location_id,
segment_area),
transitions={"done": next_state})
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
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")
def __init__(self,
robot,
entity_des,
operator_distance=1.5,
operator_radius=0.5):
""" Constructor
:param robot: robot object
:param entity_des: designator resolving to a room or a piece of furniture
:param operator_distance: (float) check for the operator to be within this range of the robot
:param operator_radius: (float) from the point behind the robot defined by `distance`, the person must be within this radius
"""
smach.StateMachine.__init__(self,
outcomes=[
"arrived", "unreachable",
"goal_not_defined", "lost_operator",
"preempted"
])
self.operator_distance = operator_distance
self.operator_radius = operator_radius
with self:
@smach.cb_interface(outcomes=["room", "object"])
def determine_type(userdata=None):
entity = entity_des.resolve()
entity_type = entity.type
if entity_type == "room":
return "room"
else:
return "object"
smach.StateMachine.add("DETERMINE_TYPE",
smach.CBState(determine_type),
transitions={
"room": "GUIDE_TO_ROOM",
"object": "GUIDE_TO_FURNITURE"
})
smach.StateMachine.add(
"GUIDE_TO_ROOM",
guidance.GuideToSymbolic(
robot, {entity_des: "in"},
entity_des,
operator_distance=self.operator_distance,
operator_radius=self.operator_radius),
transitions={
"arrived": "arrived",
"unreachable": "WAIT_ROOM_BACKUP",
"goal_not_defined": "goal_not_defined",
"lost_operator": "ROOM_NAV_BACKUP",
"preempted": "preempted"
})
smach.StateMachine.add("WAIT_ROOM_BACKUP",
states.WaitTime(robot, 3.0),
transitions={
"waited": "GUIDE_TO_ROOM_BACKUP",
"preempted": "preempted"
})
smach.StateMachine.add(
"GUIDE_TO_ROOM_BACKUP",
guidance.GuideToSymbolic(
robot, {entity_des: "in"},
entity_des,
operator_distance=self.operator_distance,
operator_radius=self.operator_radius),
transitions={
"arrived": "arrived",
"unreachable": "unreachable",
"goal_not_defined": "goal_not_defined",
"lost_operator": "ROOM_NAV_BACKUP",
"preempted": "preempted"
})
smach.StateMachine.add(
"GUIDE_TO_FURNITURE",
guidance.GuideToSymbolic(
robot, {entity_des: "in_front_of"},
entity_des,
operator_distance=self.operator_distance,
operator_radius=self.operator_radius),
transitions={
"arrived": "arrived",
"unreachable":
"WAIT_FURNITURE_BACKUP", # Something is blocking
"goal_not_defined":
"GUIDE_NEAR_FURNITURE", # in_front_of not defined
"lost_operator": "FURNITURE_NAV_BACKUP",
"preempted": "preempted"
})
smach.StateMachine.add(
"GUIDE_NEAR_FURNITURE",
guidance.GuideToSymbolic(
robot, {entity_des: "near"},
entity_des,
operator_distance=self.operator_distance,
operator_radius=self.operator_radius),
transitions={
"arrived": "arrived",
"unreachable": "WAIT_FURNITURE_BACKUP",
"goal_not_defined": "goal_not_defined",
"lost_operator": "FURNITURE_NAV_BACKUP",
"preempted": "preempted"
})
smach.StateMachine.add("WAIT_FURNITURE_BACKUP",
states.WaitTime(robot, 3.0),
transitions={
"waited": "GUIDE_NEAR_FURNITURE_BACKUP",
"preempted": "preempted"
})
smach.StateMachine.add(
"GUIDE_NEAR_FURNITURE_BACKUP",
guidance.GuideToSymbolic(
robot, {entity_des: "near"},
entity_des,
operator_distance=self.operator_distance,
operator_radius=self.operator_radius),
transitions={
"arrived": "arrived",
"unreachable": "unreachable",
"goal_not_defined": "goal_not_defined",
"lost_operator": "FURNITURE_NAV_BACKUP",
"preempted": "preempted"
})
smach.StateMachine.add("ROOM_NAV_BACKUP",
states.NavigateToSymbolic(
robot, {entity_des: "in"}, entity_des),
transitions={
"arrived": "SAY_ARRIVED",
"unreachable": "unreachable",
"goal_not_defined": "goal_not_defined",
})
smach.StateMachine.add("FURNITURE_NAV_BACKUP",
states.NavigateToSymbolic(
robot, {entity_des: "near"},
entity_des),
transitions={
"arrived": "SAY_ARRIVED",
"unreachable": "unreachable",
"goal_not_defined": "goal_not_defined",
})
smach.StateMachine.add(
"SAY_ARRIVED",
states.Say(
robot,
"We have arrived. I'll go back to the meeting point"),
transitions={"spoken": "arrived"})
def __init__(self, robot):
""" Constructor
:param robot: robot object
"""
smach.StateMachine.__init__(self, outcomes=["Done", "Aborted"])
# Designators
furniture_move_designator = PointingDesignator(robot=robot)
furniture_pick_designator = PointingDesignator(robot=robot)
furniture_place_designator = PointingDesignator(robot=robot)
with self:
# Start challenge
smach.StateMachine.add("START_CHALLENGE",
states.StartChallengeRobust(
robot=robot,
initial_pose=knowledge.initial_pose),
transitions={
"Done": "NAVIGATE_TO_MEETING_POINT0",
"Aborted": "Aborted",
"Failed": "Aborted"
})
# Move to meeting point
# ToDo: add challenge knowledge
smach.StateMachine.add("NAVIGATE_TO_MEETING_POINT0",
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=EdEntityDesignator(
robot=robot,
id=knowledge.meeting_point),
radius=0.15),
transitions={
"arrived": "DETECT_POINTING0",
"unreachable": "DETECT_POINTING0",
"goal_not_defined": "DETECT_POINTING0"
})
# Wait for the operator to appear and detect what he's pointing at
smach.StateMachine.add("DETECT_POINTING0",
PointingDetector(
robot=robot,
designator=furniture_move_designator,
default_entity_id="couch_table",
super_type="furniture"),
transitions={
"succeeded": "MOVE_TO_ITEM",
"failed": "DETECT_POINTING0"
})
# Move to jury table
smach.StateMachine.add("MOVE_TO_ITEM",
states.NavigateToSymbolic(
robot,
{furniture_move_designator: "near"},
furniture_move_designator),
transitions={
"arrived": "SAY_HI_TO_JURY",
"unreachable": "SAY_HI_TO_JURY",
"goal_not_defined": "SAY_HI_TO_JURY"
})
smach.StateMachine.add(
"SAY_HI_TO_JURY",
states.Say(robot, "Hi guys, I'm glad you're here", block=True),
transitions={"spoken": "NAVIGATE_TO_MEETING_POINT1"})
smach.StateMachine.add("NAVIGATE_TO_MEETING_POINT1",
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=EdEntityDesignator(
robot=robot,
id=knowledge.meeting_point),
radius=0.15),
transitions={
"arrived": "DETECT_POINTING1",
"unreachable": "DETECT_POINTING1",
"goal_not_defined": "DETECT_POINTING1"
})
# Wait for the operator to appear and detect what he's pointing at
smach.StateMachine.add("DETECT_POINTING1",
PointingDetector(
robot=robot,
designator=furniture_pick_designator,
default_entity_id="desk",
super_type="furniture"),
transitions={
"succeeded": "PICKUP_ITEM",
"failed": "DETECT_POINTING1"
})
# Inspect and pickup
smach.StateMachine.add(
"PICKUP_ITEM",
PickupItem(robot=robot,
furniture_designator=furniture_pick_designator),
transitions={
"succeeded": "NAVIGATE_TO_MEETING_POINT2",
"failed": "NAVIGATE_TO_MEETING_POINT2"
})
# Move back to meeting point
# ToDo: add challenge knowledge
smach.StateMachine.add("NAVIGATE_TO_MEETING_POINT2",
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=EdEntityDesignator(
robot=robot,
id=knowledge.meeting_point),
radius=0.15),
transitions={
"arrived": "DETECT_POINTING2",
"unreachable": "DETECT_POINTING2",
"goal_not_defined": "DETECT_POINTING2"
})
# Wait for the operator to appear and detect what he's pointing at
smach.StateMachine.add("DETECT_POINTING2",
PointingDetector(
robot=robot,
designator=furniture_place_designator,
default_entity_id="side_table",
super_type="furniture"),
transitions={
"succeeded": "PLACE_ITEM",
"failed": "DETECT_POINTING2"
})
# Place the object
smach.StateMachine.add(
"PLACE_ITEM",
PlaceSingleItem(robot=robot,
place_designator=furniture_place_designator),
transitions={
"succeeded": "SAY_DONE",
"failed": "SAY_DONE"
})
# Say that we're done
smach.StateMachine.add(
"SAY_DONE",
states.Say(
robot,
"That's it for today, you can have a closer look at my"
"skills on the screen",
block=False),
transitions={"spoken": "RESET_HEAD"})
@smach.cb_interface(outcomes=["done"])
def reset_head(userdata=None):
""" Resets the head """
print "Resetting head"
robot.head.reset()
rospy.sleep(rospy.Duration(1.0))
return "done"
smach.StateMachine.add("RESET_HEAD",
smach.CBState(reset_head),
transitions={"done": "Done"})
robot = Robot()
if len(sys.argv) > 2:
ids = sys.argv[2:]
else:
robot.speech.speak("No ids specified, I will do them all", block=False)
ids = [e.id for e in robot.ed.get_entities() if e.is_a("furniture")]
random.shuffle(ids)
print "IDS:", ids
for id in ids:
robot.speech.speak("I am going to navigate to the %s" % id, block=False)
machine = robot_smach_states.NavigateToSymbolic(robot, {robot_smach_states.util.designators.EntityByIdDesignator(robot, id=id): "in_front_of"},
robot_smach_states.util.designators.EntityByIdDesignator(robot, id=id))
machine.execute()
robot.head.look_down()
robot.head.wait_for_motion_done()
import time
time.sleep(1)
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# Segment
robot.speech.speak("Segmenting on top of the %s" % id, block=False)
segmented_entities = robot.ed.update_kinect("on_top_of %s" % id)
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:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={'initialized': 'AWAIT_START',
'abort': 'Aborted'})
smach.StateMachine.add("AWAIT_START",
states.AskContinue(robot),
transitions={'continue': "MOVE_TABLE",
'no_response': 'AWAIT_START'})
@smach.cb_interface(outcomes=["done"])
def move_table(userdata=None):
""" 'Locks' a locking designator """
# For now, don't do anything
return "done"
# Move away the cabinet
robot.ed.update_entity(id="cabinet",
frame_stamped=FrameStamped(frame=kdl.Frame(kdl.Rotation(),
kdl.Vector(12.0, 0, 0)),
frame_id="map"))
# 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())
# Update the world model
robot.ed.update_entity(id=CABINET, frame_stamped=ENTITY_POSES[0][0])
robot.ed.update_entity(id=TABLE, frame_stamped=ENTITY_POSES[0][1])
return "done"
smach.StateMachine.add("MOVE_TABLE",
smach.CBState(move_table),
transitions={'done': 'NAV_TO_START'})
cabinet = ds.EntityByIdDesignator(robot, id=CABINET)
room = ds.EntityByIdDesignator(robot, id=ROOM)
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),
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:
single_item = ManipulateMachine(robot, pdf_writer=pdf_writer) # ToDo: add more pdf stuff
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")
