def __init__(self, robot):
smach.StateMachine.__init__(
self, outcomes=['succeeded', 'failed', 'aborted'])
self.door_waypoint = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.waypoint_door['id'])
self.livingroom_waypoint = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.waypoint_livingroom['id'])
self.operator_designator = ds.VariableDesignator(
resolve_type=Entity)
self.guest1_entity_des = ds.VariableDesignator(
resolve_type=Entity, name='guest1_entity')
self.guest1_name_des = ds.VariableDesignator('guest 1',
name='guest1_name')
self.guest1_drink_des = ds.VariableDesignator(
resolve_type=HMIResult, name='guest1_drink')
self.guest1_drinkname_des = ds.FieldOfHMIResult(
self.guest1_drink_des,
semantics_field='drink',
name='guest1_drinkname')
with self:
smach.StateMachine.add('LEARN_GUEST',
LearnGuest(robot, self.door_waypoint,
self.guest1_entity_des,
self.guest1_name_des,
self.guest1_drink_des),
transitions={
'succeeded': 'succeeded',
'failed': 'failed',
'aborted': 'aborted'
})
def test_iteration():
sm = smach.StateMachine(outcomes=['succeeded', 'failed'])
numbers = ds.VariableDesignator(range(3), resolve_type=[int])
number = ds.VariableDesignator(resolve_type=int)
global gather
gather = []
with sm:
smach.StateMachine.add( "STEP",
IteratorState(numbers.writeable, number.writeable),
transitions={ "next" :"USE_NUMBER",
"stop_iteration":"succeeded"})
def lengthen_list(*args, **kwargs):
resolved = number.resolve()
print resolved
global gather
gather += [resolved]
smach.StateMachine.add( "USE_NUMBER",
CallFunction("amigo", lengthen_list),
transitions={ "succeeded" :"STEP",
"failed" :"failed"})
sm.execute()
print "gather = {0}".format(gather)
print "numbers.resolve() = {0}".format(numbers.resolve())
assert numbers.resolve() == []
assert gather == [0, 1, 2]
def arm_left_grab(pre):
global arm_left_object_in_arm
global task_object_grabbed
global left_grab_exec_object_in_WS
classification_result_designator = ds.VariableDesignator([], resolve_type=[ClassificationResult])
# Inspect the waypoint and classify objects on it
segmSM = SegmentObjects(robot, classification_result_designator.writeable, wp)
status = segmSM.execute()
grab_obj = ds.VariableDesignator(resolve_type=Entity)
# take the first seen coke
for obj in classification_result_designator.resolve():
if obj.type == 'coke':
grab_obj = ds.EntityByIdDesignator(robot, type=obj.id)
break
grabSM = Grab(robot, grab_obj, leftarm)
status = grabSM.execute()
if status == 'done':
arm_left_object_in_arm = True
task_object_grabbed = True
# left_grab_exec_object_in_WS = 0
else:
arm_left_object_in_arm = False # RETRY
task_object_grabbed = False
Amigo_AssignInputVariables()
def __init__(self, robot, entityDes, objectIDsDes=None, searchArea="on_top_of", navigation_area="",
threshold=0.0):
""" Constructor
:param robot: robot object
:param entityDes: EdEntityDesignator indicating the (furniture) object to inspect
:param objectIDsDes: designator that is used to store the segmented objects
:param searchArea: string defining where the objects are w.r.t. the entity, default = on_top_of
:param navigation_area: string identifying the inspection area. If provided, NavigateToSymbolic is used.
If left empty, NavigateToObserve is used.
:param threshold: float for classification score. Entities whose classification score is lower are ignored
(i.e. are not added to the segmented_entity_ids_designator)
"""
smach.StateMachine.__init__(self, outcomes=['done', 'failed'])
if not objectIDsDes:
objectIDsDes = ds.VariableDesignator([], resolve_type=[ClassificationResult])
with self:
if navigation_area:
smach.StateMachine.add('NAVIGATE_TO_INSPECT', NavigateToSymbolic(robot, {entityDes: navigation_area},
entityDes),
transitions={'unreachable': 'failed',
'goal_not_defined': 'failed',
'arrived': 'SEGMENT'})
else:
smach.StateMachine.add('NAVIGATE_TO_INSPECT', NavigateToObserve(robot, entityDes, radius=1.0),
transitions={'unreachable': 'failed',
'goal_not_defined': 'failed',
'arrived': 'SEGMENT'})
smach.StateMachine.add('SEGMENT', SegmentObjects(robot, objectIDsDes.writeable, entityDes, searchArea,
threshold=threshold),
transitions={'done': 'done'})
def __init__(self,
robot,
segmented_entity_ids_designator,
entity_to_inspect_designator,
segmentation_area="on_top_of"):
""" Constructor
:param robot: robot object
:param segmented_entity_ids_designator: designator that is used to store the segmented objects
:param entity_to_inspect_designator: EdEntityDesignator indicating the (furniture) object to inspect
:param segmentation_area: string defining where the objects are w.r.t. the entity, default = on_top_of
"""
smach.State.__init__(self, outcomes=["done"])
self.robot = robot
ds.check_resolve_type(entity_to_inspect_designator, Entity)
self.entity_to_inspect_designator = entity_to_inspect_designator
if isinstance(segmentation_area, str):
self.segmentation_area_des = ds.VariableDesignator(
segmentation_area)
else:
# ds.check_resolve_type(segmentation_area, "str")
self.segmentation_area_des = segmentation_area
ds.check_resolve_type(segmented_entity_ids_designator,
[ClassificationResult])
ds.is_writeable(segmented_entity_ids_designator)
self.segmented_entity_ids_designator = segmented_entity_ids_designator
def __init__(self, robot, where_to_count_designator, type_to_count_designator, count_designator):
"""
:param robot: robot object
:param where_to_count_designator: Where to look for objects?
:param type_to_count_designator: a VariableDesignator (resolve_type=string) that stores the type of the objects
that should be counted
:param count_designator: VariableDesignator(resolve_type=int).writeable() that will store the number
of objects
"""
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
entities = ds.VariableDesignator([], resolve_type=[ClassificationResult])
with self:
smach.StateMachine.add("INSPECT_TABLE", Inspect(robot=robot, entityDes=where_to_count_designator,
objectIDsDes=entities,
searchArea="on_top_of",
navigation_area="in_front_of"),
transitions={"done": "COUNT",
"failed": "Aborted"})
smach.StateMachine.add("COUNT",
CountObjectsOnLocation(robot,
location=where_to_count_designator,
segmented_objects_designator=entities,
object_type=type_to_count_designator,
num_objects_designator=count_designator.writeable),
transitions={'done': 'Done',
'failed': 'Aborted'})
def __init__(self, robot, room_grammar, roomw, cleanup_locationsw):
smach.StateMachine.__init__(self, outcomes=["done"])
"""
Ask the operator which room has to be cleaned
"""
hmi_result_des = ds.VariableDesignator(resolve_type=hmi.HMIResult,
name="hmi_result_des")
room_name_des = ds.FuncDesignator(ds.AttrDesignator(
hmi_result_des, "semantics", resolve_type=unicode),
str,
resolve_type=str)
@smach.cb_interface(outcomes=['done'])
def write_room(ud, des_read, des_write):
# type: (object, ds.Designator, ds.Designator) -> str
assert (ds.is_writeable(des_write))
assert (des_write.resolve_type == des_read.resolve_type)
des_write.write(des_read.resolve())
return 'done'
with self:
smach.StateMachine.add("ASK_WHICH_ROOM",
Say(robot,
"Which room should I clean for you?",
block=True),
transitions={"spoken": "HEAR_ROOM"})
smach.StateMachine.add("HEAR_ROOM",
HearOptionsExtra(
robot, room_grammar,
ds.writeable(hmi_result_des)),
transitions={
"heard": "SAY_HEARD_CORRECT",
"no_result": "ASK_WHICH_ROOM"
})
smach.StateMachine.add(
"SAY_HEARD_CORRECT",
Say(robot,
"I understood that the {room} should be cleaned, is this correct?",
room=room_name_des,
block=True),
transitions={"spoken": "HEAR_CORRECT"})
smach.StateMachine.add("HEAR_CORRECT",
AskYesNo(robot),
transitions={
"yes": "FILL_LOCATIONS",
"no": "ASK_WHICH_ROOM",
"no_result": "ASK_WHICH_ROOM"
})
smach.StateMachine.add("FILL_LOCATIONS",
RoomToCleanUpLocations(
challenge_knowledge, room_name_des,
cleanup_locationsw),
transitions={"done": "WRITE_ROOM"})
smach.StateMachine.add('WRITE_ROOM',
smach.CBState(
write_room,
cb_args=[room_name_des, roomw]),
transitions={'done': 'done'})
def setUp(self):
self.arm_ds = ds.ArmDesignator(
self.robot, {
'required_arm_name': 'leftArm',
"required_gripper_types": [arms.GripperTypes.GRASPING]
})
self.entity = Entity("12345", "dummy", "/map", None, None, {}, None, 0)
self.entity_ds = ds.VariableDesignator(self.entity)
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted'])
runs = ds.Designator([0, 1])
run = ds.VariableDesignator(resolve_type=int)
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': 'HANDLE_GUEST_1',
"preempted": 'aborted',
'error': 'HANDLE_GUEST_1'
})
smach.StateMachine.add('HANDLE_GUEST_1',
HandleSingleGuest(robot, assume_john=True),
transitions={
'succeeded': 'HANDLE_GUEST_2',
'aborted': 'HANDLE_GUEST_2'
})
smach.StateMachine.add('HANDLE_GUEST_2',
HandleSingleGuest(robot, assume_john=False),
transitions={
'succeeded': 'SAY_DONE',
'aborted': 'SAY_DONE'
})
smach.StateMachine.add(
'SAY_DONE',
states.Say(robot,
["That's all folks, my job is done, bye bye!"],
block=False),
transitions={'spoken': 'GO_BACK'})
smach.StateMachine.add(
'GO_BACK',
states.NavigateToWaypoint(
robot,
ds.EntityByIdDesignator(
robot, id=challenge_knowledge.waypoint_door['id']),
challenge_knowledge.waypoint_door['radius']),
transitions={
'arrived': 'succeeded',
'unreachable': 'succeeded',
'goal_not_defined': 'succeeded'
})
def setup_statemachine(robot):
state_machine = StateMachine(outcomes=['done'])
furniture_designator = ds.VariableDesignator(resolve_type=Entity)
entity_designator = ds.VariableDesignator(resolve_type=Entity)
arm_designator = ds.UnoccupiedArmDesignator(robot, {})
with state_machine:
# Intro
StateMachine.add('START_CHALLENGE_ROBUST', StartChallengeRobust(robot, STARTING_POINT),
transitions={'Done': 'SAY_START',
'Aborted': 'done',
'Failed': 'SAY_START'})
# Say we're gonna start
StateMachine.add('SAY_START', Say(robot, "Hand me that it is!", block=False),
transitions={'spoken': 'NAVIGATE_TO_START'})
# Drive to the start location
StateMachine.add('NAVIGATE_TO_START',
NavigateToWaypoint(robot, ds.EdEntityDesignator(robot, id=HOME_LOCATION)),
transitions={'arrived': 'GET_FURNITURE_FROM_OPERATOR_POSE',
'unreachable': 'NAVIGATE_TO_START', # ToDo: other fallback
'goal_not_defined': 'done'}) # I'm not even going to fill this in
# The pre-work
StateMachine.add('GET_FURNITURE_FROM_OPERATOR_POSE',
GetFurnitureFromOperatorPose(robot, furniture_designator.writeable),
transitions={'done': 'INSPECT_FURNITURE'})
# Go to the furniture object that was pointing to see what's there
StateMachine.add('INSPECT_FURNITURE',
InspectFurniture(robot, furniture_designator, entity_designator.writeable),
transitions={"succeeded": "IDENTIFY_OBJECT",
"failed": "NAVIGATE_TO_START"}) # If no entities, try again
# Point at the object
StateMachine.add('IDENTIFY_OBJECT',
IdentifyObject(robot, entity_designator, arm_designator),
transitions={'done': 'NAVIGATE_TO_START', # Just keep on going
'failed': 'NAVIGATE_TO_START'}) # Just keep on going
return state_machine
def __init__(self, robot, category_grammar, categoryw):
smach.StateMachine.__init__(self, outcomes=["done"])
hmi_result_des = ds.VariableDesignator(resolve_type=hmi.HMIResult,
name="hmi_result_des")
category_des = ds.FuncDesignator(ds.AttrDesignator(
hmi_result_des, "semantics", resolve_type=unicode),
str,
resolve_type=str)
@smach.cb_interface(outcomes=['done'])
def write_category(ud, des_read, des_write):
# type: (object, ds.Designator, ds.Designator) -> str
assert (ds.is_writeable(des_write))
assert (des_write.resolve_type == des_read.resolve_type)
des_write.write(des_read.resolve())
return 'done'
with self:
smach.StateMachine.add(
"ASK_WHERE_TO_DROP",
Say(robot,
"Please look at the object in my gripper and tell me"
"which category it is. If it should be thrown away,"
"call it trash",
block=True),
transitions={"spoken": "HEAR_LOCATION"})
smach.StateMachine.add("HEAR_LOCATION",
HearOptionsExtra(
robot, category_grammar,
ds.writeable(hmi_result_des)),
transitions={
"heard": "SAY_HEARD_CORRECT",
"no_result": "ASK_WHERE_TO_DROP"
})
smach.StateMachine.add(
"SAY_HEARD_CORRECT",
Say(robot,
"I understood that the object is of category {category}, is this correct?",
category=category_des,
block=True),
transitions={"spoken": "HEAR_CORRECT"})
smach.StateMachine.add("HEAR_CORRECT",
AskYesNo(robot),
transitions={
"yes": "WRITE_CATEGORY",
"no": "ASK_WHERE_TO_DROP",
"no_result": "ASK_WHERE_TO_DROP"
})
smach.StateMachine.add('WRITE_CATEGORY',
smach.CBState(
write_category,
cb_args=[category_des, categoryw]),
transitions={'done': 'done'})
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
self.robot = robot
@smach.cb_interface(outcomes=['done'])
def look_at_standing_person(userdata=None):
robot.head.look_at_standing_person()
return 'done'
with self:
smach.StateMachine.add('LOOK_AT_OPERATOR',
smach.CBState(look_at_standing_person),
transitions={'done': 'SAY_LOOK_AT_ME'})
smach.StateMachine.add(
'SAY_LOOK_AT_ME',
states.Say(
robot,
"Please stand one meter in front of me and look at me while I \
learn to recognize your face.",
block=True),
transitions={'spoken': 'LEARN_PERSON'})
smach.StateMachine.add(
'LEARN_PERSON',
states.LearnPerson(
robot, name_designator=ds.VariableDesignator("operator")),
transitions={
'succeeded_learning': 'SAY_OPERATOR_LEARNED',
'failed_learning': 'SAY_FAILED_LEARNING',
'timeout_learning': 'SAY_FAILED_LEARNING'
})
smach.StateMachine.add(
'SAY_FAILED_LEARNING',
states.Say(
robot,
"I could not learn my operator's face. Let me try again.",
block=True),
transitions={'spoken': 'failed'})
smach.StateMachine.add(
'SAY_OPERATOR_LEARNED',
states.Say(
robot,
"Now i know what you look like. Please go mix with the crowd."
),
transitions={'spoken': 'succeeded'})
def execute(self, userdata=None):
limit_reached = 0
for i in range(self._nr_tries):
rospy.loginfo("AskPersonName")
self.robot.speech.speak("What is your name?", block=True)
names_spec = "T['name': N] -> NAME[N]\n\n"
for dn in self.name_options:
names_spec += "NAME['{name}'] -> {name}\n".format(name=dn)
spec = ds.Designator(names_spec)
answer = ds.VariableDesignator(resolve_type=HMIResult)
state = HearOptionsExtra(self.robot, spec, answer.writeable)
try:
outcome = state.execute()
if not outcome == "heard":
limit_reached += 1
if limit_reached == self._nr_tries:
self.person_name_des.write(self.default_name)
rospy.logwarn(
"Speech recognition outcome was not successful. Using default name '{}'"
.format(self.person_name_des.resolve()))
return 'failed'
if outcome == "heard":
try:
rospy.logdebug("Answer: '{}'".format(answer.resolve()))
name = answer.resolve().semantics["name"]
rospy.loginfo(
"This person's name is: '{}'".format(name))
self.person_name_des.write(str(name))
rospy.loginfo(
"Result received from speech recognition is '" +
name + "'")
except KeyError as ke:
rospy.loginfo("KeyError resolving the name heard: " +
str(ke))
return 'failed'
return 'succeeded'
except TimeoutException:
return "timeout"
return 'succeeded'
def test_handover_from_human(self):
entitydes = ds.VariableDesignator(self.entity)
state = HandoverFromHuman(self.robot, self.arm_ds, "", entitydes)
state.check_consistency()
self.assertEqual(state.execute(), "succeeded")
self.robot.arms["leftArm"].send_joint_goal.assert_not_called()
self.robot.arms["leftArm"].send_gripper_goal.assert_not_called()
self.robot.arms["rightArm"].send_joint_goal.assert_any_call(
'handover_to_human', max_joint_vel=mock.ANY, timeout=mock.ANY)
self.robot.arms["rightArm"].handover_to_robot.assert_called_once()
self.robot.arms["rightArm"].send_gripper_goal.assert_any_call(
'open', mock.ANY, max_torque=mock.ANY)
self.robot.arms["rightArm"].send_gripper_goal.assert_called_with(
'close', mock.ANY, max_torque=mock.ANY)
self.assertEqual(self.robot.arms["rightArm"].occupied_by, self.entity)
def __init__(self,
robot,
entity_des,
volume="on_top_of",
volume_threshold=0.0):
""" Constructor
:param robot: robot object
:param entity_des: EdEntityDesignator indicating the (furniture) object to check
:param volume: string defining volume of the entity to be checked, default = on_top_of
:param volume_threshold: float [m^3] indicating the free volume above which the area is considered partially_occupied.
(None means any entities filling the volume will result in 'occupied')
"""
smach.StateMachine.__init__(
self,
outcomes=['empty', 'occupied', 'partially_occupied', 'failed'])
seen_entities_des = ds.VariableDesignator(
[], resolve_type=[ClassificationResult])
with self:
smach.StateMachine.add('INSPECT',
Inspect(robot,
entity_des,
searchArea=volume,
objectIDsDes=seen_entities_des),
transitions={
"done": "CHECK",
"failed": "failed"
})
smach.StateMachine.add('CHECK',
CheckEmpty(robot, seen_entities_des,
entity_des, volume,
volume_threshold),
transitions={
'empty': 'empty',
'partially_occupied':
'partially_occupied',
'occupied': 'occupied'
})
def __init__(self,
robot,
segmented_entity_ids_designator,
entity_to_inspect_designator,
segmentation_area="on_top_of",
unknown_threshold=0.0,
filter_threshold=0.0):
""" Constructor
:param robot: robot object
:param segmented_entity_ids_designator: designator that is used to store the segmented objects
:param entity_to_inspect_designator: EdEntityDesignator indicating the (furniture) object to inspect
:param segmentation_area: string defining where the objects are w.r.t. the entity, default = on_top_of
:param unknown_threshold: Entities whose classification score is lower than this float are not marked with a type
:param filter_threshold: Entities whose classification score is lower than this float are ignored
(i.e. are not added to the segmented_entity_ids_designator)
"""
smach.State.__init__(self, outcomes=["done"])
self.robot = robot
self.unknown_threshold = unknown_threshold
self.filter_threshold = filter_threshold
ds.check_resolve_type(entity_to_inspect_designator, Entity)
self.entity_to_inspect_designator = entity_to_inspect_designator
ds.check_type(segmentation_area, str)
if isinstance(segmentation_area, str):
self.segmentation_area_des = ds.VariableDesignator(
segmentation_area)
elif isinstance(segmentation_area, ds.Designator):
self.segmentation_area_des = segmentation_area
else:
raise RuntimeError(
"This shouldn't happen. Wrong types should have raised an exception earlier"
)
ds.check_resolve_type(segmented_entity_ids_designator,
[ClassificationResult])
ds.is_writeable(segmented_entity_ids_designator)
self.segmented_entity_ids_designator = segmented_entity_ids_designator
def execute(self, userdata):
# define answer format
spec = ds.Designator("(<positive_answer>|<negative_answer>)")
# define choices
choices = ds.Designator({ "positive_answer": ["Yes", "Correct", "Right", "Yup"],
"negative_answer": ["No", "Incorrect", "Wrong", "Nope"]})
answer = ds.VariableDesignator(resolve_type=GetSpeechResponse)
state = HearOptionsExtra(self.robot, spec, choices, answer.writeable)
# execute listen
outcome = state.execute()
if not outcome == "heard":
# if there was no answer
print "HearYesNo: did not hear anything!"
return 'heard_failed'
else:
response_negative = ""
response_positive = ""
# test if the answer was positive, if its empty it will return excepton and continue to negative answer
try:
response_positive = answer.resolve().choices["positive_answer"]
print "HearYesNo: answer is positive, heard: '" + response_positive + "'"
return 'heard_yes'
except KeyError, ke:
print "KeyError resolving the answer heard: " + str(ke)
pass
try:
response_negative = answer.resolve().choices["negative_answer"]
print "HearYesNo: answer is negative, heard: '" + response_negative + "'"
return 'heard_no'
except KeyError, ke:
print "KeyError resolving the answer heard: " + str(ke)
pass
def __init__(self, robot, assume_john):
"""
:param robot:
:param assume_john: bool indicating that John (the homeowner) is already there.
"""
smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted'])
door_waypoint = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.waypoint_door['id'])
livingroom_waypoint = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.waypoint_livingroom['id'])
guest_entity_des = ds.VariableDesignator(resolve_type=Entity,
name='guest_entity')
guest_name_des = ds.VariableDesignator('guest 1', name='guest_name')
guest_drink_des = ds.VariableDesignator(resolve_type=HMIResult,
name='guest_drink')
guest_drinkname_des = ds.FieldOfHMIResult(guest_drink_des,
semantics_field='drink',
name='guest_drinkname')
with self:
smach.StateMachine.add('LEARN_GUEST',
LearnGuest(robot, door_waypoint,
guest_entity_des, guest_name_des,
guest_drink_des),
transitions={
'succeeded': 'SAY_GOTO_OPERATOR',
'aborted': 'SAY_GOTO_OPERATOR',
'failed': 'SAY_GOTO_OPERATOR'
})
smach.StateMachine.add(
'SAY_GOTO_OPERATOR',
states.SayFormatted(robot, [
"Okidoki, you are {name} and you like {drink}, lets go inside. Please follow me"
],
name=guest_name_des,
drink=guest_drinkname_des,
block=True,
look_at_standing_person=True),
transitions={'spoken': 'GOTO_LIVINGROOM'})
smach.StateMachine.add(
'GOTO_LIVINGROOM',
states.NavigateToWaypoint(
robot, livingroom_waypoint,
challenge_knowledge.waypoint_livingroom['radius']),
transitions={
'arrived': 'INTRODUCE_GUEST',
'unreachable': 'INTRODUCE_GUEST',
'goal_not_defined': 'aborted'
})
smach.StateMachine.add('INTRODUCE_GUEST',
IntroduceGuest(robot,
guest_entity_des,
guest_name_des,
guest_drinkname_des,
assume_john=assume_john),
transitions={
'succeeded': 'FIND_SEAT_FOR_GUEST',
'abort': 'FIND_SEAT_FOR_GUEST'
})
smach.StateMachine.add(
'FIND_SEAT_FOR_GUEST',
FindEmptySeat(robot,
seats_to_inspect=challenge_knowledge.seats,
room=ds.EntityByIdDesignator(
robot, challenge_knowledge.sitting_room),
seat_is_for=guest_name_des),
transitions={
'succeeded': 'succeeded',
'failed': 'aborted'
})
def __init__(self, robot):
# type: (Robot) -> str
"""
Initialization method
:param robot: robot api object
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"])
# Designators
bar_designator = ds.EdEntityDesignator(robot=robot, id=challenge_knowledge.bar_id, name='bar_des')
room_designator = ds.EdEntityDesignator(robot=robot, id=challenge_knowledge.room_id, name='room_des')
objects_list_des = ds.VariableDesignator(resolve_type=[ClassificationResult], name='objects_list_des')
unav_drink_des = ds.VariableDesignator(resolve_type=str, name='unav_drink_str_des')
hacky_arm_des = ds.VariableDesignator(initial_value=robot.get_arm(), name='hacky_arm')
with self:
smach.StateMachine.add("INITIALIZE",
states.Initialize(robot=robot),
transitions={"initialized": "INITIAL_POSE",
"abort": "aborted"})
smach.StateMachine.add("INITIAL_POSE",
states.SetInitialPose(robot,
challenge_knowledge.starting_point),
transitions={"done": "INSPECT_BAR",
"preempted": "aborted",
"error": "INSPECT_BAR"})
# Inspect bar and store the list of available drinks
smach.StateMachine.add("INSPECT_BAR",
states.Inspect(robot=robot,
entityDes=bar_designator,
navigation_area="in_front_of",
objectIDsDes=objects_list_des),
transitions={"done": "INSPECT_FALLBACK", #TODO: Change to CHECK_INSPECT_RESULT after RWC2019
"failed": "INSPECT_FALLBACK"})
smach.StateMachine.add("CHECK_INSPECT_RESULT",
CheckInspect(objects_list_des,
[ClassificationResult]),
transitions={"true": "IDENTIFY_UNAVAILABLE_DRINK",
"false": "INSPECT_FALLBACK"})
smach.StateMachine.add("IDENTIFY_UNAVAILABLE_DRINK",
IdentifyUnavailableDrinkFromRecognitions(objects=common_knowledge.objects,
classification_list_designator=objects_list_des,
unavailable_drink_designator=unav_drink_des.writeable,
max_unavailable_drinks=challenge_knowledge.MAX_UNAVAILABLE_DRINKS),
transitions={"done": "NAVIGATE_TO_ROOM",
"failed": "INSPECT_FALLBACK"})
# Inspect fallback - ask the bartender which drink is unavailable and store the unavailable drink
smach.StateMachine.add("INSPECT_FALLBACK",
AskAvailability(robot=robot,
unavailable_drink_designator=unav_drink_des.writeable,
objects=common_knowledge.objects),
transitions={"succeeded": "RESET_ROBOT",
"failed": "RESET_ROBOT"})
smach.StateMachine.add("RESET_ROBOT",
states.ArmToJointConfig(robot=robot,
arm_designator=hacky_arm_des,
configuration="reset"),
transitions={'succeeded': "NAVIGATE_TO_ROOM",
'failed': "NAVIGATE_TO_ROOM"})
# Navigate to the predefined room
smach.StateMachine.add("NAVIGATE_TO_ROOM",
states.NavigateToRoom(robot=robot, entity_designator_room=room_designator),
transitions={"arrived": "SAY_HI",
"unreachable": "SAY_HI",
"goal_not_defined": "aborted"})
smach.StateMachine.add("SAY_HI",
states.Say(robot, "Hi, I am {}. I'll be your waiter today".format(robot.robot_name)),
transitions={"spoken": "SERVE_DRINK_1"})
# Explicitly add a new state for each drink, i.e., don't use a range iterator to make sure a new state
# is constructed every time
for idx in range(1, challenge_knowledge.NR_DRINKS + 1):
next_state = "SERVE_DRINK_{}".format(idx + 1) if idx < challenge_knowledge.NR_DRINKS else "SAY_DONE"
smach.StateMachine.add("SERVE_DRINK_{}".format(idx),
ServeOneDrink(robot=robot,
bar_designator=bar_designator,
room_id=challenge_knowledge.room_id,
room_designator=room_designator,
objects_list_des=objects_list_des,
unav_drink_des=unav_drink_des,
name_options=common_knowledge.names,
objects=common_knowledge.objects),
transitions={"succeeded": next_state,
"failed": next_state,
"aborted": next_state})
smach.StateMachine.add("SAY_DONE",
states.Say(robot, "My job here is done. Enjoy your day and see you next time"),
transitions={"spoken": "succeeded"})
def setUp(self):
self.robot = Mockbot()
self.storage_designator = ds.VariableDesignator(
[], resolve_type=[ClassificationResult])
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 setUp(self):
self.arm_ds = ds.ArmDesignator(self.robot,
{'required_arm_name': 'leftArm'})
self.entity = Entity("12345", "dummy", "/map", None, None, {}, None, 0)
self.entity_ds = ds.VariableDesignator(self.entity)
def __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"})
return random.choice([
"Unfortunately we don't have {drink}".format(
drink=drink_request),
"The requested {drink} is unavailable".format(
drink=drink_request),
"I'm sorry but {drink} is out of stock".format(
drink=drink_request),
])
else:
rospy.logerr(
"No correct message type defined for DescriptionStrDesignator")
if __name__ == "__main__":
# Test code
rospy.init_node('get_drinks')
from robot_skills.get_robot import get_robot_from_argv
_robot = get_robot_from_argv(index=1)
rospy.loginfo("Waiting for tf cache to be filled")
rospy.sleep(0.5) # wait for tf cache to be filled
state = AskDrink(
robot=_robot,
drink_designator=ds.VariableDesignator(resolve_type=str).writeable)
state.execute(None)
def __init__(self,
robot,
dst_entity_designator,
dst_entity_type="waypoint",
found_person_designator=None,
properties=None,
query_entity_designator=None,
look_distance=10.0,
speak=False,
strict=True,
nearest=False,
attempts=1,
search_timeout=60,
look_range=(-np.pi/2, np.pi/2),
look_steps=8):
"""
Initialization method
:param robot: robot api object
:param dst_entity_designator: A designator of an Entity whose pose must be updated.
:param dst_entity_type: (str) (default: waypoint) Type of the destination entity
:param found_person_designator: (default: None) A designator to write the search result to.
:param properties: (dict) (default: None) keyvalue pair of the properties a person must
possess. None as a value for a property would search for all possible
values of the property.
:param query_entity_designator: (default: None) An entity designator to match all found
people to
:param look_distance: (float) (default: 10.0) The distance (radius) which the robot must look at
:param speak: (bool) (default: False) If True, the robot will speak while trying to find
a named person
:param strict: (bool) (default: True) Only used if properties is not None AND the {key:value} pair of a
property has non None values.
If set to True then only people with all specified
properties are returned, else all people with at least one true property.
Example:
properties = {'tags': ['LWave', 'RWave', 'LHolding', 'RHolding']}
strict = True
This will return a list of people who have the tags:
'LWave' AND 'RWave' AND 'LHolding' AND 'RHolding'
strict = False
This will return a list of people who have the tags:
'LWave' OR 'RWave' OR 'LHolding' OR 'RHolding'
:param nearest: (bool) (default: False) If True, selects the people nearest to the robot who match the
requirements posed using the properties, query_entity_designator, look distance and strict arguments
:param attempts: (int) (default: 1) Max number of search attempts
:param search_timeout: (float) (default: 60) maximum time the robot is allowed to search
:param look_range: (tuple of size 2) (default: (-np.pi/2, np.pi/2)) from what to what head angle should the
robot search
:param look_steps: (int) (default: 8) How many steps does it take in that range
"""
super(SetPoseFirstFoundPersonToEntity,
self).__init__(outcomes=["done", "failed"])
if not found_person_designator:
found_person_designator = ds.VariableDesignator(resolve_type=Entity, name='new_person').writeable
with self:
self.add("FIND_FIRST_PERSON",
FindFirstPerson(
robot=robot,
found_person_designator=found_person_designator,
properties=properties,
query_entity_designator=query_entity_designator,
look_distance=look_distance,
speak=speak,
strict=strict,
nearest=nearest,
attempts=attempts,
search_timeout=search_timeout,
look_range=look_range,
look_steps=look_steps),
transitions={
'found': 'UPDATE_POSE',
'failed': 'failed'
})
self.add("UPDATE_POSE",
states.UpdateDestEntityPoseWithSrcEntity(
robot=robot,
src_entity_designator=found_person_designator,
dst_entity_designator=dst_entity_designator,
dst_entity_type=dst_entity_type),
transitions={
'done': 'done',
'failed': 'failed'
})
default="hero",
help="Robot name (amigo, hero, sergio)")
parser.add_argument("entity",
default="dinner_table",
help="Name of the entity on which to segment objects")
args = parser.parse_args()
rospy.init_node("example_segment_objects")
robot = get_robot(args.robot)
location_name = args.entity
rospy.loginfo("Creating location designator")
location_designator = ds.EntityByIdDesignator(robot, location_name)
rospy.loginfo("Creating found entity designator")
segmented_entities_designator = ds.VariableDesignator(
[], resolve_type=[ClassificationResult])
rospy.loginfo("Creating segment objects state")
sm = SegmentObjects(robot, segmented_entities_designator.writeable,
location_designator, "on_top_of")
rospy.loginfo("Executing segment objects state")
sm.execute()
rospy.loginfo("Resulting classification result: \n {}".format(
segmented_entities_designator.resolve()))
rospy.loginfo("Done")
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, 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 __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):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
msg = "\n".join([
"==============================================",
"== CHALLENGE HELP ME CARRY ==",
"=============================================="
])
rospy.loginfo("\n" + msg)
self.target_destination = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.default_place)
self.car_waypoint = ds.EntityByIdDesignator(
robot, id=challenge_knowledge.waypoint_car['id'])
self.empty_arm_designator = ds.UnoccupiedArmDesignator(
robot,
arm_properties={
"required_goals": [
"handover_to_human", "reset",
challenge_knowledge.driving_bag_pose,
challenge_knowledge.drop_bag_pose
],
"required_gripper_types": [GripperTypes.GRASPING]
},
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()
self.place_position = ds.LockingDesignator(EmptySpotDesignator(
robot,
self.target_destination,
arm_designator=self.bag_arm_designator,
name="placement",
area=challenge_knowledge.default_area),
name="place_position")
# 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': 'ASK_FOR_TASK'
})
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',
hmc_states.WaitForOperatorCommand(
robot,
possible_commands=challenge_knowledge.commands.keys(),
commands_as_outcomes=True),
transitions={
'no': 'FOLLOW_OPERATOR',
'yes': 'REMEMBER_CAR_LOCATION',
'abort': 'Aborted'
})
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'
})
# 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'
})
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.NavigateToWaypoint(
robot, self.target_destination,
challenge_knowledge.default_target_radius),
transitions={
'arrived': 'PUTDOWN_ITEM',
'unreachable': 'GOTO_DESTINATION_BACKUP',
# implement avoid obstacle behaviour later
'goal_not_defined': 'Aborted'
})
smach.StateMachine.add(
'GOTO_DESTINATION_BACKUP',
states.NavigateToWaypoint(
robot, self.target_destination,
challenge_knowledge.backup_target_radius),
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),
transitions={'done': '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",
block=True,
look_at_standing_person=True),
transitions={'spoken': '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")
