def __init__(self,
robot,
table_id,
table_navigation_area,
table_close_navigation_area,
placement_height=0.7):
StateMachine.__init__(self,
outcomes=["succeeded", "failed"],
input_keys=["item_picked"])
table = EdEntityDesignator(robot=robot, id=table_id)
with self:
# StateMachine.add("NAVIGATE_TO_TABLE",
# NavigateToSymbolic(robot, {table: table_navigation_area}, table),
# transitions={'arrived': 'NAVIGATE_TO_TABLE_CLOSE',
# 'unreachable': 'failed',
# 'goal_not_defined': 'failed'})
StateMachine.add(
"NAVIGATE_TO_TABLE_CLOSE",
NavigateToSymbolic(robot, {table: table_close_navigation_area},
table),
transitions={
'arrived': 'PLACE_ITEM_ON_TABLE',
'unreachable': 'SAY_PICK_AWAY_THE_CHAIR',
'goal_not_defined': 'failed'
})
StateMachine.add("FORCE_DRIVE",
ForceDrive(robot, -0.1, 0, 0, 5.0),
transitions={'done': 'SAY_PICK_AWAY_THE_CHAIR'})
StateMachine.add(
"SAY_PICK_AWAY_THE_CHAIR",
Say(
robot,
"Please pick away the chair, I cannot get close enough to the {}"
.format(table_id)),
transitions={'spoken': 'WAIT_FOR_PICK_AWAY_CHAIR'})
StateMachine.add('WAIT_FOR_PICK_AWAY_CHAIR',
WaitTime(robot, 5),
transitions={
'waited': 'SAY_THANKS',
'preempted': 'failed'
})
StateMachine.add('SAY_THANKS',
Say(robot, "Thank you darling"),
transitions={'spoken': 'NAVIGATE_TO_TABLE_CLOSE'})
StateMachine.add("PLACE_ITEM_ON_TABLE",
PlaceItemOnTable(robot, table_id,
placement_height),
transitions={
'succeeded': 'succeeded',
'failed': 'failed'
})
def __init__(self, robot, arm_designator, timeout=10):
smach.StateMachine.__init__(self, outcomes=['succeeded','failed'])
#A designator can resolve to a different item every time its resolved. We don't want that here, so lock
check_type(arm_designator, Arm)
locked_arm = LockingDesignator(arm_designator)
with self:
smach.StateMachine.add("LOCK_ARM",
LockDesignator(locked_arm),
transitions={'locked' :'SPINDLE_MEDIUM'})
smach.StateMachine.add("SPINDLE_MEDIUM",
ResetPart(robot, robot.torso),
transitions={'done' :'MOVE_HUMAN_HANDOVER_JOINT_GOAL'})
smach.StateMachine.add("MOVE_HUMAN_HANDOVER_JOINT_GOAL",
ArmToJointConfig(robot, locked_arm, 'handover_to_human'),
transitions={ 'succeeded' :'SAY_OPEN_GRIPPER',
'failed' :'SAY_OPEN_GRIPPER'})
smach.StateMachine.add("SAY_OPEN_GRIPPER",
Say(robot, [ "Watch out, I will open my gripper in one second. Please take it from me."]),
transitions={ 'spoken' :'OPEN_GRIPPER_HANDOVER'})
smach.StateMachine.add('OPEN_GRIPPER_HANDOVER', SetGripper(robot, locked_arm, gripperstate=GripperState.OPEN, timeout=2.0),
transitions={'succeeded' : 'SAY_CLOSE_NOW_GRIPPER',
'failed' : 'SAY_CLOSE_NOW_GRIPPER'})
smach.StateMachine.add("SAY_CLOSE_NOW_GRIPPER",
Say(robot, [ "I will close my gripper now"]),
transitions={ 'spoken' :'CLOSE_GRIPPER_HANDOVER'})
# smach.StateMachine.add('OPEN_GRIPPER_ON_HANDOVER', OpenGripperOnHandoverToHuman(robot, locked_arm, timeout=timeout),
# transitions={'succeeded' : 'CLOSE_GRIPPER_HANDOVER',
# 'failed' : 'SAY_I_WILL_KEEP_IT'})
smach.StateMachine.add('CLOSE_GRIPPER_HANDOVER', SetGripper(robot, locked_arm, gripperstate=GripperState.CLOSE, timeout=0.0),
transitions={'succeeded' : 'RESET_ARM',
'failed' : 'RESET_ARM'})
# smach.StateMachine.add("SAY_I_WILL_KEEP_IT",
# Say(robot, [ "If you don't want it, I will keep it"]),
# transitions={ 'spoken' :'RESET_ARM'})
smach.StateMachine.add('RESET_ARM',
ArmToJointConfig(robot, locked_arm, 'reset'),
transitions={'succeeded' :'RESET_TORSO',
'failed' :'RESET_TORSO' })
smach.StateMachine.add('RESET_TORSO',
ResetPart(robot, robot.torso),
transitions={'done':'UNLOCK_ARM'})
smach.StateMachine.add("UNLOCK_ARM",
UnlockDesignator(locked_arm),
transitions={'unlocked' :'succeeded'})
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, object_category_des, room_des):
smach.StateMachine.__init__(self, outcomes=["done", "failed"])
room_id_des = ds.AttrDesignator(room_des, "id", resolve_type=str)
with self:
smach.StateMachine.add("LOOK_INTO_ROOM",
NavigateToRoom(robot, room_des, room_des),
transitions={
"arrived": "SAY_COME_TO_ME",
"unreachable": "SAY_COME_TO_ME",
"goal_not_defined": "SAY_COME_TO_ME"
})
smach.StateMachine.add(
"SAY_COME_TO_ME",
Say(robot,
"Operator, please come to me in the {room}",
room=room_id_des,
block=True),
transitions={"spoken": "WAIT_FOR_OPERATOR"})
smach.StateMachine.add("WAIT_FOR_OPERATOR",
WaitTime(4),
transitions={
"waited": "ASK_WHICH_CATERGORY",
"preempted": "ASK_WHICH_CATERGORY"
})
smach.StateMachine.add(
"ASK_WHICH_CATERGORY",
AskWhichCategory(
robot, ds.Designator(challenge_knowledge.category_grammar),
object_category_des),
transitions={"done": "done"})
def __init__(self, robot, arm_designator, grabbed_entity_label="", grabbed_entity_designator=None, timeout=10, arm_configuration="handover_to_human"):
"""
Hold up hand to accept an object and close hand once something is inserted
:param robot: Robot with which to execute this behavior
:param arm_designator: ArmDesignator resolving to arm accept item into
:param grabbed_entity_label: What ID to give a dummy item in case no grabbed_entity_designator is supplied
:param grabbed_entity_designator: EntityDesignator resolving to the accepted item. Can be a dummy
:param timeout: How long to hold hand over before closing without anything
:param arm_configuration: Which pose to put arm in when holding hand up for the item.
"""
smach.StateMachine.__init__(self, outcomes=['succeeded','failed','timeout'])
check_type(arm_designator, Arm)
if not grabbed_entity_designator and grabbed_entity_label == "":
rospy.logerr("No grabbed entity label or grabbed entity designator given")
with self:
smach.StateMachine.add("POSE", ArmToJointConfig(robot, arm_designator, arm_configuration),
transitions={'succeeded':'OPEN_BEFORE_INSERT','failed':'OPEN_BEFORE_INSERT'})
smach.StateMachine.add( 'OPEN_BEFORE_INSERT', SetGripper(robot, arm_designator, gripperstate=GripperState.OPEN),
transitions={'succeeded' : 'SAY1',
'failed' : 'SAY1'})
smach.StateMachine.add("SAY1", Say(robot,'Please hand over the object by pushing it gently in my gripper'),
transitions={'spoken':'CLOSE_AFTER_INSERT'})
smach.StateMachine.add( 'CLOSE_AFTER_INSERT', CloseGripperOnHandoverToRobot(robot,
arm_designator,
grabbed_entity_label=grabbed_entity_label,
grabbed_entity_designator=grabbed_entity_designator,
timeout=timeout),
transitions={'succeeded' : 'succeeded',
'timeout' : 'timeout',
'failed' : 'failed'})
def __init__(self,
robot,
arm_designator,
grabbed_entity_label="",
grabbed_entity_designator=None,
timeout=10):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
check_type(arm_designator, Arm)
if not grabbed_entity_designator and grabbed_entity_label == "":
rospy.logerr(
"No grabbed entity label or grabbed entity designator given")
with self:
smach.StateMachine.add("POSE",
ArmToJointConfig(robot, arm_designator,
"handover_to_human"),
transitions={
'succeeded': 'OPEN_BEFORE_INSERT',
'failed': 'OPEN_BEFORE_INSERT'
})
smach.StateMachine.add('OPEN_BEFORE_INSERT',
SetGripper(robot,
arm_designator,
gripperstate='open'),
transitions={
'succeeded': 'SAY1',
'failed': 'SAY1'
})
smach.StateMachine.add(
"SAY1",
Say(
robot,
'Please hand over the object by pushing it gently in my gripper'
),
transitions={'spoken': 'CLOSE_AFTER_INSERT'})
smach.StateMachine.add(
'CLOSE_AFTER_INSERT',
CloseGripperOnHandoverToRobot(
robot,
arm_designator,
grabbed_entity_label=grabbed_entity_label,
grabbed_entity_designator=grabbed_entity_designator,
timeout=timeout),
transitions={
'succeeded': 'succeeded',
'failed': 'failed'
})
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, 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 setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['done', 'failed', 'aborted'])
with sm:
smach.StateMachine.add('START_CHALLENGE_ROBUST',
StartChallengeRobust(robot, STARTING_POINT),
transitions={
'Done': 'GO_TO_SEARCH_POSE',
'Aborted': 'aborted',
'Failed': 'GO_TO_SEARCH_POSE'
})
smach.StateMachine.add('SAY_START',
Say(robot,
"Finding your mates, here we go!",
block=False),
transitions={'spoken': 'GO_TO_SEARCH_POSE'})
smach.StateMachine.add('GO_TO_SEARCH_POSE',
NavigateToWaypoint(robot,
ds.EntityByIdDesignator(
robot, id=SEARCH_POINT),
radius=0.375),
transitions={
'arrived': 'RISE_FOR_THE_PEOPLE',
'goal_not_defined': 'failed',
'unreachable': 'WAIT_SEARCH_POSE'
})
smach.StateMachine.add('WAIT_SEARCH_POSE',
WaitTime(robot, 5),
transitions={
'preempted': 'aborted',
'waited': 'GO_TO_SEARCH_POSE'
})
# noinspection PyUnusedLocal
@smach.cb_interface(outcomes=["done"])
def _rise_for_the_people(userdata=None):
""" Resets the location hmi attempt so that each operator gets three attempts """
robot.arms.values()[0]._send_joint_trajectory(
[[0.70, -1.9, 0.0, -1.57, 0.0]])
robot.speech.speak(
"Hi there. My Name is Hero. I'm looking for the mates of my operator",
block=False)
return "done"
smach.StateMachine.add("RISE_FOR_THE_PEOPLE",
smach.CBState(_rise_for_the_people),
transitions={"done": "LOCATE_PEOPLE"})
# locate all four people
smach.StateMachine.add('LOCATE_PEOPLE',
LocatePeople(robot, room_id=ROOM_ID),
transitions={
'done': 'RESET_FOR_DRIVING',
})
# noinspection PyUnusedLocal
@smach.cb_interface(outcomes=["done"])
def _reset_for_driving(userdata=None):
""" Resets the location hmi attempt so that each operator gets three attempts """
robot.speech.speak("Thank you for your attention", block=False)
robot.arms.values()[0]._send_joint_trajectory([
[0.01, -1.9, 0.0, -1.57, 0.0], # Inspect with q0 low
[0.01, 0.0, -1.57, -1.57, 0.0]
]) # Reset
return "done"
smach.StateMachine.add("RESET_FOR_DRIVING",
smach.CBState(_reset_for_driving),
transitions={"done": "GO_BACK_TO_OPERATOR"})
# drive back to the operator to describe the mates
smach.StateMachine.add('GO_BACK_TO_OPERATOR',
NavigateToWaypoint(
robot,
ds.EntityByIdDesignator(robot,
id=OPERATOR_POINT),
radius=0.7,
look_at_designator=ds.EntityByIdDesignator(
robot, id=OPERATOR_POINT)),
transitions={
'arrived': 'REPORT_PEOPLE',
'goal_not_defined': 'REPORT_PEOPLE',
'unreachable': 'WAIT_GO_BACK'
})
smach.StateMachine.add('WAIT_GO_BACK',
WaitTime(robot, 5),
transitions={
'preempted': 'aborted',
'waited': 'GO_BACK_TO_OPERATOR'
})
# check how to uniquely define them # ToDo: make this more interesting
smach.StateMachine.add('REPORT_PEOPLE',
ReportPeople(robot),
transitions={'done': 'done'})
return sm
#ROS
import rospy
import argparse
# TU/e Robotics
from robot_skills.get_robot import get_robot
# Robot Smach States
from robot_smach_states.human_interaction import Say
if __name__ == "__main__":
parser = argparse.ArgumentParser(
description="Test the say state with a sentence")
parser.add_argument("--robot",
default="hero",
help="Robot name (amigo, hero, sergio)")
args = parser.parse_args()
rospy.init_node('test_say')
robot = get_robot(args.robot)
sentence = 'I have said something useful'
say_state = Say(robot, sentence)
say_state.execute()
def single_item(robot,
results_writer,
cls,
support,
waypoint,
inspect_from_area=None,
non_strict_class=False,
search_area='on_top_of'):
"""
Benchmark grasping for a single item. Outputs a record dictionary
:param robot: an instance of Robot
:param results_writer: a csv.DictWriter to which the output record is written
:param cls: class/type of item to grab
:param support: ID of the entity supporting the item-to-grab
:param waypoint: From where should the robot start the grasp
:param inspect_from_area: Which area of the support-entity should the robot be in to start the inspection
:param non_strict_class: If set to True, the robot is not strict about the type of item it grabs, eg. it continues grasping with another type of object
:param search_area: which area of the support-entity to search/inspect for an item of the given class
:return: a dict with the benchmark result
"""
grasp_cls = ds.Designator(cls, name='grasp_cls')
support_entity = ds.EdEntityDesignator(robot,
id=support,
name='support_entity')
entity_ids = ds.VariableDesignator([],
resolve_type=[ClassificationResult],
name='entity_ids')
waypoint_des = ds.EdEntityDesignator(robot, id=waypoint, name='waypoint')
arm = ds.LockingDesignator(
ds.UnoccupiedArmDesignator(robot,
name='unoccupied-arm',
arm_properties={
"required_trajectories":
["prepare_grasp"],
"required_goals": ["carrying_pose"],
"required_gripper_types":
[arms.GripperTypes.GRASPING]
}))
arm.lock()
record = {
'robot': robot.robot_name,
'start_waypoint': waypoint,
'expected_class': cls,
'id': None,
'timestamp': datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S')
}
try:
say_announce = Say(
robot,
sentence="Please put a {cls} {search_area} the {support}".format(
cls=cls, support=support, search_area=search_area))
say_announce.execute()
nav_to_start = NavigateToWaypoint(robot,
waypoint_designator=waypoint_des,
look_at_designator=support_entity)
assert nav_to_start.execute(
) == 'arrived', "I did not arrive at the start"
inspect = Inspect(robot=robot,
entityDes=support_entity,
objectIDsDes=entity_ids,
navigation_area=inspect_from_area,
searchArea=search_area)
record['inspect_start'] = time.time()
assert inspect.execute(
) == 'done', "I could not inspect the support entity"
record['inspect_end'] = time.time()
record['inspect_duration'] = '{:3.3f}'.format(record['inspect_end'] -
record['inspect_start'])
inspection_result = entity_ids.resolve(
) # type: List[ClassificationResult]
if inspection_result:
if grasp_cls.resolve() in ANY_OPTIONS or non_strict_class:
rospy.loginfo("Any item will match")
matching_results = inspection_result
else:
matching_results = [
result for result in inspection_result
if result.type == grasp_cls.resolve()
]
rospy.loginfo("Found {} items of class {}".format(
len(matching_results), grasp_cls.resolve()))
if matching_results:
if len(matching_results) > 1:
rospy.logwarn(
"There are multiple items OK to grab, will select the last one"
)
record['observed_class'] = ' '.join(
[result.type for result in matching_results])
else:
record['observed_class'] = matching_results[-1].type
selected_entity_id = matching_results[-1].id
rospy.loginfo("Selected entity {} for grasping".format(
selected_entity_id))
grasp_entity = ds.EdEntityDesignator(robot,
id=selected_entity_id)
record['id'] = selected_entity_id[:6]
entity = grasp_entity.resolve() # type: Entity
if entity:
vector_stamped = entity.pose.extractVectorStamped(
) # type: VectorStamped
record['x'] = '{:.3f}'.format(vector_stamped.vector.x())
record['y'] = '{:.3f}'.format(vector_stamped.vector.y())
record['z'] = '{:.3f}'.format(vector_stamped.vector.z())
grab_state = Grab(robot, grasp_entity, arm)
record['grab_start'] = time.time()
assert grab_state.execute() == 'done', "I couldn't grasp"
record['grab_end'] = time.time()
record['grab_duration'] = '{:3.3f}'.format(
record['grab_end'] - record['grab_start'])
assert nav_to_start.execute(
) == 'arrived', "I could not navigate back to the start"
rospy.logwarn("Robot will turn around to drop the {}".format(
grasp_cls.resolve()))
force_drive = ForceDrive(robot, 0, 0, 1,
3.14) # rotate 180 degs in pi seconds
force_drive.execute()
say_drop = Say(
robot,
sentence="I'm going to drop the item, please hold it!")
say_drop.execute()
drop_it = SetGripper(robot,
arm_designator=arm,
grab_entity_designator=grasp_entity)
drop_it.execute()
force_drive_back = ForceDrive(
robot, 0, 0, -1, 3.14) # rotate -180 degs in pi seconds
force_drive_back.execute()
nav_to_start.execute()
else:
raise AssertionError("No {} found".format(grasp_cls.resolve()))
else:
rospy.logerr("No entities found at all :-(")
except AssertionError as assertion_err:
say_fail = Say(robot, sentence=assertion_err.message + ", sorry")
say_fail.execute()
finally:
results_writer.writerow(record)
return record
def __init__(self, robot, arm_designator, timeout=10):
"""
State to hand over the object in the arm to a human operator
:param robot: robot to execute state with
:param arm_designator: designator that resolves to arm holding the object
:param timeout: float amount of time the procedure may take
"""
smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
# A designator can resolve to a different item every time its resolved. We don't want that here, so lock
check_type(arm_designator, PublicArm)
locked_arm = LockingDesignator(arm_designator)
with self:
smach.StateMachine.add("LOCK_ARM",
LockDesignator(locked_arm),
transitions={'locked': 'SPINDLE_MEDIUM'})
smach.StateMachine.add(
"SPINDLE_MEDIUM",
ResetPart(robot, robot.torso),
transitions={'done': 'MOVE_HUMAN_HANDOVER_JOINT_GOAL'})
smach.StateMachine.add("MOVE_HUMAN_HANDOVER_JOINT_GOAL",
ArmToJointConfig(robot, locked_arm,
'handover_to_human'),
transitions={
'succeeded': 'SAY_DETECT_HANDOVER',
'failed': 'SAY_DETECT_HANDOVER'
})
smach.StateMachine.add("SAY_DETECT_HANDOVER",
Say(robot, [
"I will handover the object now"
"Please take it from my gripper."
]),
transitions={'spoken': 'DETECT_HANDOVER'})
smach.StateMachine.add("DETECT_HANDOVER",
HandOverTo(robot, locked_arm),
transitions={
'succeeded': 'SAY_CLOSE_NOW_GRIPPER',
'failed': 'SAY_CLOSE_NOW_GRIPPER'
})
smach.StateMachine.add(
"SAY_CLOSE_NOW_GRIPPER",
Say(robot, ["I will close my gripper now"]),
transitions={'spoken': 'CLOSE_GRIPPER_HANDOVER'})
smach.StateMachine.add('CLOSE_GRIPPER_HANDOVER',
SetGripper(robot,
locked_arm,
gripperstate=GripperState.CLOSE,
timeout=0),
transitions={
'succeeded': 'RESET_ARM',
'failed': 'RESET_ARM'
})
smach.StateMachine.add('RESET_ARM',
ArmToJointConfig(robot, locked_arm,
'reset'),
transitions={
'succeeded': 'UNLOCK_ARM',
'failed': 'UNLOCK_ARM'
})
smach.StateMachine.add("UNLOCK_ARM",
UnlockDesignator(locked_arm),
transitions={'unlocked': 'succeeded'})
def setup_statemachine(robot):
state_machine = StateMachine(outcomes=['done'])
state_machine.userdata['item_picked'] = None
with state_machine:
# Intro
# StateMachine.add('START_CHALLENGE_ROBUST',
# Initialize(robot),
# transitions={'initialized': 'SAY_START', 'abort': 'done'})
StateMachine.add('START_CHALLENGE_ROBUST', StartChallengeRobust(robot, "initial_pose"), # ToDo: in knowledge
transitions={'Done': 'SAY_START',
'Aborted': 'done',
'Failed': 'SAY_START'})
StateMachine.add('SAY_START',
Say(robot,
"Let's set the table baby! If there are any chairs near the kitchen_table, please remove them",
block=False),
transitions={'spoken': 'NAVIGATE_AND_OPEN_CUPBOARD'})
# The pre-work
StateMachine.add('NAVIGATE_AND_OPEN_CUPBOARD',
NavigateToAndOpenCupboard(robot, "kitchen_cabinet", "in_front_of"),
transitions={'succeeded': 'NAVIGATE_AND_PICK_ITEM_FROM_CUPBOARD_DRAWER',
'failed': 'SAY_OPEN_FAILED'})
StateMachine.add('SAY_OPEN_FAILED',
Say(robot, "I failed to open the cupboard drawer"),
transitions={'spoken': 'WAIT_OPEN'})
StateMachine.add('WAIT_OPEN',
WaitTime(robot, 5),
transitions={'waited': 'SAY_OPEN_THANKS', 'preempted': 'done'})
StateMachine.add('SAY_OPEN_THANKS',
Say(robot, "Thank you darling"),
transitions={'spoken': 'NAVIGATE_AND_PICK_ITEM_FROM_CUPBOARD_DRAWER'})
# The loop
StateMachine.add('NAVIGATE_AND_PICK_ITEM_FROM_CUPBOARD_DRAWER',
NavigateToAndPickItemFromCupboardDrawer(robot, "kitchen_cabinet", "in_front_of",
required_items),
transitions={'succeeded': 'PLACE_ITEM_ON_TABLE',
'failed': 'CHECK_IF_WE_HAVE_IT_ALL'})
StateMachine.add('PLACE_ITEM_ON_TABLE',
NavigateToAndPlaceItemOnTable(robot, "kitchen_table", "right_of", "right_of_close"),
transitions={'succeeded': 'CHECK_IF_WE_HAVE_IT_ALL',
'failed': 'WAIT'})
StateMachine.add('WAIT',
WaitTime(robot, 2),
transitions={'waited': 'CHECK_IF_WE_HAVE_IT_ALL', 'preempted': 'done'})
StateMachine.add('CHECK_IF_WE_HAVE_IT_ALL',
CBState(check_if_we_have_it_all, cb_args=[robot]),
transitions={'we_have_it_all': 'SAY_END_CHALLENGE',
'keep_going': 'NAVIGATE_AND_PICK_ITEM_FROM_CUPBOARD_DRAWER'})
# Outro
StateMachine.add('SAY_END_CHALLENGE',
Say(robot, "That was all folks, have a nice meal!"),
transitions={'spoken': 'done'})
StateMachine.add('NAVIGATE_AND_CLOSE_CUPBOARD',
NavigateToAndCloseCupboard(robot, "cupboard", "in_front_of"),
transitions={'succeeded': 'done',
'failed': 'done'})
return state_machine
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
with sm:
# Start challenge via StartChallengeRobust
smach.StateMachine.add("START_CHALLENGE_ROBUST",
StartChallengeRobust(robot, STARTING_POINT),
transitions={
"Done": "GO_TO_INTERMEDIATE_WAYPOINT",
"Aborted": "GO_TO_INTERMEDIATE_WAYPOINT",
"Failed": "GO_TO_INTERMEDIATE_WAYPOINT"
})
# There is no transition to Failed in StartChallengeRobust (28 May)
smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT',
NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id=INTERMEDIATE_1),
radius=0.5),
transitions={
'arrived':
'ASK_CONTINUE',
'unreachable':
'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1',
'goal_not_defined':
'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1'
})
smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1',
NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id=INTERMEDIATE_2),
radius=0.5),
transitions={
'arrived':
'ASK_CONTINUE',
'unreachable':
'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2',
'goal_not_defined':
'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2'
})
smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2',
NavigateToWaypoint(
robot,
EntityByIdDesignator(robot,
id=INTERMEDIATE_3),
radius=0.5),
transitions={
'arrived': 'ASK_CONTINUE',
'unreachable': 'ASK_CONTINUE',
'goal_not_defined': 'ASK_CONTINUE'
})
smach.StateMachine.add("ASK_CONTINUE",
AskContinue(robot, 30),
transitions={
'continue': 'SAY_CONTINUEING',
'no_response': 'SAY_CONTINUEING'
})
smach.StateMachine.add(
'SAY_CONTINUEING',
Say(robot, [
"I heard continue, so I will move to the exit now. See you guys later!"
],
block=False),
transitions={'spoken': 'GO_TO_EXIT'})
# Amigo goes to the exit (waypoint stated in knowledge base)
smach.StateMachine.add('GO_TO_EXIT',
NavigateToWaypoint(robot,
EntityByIdDesignator(
robot, id=EXIT_1),
radius=0.7),
transitions={
'arrived': 'AT_END',
'unreachable': 'GO_TO_EXIT_2',
'goal_not_defined': 'GO_TO_EXIT_2'
})
smach.StateMachine.add('GO_TO_EXIT_2',
NavigateToWaypoint(robot,
EntityByIdDesignator(
robot, id=EXIT_2),
radius=0.5),
transitions={
'arrived': 'AT_END',
'unreachable': 'GO_TO_EXIT_3',
'goal_not_defined': 'GO_TO_EXIT_3'
})
smach.StateMachine.add('GO_TO_EXIT_3',
NavigateToWaypoint(robot,
EntityByIdDesignator(
robot, id=EXIT_3),
radius=0.5),
transitions={
'arrived': 'AT_END',
'unreachable': 'RESET_ED_TARGET',
'goal_not_defined': 'AT_END'
})
smach.StateMachine.add('RESET_ED_TARGET',
ResetED(robot),
transitions={'done': 'GO_TO_EXIT'})
# Finally amigo will stop and says 'goodbye' to show that he's done.
smach.StateMachine.add('AT_END',
Say(robot, "Goodbye"),
transitions={'spoken': 'Done'})
analyse_designators(sm, "rips")
return sm
def __init__(self, robot, operator_distance=1.0, operator_radius=0.5):
# type: (Robot, float, float) -> None
"""
Base Smach state to guide an operator to a designated position
:param robot: (Robot) robot api object
: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.robot = robot
self.operator_distance = operator_distance
self.operator_radius = operator_radius
self.execute_plan = ExecutePlanGuidance(robot=self.robot,
operator_distance=self.operator_distance,
operator_radius=self.operator_radius)
with self:
@smach.cb_interface(outcomes=["done"])
def _reset_mentioned_entities(userdata=None):
"""
Resets the entities that have been mentioned so that the robot will mention all entities to all
operators
"""
self.execute_plan.reset_tourguide()
return "done"
smach.StateMachine.add("RESET_MENTIONED_ENTITIES",
smach.CBState(_reset_mentioned_entities),
transitions={"done": "SAY_BEHIND"})
smach.StateMachine.add("SAY_BEHIND",
Say(robot, "Please stand behind me and look at me", block=True),
transitions={"spoken": "WAIT"})
smach.StateMachine.add("WAIT",
WaitTime(robot, waittime=3.0),
transitions={"waited": "GET_PLAN",
"preempted": "preempted"})
smach.StateMachine.add("GET_PLAN", navigation.getPlan(self.robot, self.generate_constraint),
transitions={"unreachable": "unreachable",
"goal_not_defined": "goal_not_defined",
"goal_ok": "EXECUTE_PLAN"})
smach.StateMachine.add("EXECUTE_PLAN", self.execute_plan,
transitions={"arrived": "arrived",
"blocked": "PLAN_BLOCKED",
"preempted": "preempted",
"lost_operator": "WAIT_FOR_OPERATOR"})
smach.StateMachine.add("WAIT_FOR_OPERATOR",
WaitForOperator(robot=self.robot,
distance=self.operator_distance,
radius=self.operator_radius),
transitions={"is_following": "GET_PLAN",
"is_lost": "lost_operator"})
smach.StateMachine.add("PLAN_BLOCKED", navigation.planBlocked(self.robot),
transitions={"blocked": "GET_PLAN",
"free": "EXECUTE_PLAN"})
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",
StartChallengeRobust(
robot, INITIAL_POSE_ID),
transitions={
"Done": "ASK_WHERE_TO_GO",
"Aborted": "Aborted",
"Failed": "Aborted"
})
smach.StateMachine.add(
"ASK_WHERE_TO_GO",
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",
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",
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",
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",
Say(robot, "Please move your ass so I can get going!"),
transitions={"spoken": "TC_MOVE_TIME"})
smach.StateMachine.add("TC_MOVE_TIME",
WaitTime(robot=robot, waittime=3),
transitions={
"waited": "NAV_TO_START",
"preempted": "Aborted"
})
smach.StateMachine.add(
"NAV_TO_START",
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",
WaitTime(robot, 3.0),
transitions={
"waited": "NAV_TO_START_BACKUP",
"preempted": "Aborted"
})
smach.StateMachine.add(
"NAV_TO_START_BACKUP",
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",
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",
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",
Say(robot, "Goodbye"),
transitions={"spoken": "Done"})
def __init__(self, robot):
"""
Constructor
:param robot: robot object
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"])
with self:
self.spec_des = ds.Designator(knowledge.location_grammar)
self.answer_des = ds.VariableDesignator(resolve_type=HMIResult)
self.entity_des = EntityFromHmiResults(robot, self.answer_des)
self._location_hmi_attempt = 0
self._max_hmi_attempts = 3 # ToDo: parameterize?
@smach.cb_interface(outcomes=["reset"])
def _reset_location_hmi_attempt(userdata=None):
""" Resets the location hmi attempt so that each operator gets three attempts """
self._location_hmi_attempt = 0
return "reset"
smach.StateMachine.add("RESET_HMI_ATTEMPT",
smach.CBState(_reset_location_hmi_attempt),
transitions={"reset": "ANNOUNCE_ITEM"})
if WAIT_MODE == WaitMode.SPEECH:
smach.StateMachine.add(
"ANNOUNCE_ITEM",
Say(robot,
"Hello, my name is {}. Please call me by my name. "
"Talk loudly into my microphone and wait for the ping".
format(robot.robot_name),
block=True),
transitions={"spoken": "WAIT_TO_BE_CALLED"})
smach.StateMachine.add(
"WAIT_TO_BE_CALLED",
HearOptions(robot, ["{}".format(robot.robot_name)],
timeout=10),
transitions={
"{}".format(robot.robot_name): "INSTRUCT",
"no_result": "ANNOUNCE_ITEM"
})
elif WAIT_MODE == WaitMode.VISUAL:
smach.StateMachine.add(
"ANNOUNCE_ITEM",
Say(robot,
"Hello, my name is {}. Please step in front of me.".
format(robot.robot_name),
block=True),
transitions={"spoken": "WAIT_TO_BE_CALLED"})
smach.StateMachine.add("WAIT_TO_BE_CALLED",
WaitForPersonInFront(
robot,
attempts=10,
sleep_interval=1.0),
transitions={
"success": "INSTRUCT",
"failed": "SAY_NOT_DETECTED"
})
smach.StateMachine.add(
"SAY_NOT_DETECTED",
Say(robot,
"I did not see you but will try to continue anyway.".
format(robot.robot_name),
block=True),
transitions={"spoken": "INSTRUCT"})
smach.StateMachine.add(
"INSTRUCT",
Say(robot, [
"Please tell me where you would like to go. "
"Talk loudly into my microphone and wait for the ping"
],
block=True),
transitions={"spoken": "LISTEN_FOR_LOCATION"})
smach.StateMachine.add("LISTEN_FOR_LOCATION",
HearOptionsExtra(robot, self.spec_des,
self.answer_des.writeable,
6),
transitions={
"heard": "ASK_CONFIRMATION",
"no_result": "HANDLE_FAILED_HMI"
})
smach.StateMachine.add(
"ASK_CONFIRMATION",
Say(robot, [
"I hear that you would like to go to the {place}, is this correct?"
],
place=ds.AttrDesignator(self.entity_des,
"id",
resolve_type=str)),
transitions={"spoken": "CONFIRM_LOCATION"})
smach.StateMachine.add("CONFIRM_LOCATION",
HearOptions(robot=robot,
options=["yes", "no"]),
transitions={
"yes": "INSTRUCT_FOR_WAIT",
"no": "INSTRUCT",
"no_result": "HANDLE_FAILED_HMI"
})
@smach.cb_interface(outcomes=["retry", "fallback", "failed"])
def _handle_failed_hmi(userdata=None):
""" Handle failed HMI queries so we can try up to x times """
self._location_hmi_attempt += 1 # Increment
if self._location_hmi_attempt == self._max_hmi_attempts:
rospy.logwarn(
"HMI failed for the {} time, returning 'failed'".
format(self._max_hmi_attempts))
if not BACKUP_SCENARIOS:
rospy.logwarn(
"No fallback scenario's available anymore")
return "failed"
backup = BACKUP_SCENARIOS.pop(0)
robot.speech.speak("I am sorry but I did not hear you",
mood="sad",
block=False)
robot.speech.speak(backup.sentence, block=False)
self.answer_des.writeable.write(
HMIResult("", backup.entity_id))
return "fallback"
rospy.loginfo(
"HMI failed for the {} time out of {}, retrying".format(
self._location_hmi_attempt, self._max_hmi_attempts))
return "retry"
smach.StateMachine.add("HANDLE_FAILED_HMI",
smach.CBState(_handle_failed_hmi),
transitions={
"retry": "INSTRUCT",
"fallback": "INSTRUCT_FOR_WAIT",
"failed": "failed"
})
smach.StateMachine.add(
"INSTRUCT_FOR_WAIT",
Say(robot, [
"Let me think how to get to the {entity_id}",
"I will now determine the best route to the {entity_id}"
],
entity_id=ds.AttrDesignator(self.entity_des,
"id",
resolve_type=str)),
transitions={"spoken": "GIVE_DIRECTIONS"})
smach.StateMachine.add("GIVE_DIRECTIONS",
GiveDirections(robot, self.entity_des),
transitions={
"succeeded": "INSTRUCT_FOLLOW",
"failed": "failed"
})
smach.StateMachine.add("INSTRUCT_FOLLOW",
Say(robot, ["Please follow me"],
block=True),
transitions={"spoken": "GUIDE_OPERATOR"})
smach.StateMachine.add("GUIDE_OPERATOR",
GuideToRoomOrObject(robot, self.entity_des),
transitions={
"arrived": "SUCCESS",
"unreachable": "SAY_CANNOT_REACH",
"goal_not_defined": "SAY_CANNOT_REACH",
"lost_operator": "SAY_LOST_OPERATOR",
"preempted": "failed"
})
smach.StateMachine.add(
"SUCCESS",
Say(robot, ["We have arrived"], block=True),
transitions={"spoken": "RETURN_TO_INFORMATION_POINT"})
smach.StateMachine.add(
"SAY_CANNOT_REACH",
Say(robot, ["I am sorry but I cannot reach the destination."],
block=True),
transitions={"spoken": "RETURN_TO_INFORMATION_POINT"})
smach.StateMachine.add(
"SAY_LOST_OPERATOR",
Say(robot, ["Oops I have lost you completely."], block=True),
transitions={"spoken": "RETURN_TO_INFORMATION_POINT"})
smach.StateMachine.add("RETURN_TO_INFORMATION_POINT",
NavigateToWaypoint(
robot,
ds.EntityByIdDesignator(
robot, INFORMATION_POINT_ID)),
transitions={
"arrived": "succeeded",
"unreachable": "failed",
"goal_not_defined": "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',
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',
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",
Say(robot,
"Looking {area} the {entity}",
area=segment_area,
entity=location_id_des,
block=False),
transitions={"spoken": "RISE"})
smach.StateMachine.add('RISE',
RiseForInspect(robot, e_des, segment_area),
transitions={
'succeeded': 'SEGMENT',
'failed': 'SEGMENT'
})
smach.StateMachine.add(
'SEGMENT',
SegmentObjects(robot, e_classifications_des.writeable, e_des,
segment_area),
transitions={'done': "HANDLE_DETECTED_ENTITIES"})
smach.StateMachine.add("SAY_UNREACHABLE",
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,
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",
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",
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",
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",
NavigateToSymbolic(robot,
{entity_des: "near"},
entity_des),
transitions={
"arrived": "SAY_ARRIVED",
"unreachable": "unreachable",
"goal_not_defined": "goal_not_defined",
})
smach.StateMachine.add(
"SAY_ARRIVED",
Say(robot,
"We have arrived. I'll go back to the meeting point"),
transitions={"spoken": "arrived"})
def __init__(self, robot, arm_designator, grab_entity_designator):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
check_type(arm_designator, Arm)
self.arm_designator = arm_designator
self.robot = robot
self.grab_entity_designator = grab_entity_designator
'''check check input and output keys'''
with self:
smach.StateMachine.add('CLOSE_GRIPPER',
SetGripper(robot,
arm_designator,
gripperstate=ArmState.CLOSE),
transitions={
'succeeded': 'PRE_POINT',
'failed': 'PRE_POINT'
})
smach.StateMachine.add('PRE_POINT',
ArmToQueryPoint(robot,
arm_designator,
grab_entity_designator,
time_out=20,
pre_grasp=True,
first_joint_pos_only=True),
transitions={
'succeeded': 'POINT',
'failed': 'RESET_ARM_FAILED'
})
smach.StateMachine.add('POINT',
Point_at_object(arm_designator, robot,
grab_entity_designator),
transitions={
'point_succeeded': 'SAY_POINTED',
'point_failed': 'RETRACT',
'target_lost': 'RESET_ARM_FAILED'
})
smach.StateMachine.add(
'SAY_POINTED',
Say(robot, "I hope this is the object you were looking for."),
transitions={'spoken': 'RETRACT'})
smach.StateMachine.add('RETRACT',
ArmToJointConfig(robot, arm_designator,
'retract'),
transitions={
'succeeded': 'RESET_ARM_SUCCEEDED',
'failed': 'RESET_ARM_SUCCEEDED'
})
smach.StateMachine.add('RESET_ARM_SUCCEEDED',
ArmToJointConfig(robot, arm_designator,
'reset'),
transitions={
'done': 'succeeded',
'failed': 'succeeded'
})
smach.StateMachine.add('RESET_ARM_FAILED',
ArmToJointConfig(robot, arm_designator,
'reset'),
transitions={
'done': 'failed',
'failed': 'failed'
})
def __init__(self, robot, furniture_designator, entity_designator):
# type: (Robot, object) -> None
"""
Drives to the designated furniture object, inspects this and selects the entity that will be pointed to
:param robot: (Robot) robot API object
:param furniture_designator: (EdEntityDesignator) designates the furniture object that was pointed to.
:param entity_designator: (EdEntityDesignator) writeable EdEntityDesignator
"""
# ToDo: we need to add userdata
smach.StateMachine.__init__(self,
outcomes=["succeeded", "failed"],
input_keys=["laser_dot"])
assert ds.is_writeable(
entity_designator
), "Entity designator must be writeable for this purpose"
object_ids_des = ds.VariableDesignator(
[], resolve_type=[ClassificationResult])
with self:
smach.StateMachine.add("SAY_GO",
Say(robot,
"Let's go to the {furniture_object}",
furniture_object=ds.AttrDesignator(
furniture_designator,
"id",
resolve_type=str)),
transitions={"spoken": "INSPECT_FURNITURE"})
smach.StateMachine.add("INSPECT_FURNITURE",
Inspect(robot=robot,
entityDes=furniture_designator,
objectIDsDes=object_ids_des,
navigation_area="in_front_of"),
transitions={
"done": "SELECT_ENTITY",
"failed": "SAY_INSPECTION_FAILED"
}) # ToDo: fallback?
smach.StateMachine.add(
"SAY_INSPECTION_FAILED",
Say(robot,
"I am sorry but I was not able to reach the {furniture_object}",
furniture_object=ds.AttrDesignator(furniture_designator,
"id",
resolve_type=str)),
transitions={"spoken": "failed"})
@smach.cb_interface(outcomes=["succeeded", "no_entities"],
input_keys=["laser_dot"])
def select_entity(userdata):
"""
Selects the entity that the robot believes the operator has pointed to and that the robot will
identify later on.
Userdata contains key 'laser_dot' with value geometry_msgs.msg.PointStamped where the operator pointed
at.
:param userdata: (dict)
:return: (str) outcome
"""
assert userdata.laser_dot.header.frame_id.endswith(
"map"), "Provide your laser dot in map frame"
# Extract classification results
entity_ids = [cr.id for cr in object_ids_des.resolve()]
rospy.loginfo("Segmented entities: {}".format(entity_ids))
# Obtain all corresponding entities
all_entities = robot.ed.get_entities()
segmented_entities = [
e for e in all_entities if e.id in entity_ids
]
# Filter out 'unprobable' entities
candidates = []
for entity in segmented_entities: # type: Entity
# The following filtering has been 'copied' from the cleanup challenge
# It can be considered a first step but does not take the orientation of the convex hull into
# account
shape = entity.shape
size_x = max(shape.x_max - shape.x_min, 0.001)
size_y = max(shape.y_max - shape.y_min, 0.001)
if size_x > SIZE_LIMIT or size_y > SIZE_LIMIT:
continue
if not 1 / min(RATIO_LIMIT,
1000) <= size_x / size_y <= min(
RATIO_LIMIT, 1000):
continue
candidates.append(entity)
# If no entities left: don't bother continuing
if not candidates:
rospy.logwarn("No 'probable' entities left")
return "no_entities"
# Select entity closest to the point where the operator pointed at (i.e., closest in 2D)
closest_tuple = (None, None)
x_ref = userdata.laser_dot.point.x
y_ref = userdata.laser_dot.point.y
# ToDo: use sorting for this...
for e in candidates: # type: Entity
x_e = e.pose.frame.p.x()
y_e = e.pose.frame.p.y()
distance_2d = math.hypot(x_ref - x_e, y_ref - y_e)
rospy.loginfo("Entity {} at {}, {}: distance = {}".format(
e.id, x_e, y_e, distance_2d))
if closest_tuple[0] is None or distance_2d < closest_tuple[
1]:
closest_tuple = (e, distance_2d)
rospy.loginfo("Best entity: {} at {}".format(
closest_tuple[0].id, closest_tuple[1]))
entity_designator.write(closest_tuple[0])
return "succeeded"
smach.StateMachine.add("SELECT_ENTITY",
smach.CBState(select_entity),
transitions={
"succeeded": "succeeded",
"no_entities": "failed"
})
