def test_look_at_area_looks_at_correct_point(self):
"""Test that the robot looks at the center point of the named area, w.r.t. the frame of the entity"""
entity_ds = ds.Designator(self.entity)
area_ds = ds.Designator(self.area)
state = states.LookAtArea(self.robot, entity_ds, area_ds, waittime=0)
state.execute()
vs = VectorStamped(0.5, 0.5, 0.5, "/12345")
self.robot.head.look_at_point.assert_called_with(vs, timeout=0)
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):
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 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 test_look_on_top_of_entity_looks_at_correct_point(self):
"""Test that the robot looks at the center point of the named area, w.r.t. the frame of the entity"""
entity_ds = ds.Designator(self.entity)
state = states.LookOnTopOfEntity(self.robot, entity_ds, waittime=0)
state.execute()
vs = VectorStamped(
0,
0,
1, # This is the height of the object, as indicated by the z_max
"/12345") # The ID
self.robot.head.look_at_point.assert_called_with(vs)
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'
parser = argparse.ArgumentParser(
description=
"Place an imaginary object at provided x, y, z coordinates using the "
"'Place' smach state")
parser.add_argument("x", type=float, help="x-coordinate (in map)")
parser.add_argument("y", type=float, help="y-coordinate (in map)")
parser.add_argument("z", type=float, help="z-coordinate (in map)")
parser.add_argument("--robot",
default="hero",
help="Robot name (amigo, hero, sergio)")
args = parser.parse_args()
rospy.init_node("test_placing")
robot = get_robot(args.robot)
pose = FrameStamped(frame=kdl.Frame(kdl.Rotation.RPY(0.0, 0.0, 0.0),
kdl.Vector(args.x, args.y, args.z)),
frame_id="/map")
item = Entity("dummy_id", "dummy_type", None, None, None, None, None, None)
arm = ds.ArmDesignator(robot,
arm_properties={
"required_trajectories": ["prepare_place"],
"required_gripper_types":
[arms.GripperTypes.GRASPING]
})
place_state = Place(robot, ds.Designator(item), ds.Designator(pose), arm)
place_state.execute()
def __init__(self, robot, door_waypoint, guest_ent_des, guest_name_des,
guest_drink_des):
"""
Learn what a guest looks like and what his/her favourite drink is
:param robot: Robot that should execute this state
:param door_waypoint: Entity-designator resolving to a waypoint Where are guests expected to come in
:param guest_ent_des: Entity of the guest
:param guest_name_des: designator that the name (str) of the guest is written to
:param guest_drink_des: designator that the drink type (str) of the drink the guest wants
"""
smach.StateMachine.__init__(
self, outcomes=['succeeded', 'failed', 'aborted'])
self.drink_spec_des = ds.Designator(
challenge_knowledge.common.drink_spec, name='drink_spec')
with self:
smach.StateMachine.add(
'GOTO_DOOR',
states.NavigateToWaypoint(
robot, door_waypoint,
challenge_knowledge.waypoint_door['radius']),
transitions={
'arrived': 'SAY_OPEN_DOOR',
'unreachable': 'SAY_OPEN_DOOR',
'goal_not_defined': 'aborted'
})
smach.StateMachine.add(
'SAY_OPEN_DOOR',
states.Say(
robot,
["Someone please open the door, I'm expecting guests"],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'SAY_PLEASE_COME_IN'})
smach.StateMachine.add(
'SAY_PLEASE_COME_IN',
states.Say(robot, [
"Please come in, I'm waiting for someone to step in front of me"
],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'WAIT_FOR_GUEST'})
smach.StateMachine.add("WAIT_FOR_GUEST",
states.WaitForPersonInFront(
robot, attempts=30, sleep_interval=1),
transitions={
'success': 'SAY_HELLO',
'failed': 'SAY_PLEASE_COME_IN'
})
smach.StateMachine.add(
'SAY_HELLO',
states.Say(robot, ["Hi there, I'll learn your face now"],
block=True,
look_at_standing_person=True),
transitions={'spoken': 'ASK_GUEST_NAME'})
smach.StateMachine.add('ASK_GUEST_NAME',
states.AskPersonName(
robot, guest_name_des.writeable,
challenge_knowledge.common.names),
transitions={
'succeeded': 'LEARN_PERSON',
'failed': 'SAY_HELLO',
'timeout': 'SAY_HELLO'
})
smach.StateMachine.add('LEARN_PERSON',
states.LearnPerson(
robot, name_designator=guest_name_des),
transitions={
'succeeded': 'SAY_GUEST_LEARNED',
'failed': 'SAY_FAILED_LEARNING'
})
smach.StateMachine.add(
'SAY_FAILED_LEARNING',
states.Say(robot,
["Not sure if I remember you, but I'll do my best"],
block=False),
transitions={'spoken': 'SAY_DRINK_QUESTION'})
smach.StateMachine.add(
'SAY_GUEST_LEARNED',
states.Say(robot, ["Okidoki, now I know what you look like"],
block=True),
transitions={'spoken': 'SAY_DRINK_QUESTION'})
smach.StateMachine.add('SAY_DRINK_QUESTION',
states.Say(robot,
["What's your favorite drink?"],
block=True),
transitions={'spoken': 'HEAR_DRINK_ANSWER'})
smach.StateMachine.add('HEAR_DRINK_ANSWER',
states.HearOptionsExtra(
robot, self.drink_spec_des,
guest_drink_des.writeable),
transitions={
'heard': 'RESET_1',
'no_result': 'SAY_DRINK_QUESTION'
})
smach.StateMachine.add('RESET_1',
states.ResetArms(robot),
transitions={'done': 'succeeded'})
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"})
from geometry_msgs.msg import Pose
if len(sys.argv) < 2:
print("Please provide robot name as argument.")
sys.exit(1)
robot_name = sys.argv[1]
rospy.init_node('test_take_orders')
robot = get_robot(robot_name)
pose = Pose()
pose.position.x = 1.0
pose.position.y = 1.0
pose.position.z = 1.6
customer_entity = Entity(
'random_id',
'person',
'/map', # FrameID can only be map frame unfortunately, Our KDL wrapper doesn't do well with PoseStampeds etc.
'dummy', # Only pose and frame_id are used
'shape',
'volumes',
'super_types',
'last_update_time')
customer_entity.pose = pose # This takes care of the conversion to KDL for us
orders = []
sm = TakeOrder(robot, ds.Designator(customer_entity), orders=orders)
sm.execute()
rospy.loginfo("Orders {}".format(orders))
def test_collection_non_iterable(self):
self.assertRaises(AssertionError, IterateDesignator,
ds.Designator('a'),
ds.VariableDesignator(resolve_type=str).writeable)
def __init__(self, robot, seats_to_inspect, room, seat_is_for=None):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
seats = SeatsInRoomDesignator(robot, seats_to_inspect, room,
"seats_in_room")
seat_ent_des = ds.VariableDesignator(resolve_type=Entity)
if seat_is_for:
ds.check_type(seat_is_for, str)
else:
seat_is_for = ds.Designator(' ')
with self:
smach.StateMachine.add(
'SAY_LETS_FIND_SEAT',
states.SayFormatted(robot, [
"Let me find a place for {name} to sit. Please be patient while I check out where there's place to sit"
],
name=seat_is_for,
block=False),
transitions={'spoken': 'ITERATE_NEXT_SEAT'})
smach.StateMachine.add('ITERATE_NEXT_SEAT',
states.IterateDesignator(
seats, seat_ent_des.writeable),
transitions={
'next': 'CHECK_SEAT_EMPTY',
'stop_iteration': 'SAY_NO_EMPTY_SEATS'
})
smach.StateMachine.add('CHECK_SEAT_EMPTY',
states.CheckVolumeEmpty(
robot, seat_ent_des, 'on_top_of', 0.2),
transitions={
'occupied': 'ITERATE_NEXT_SEAT',
'empty': 'POINT_AT_EMPTY_SEAT',
'partially_occupied':
'POINT_AT_PARTIALLY_OCCUPIED_SEAT',
'failed': 'ITERATE_NEXT_SEAT'
})
smach.StateMachine.add(
'POINT_AT_EMPTY_SEAT',
states.PointAt(robot=robot,
arm_designator=ds.UnoccupiedArmDesignator(
robot, {'required_goals': ['point_at']}),
point_at_designator=seat_ent_des,
look_at_designator=seat_ent_des),
transitions={
"succeeded": "SAY_SEAT_EMPTY",
"failed": "SAY_SEAT_EMPTY"
})
smach.StateMachine.add(
'SAY_SEAT_EMPTY',
states.SayFormatted(robot,
["Please sit on the {seat}, {name}"],
name=seat_is_for,
seat=ds.AttrDesignator(seat_ent_des,
'id',
resolve_type=str),
block=True),
transitions={'spoken': 'RESET_SUCCESS'})
smach.StateMachine.add(
'POINT_AT_PARTIALLY_OCCUPIED_SEAT',
states.PointAt(robot=robot,
arm_designator=ds.UnoccupiedArmDesignator(
robot, {'required_goals': ['point_at']}),
point_at_designator=seat_ent_des,
look_at_designator=seat_ent_des),
transitions={
"succeeded": "SAY_SEAT_PARTIALLY_OCCUPIED",
"failed": "SAY_SEAT_PARTIALLY_OCCUPIED"
})
smach.StateMachine.add(
'SAY_SEAT_PARTIALLY_OCCUPIED',
states.SayFormatted(robot, [
"I think there's some space left here where you can sit {name}"
],
name=seat_is_for,
block=True),
transitions={'spoken': 'RESET_SUCCESS'})
smach.StateMachine.add(
'SAY_NO_EMPTY_SEATS',
states.SayFormatted(robot, [
"Sorry, there are no empty seats. I guess you just have to stand {name}"
],
name=seat_is_for,
block=True),
transitions={'spoken': 'RESET_FAIL'})
smach.StateMachine.add('RESET_FAIL',
states.ResetArms(robot),
transitions={'done': 'failed'})
smach.StateMachine.add('RESET_SUCCESS',
states.ResetArms(robot),
transitions={'done': 'succeeded'})
robot = get_robot(args.robot)
entity_id = "test_item"
pose = FrameStamped(frame=kdl.Frame(kdl.Rotation.RPY(0.0, 0.0, 0.0),
kdl.Vector(args.x, args.y, args.z)),
frame_id="/map")
robot.ed.update_entity(id=entity_id, frame_stamped=pose)
shape = RightPrism([
kdl.Vector(0, 0, 0),
kdl.Vector(0, 0.05, 0),
kdl.Vector(0.05, 0.05, 0),
kdl.Vector(0.05, 0, 0)
], -0.1, 0.1)
item = Entity(entity_id, "test_type", pose.frame_id, pose.frame, shape,
None, None, None)
item = ds.Designator(item)
arm = ds.UnoccupiedArmDesignator(robot,
arm_properties={
"required_trajectories":
["prepare_grasp"],
"required_goals": ["carrying_pose"],
"required_gripper_types":
[arms.GripperTypes.GRASPING]
})
grab_state = Grab(robot, item, arm)
grab_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
import rospy
from robot_smach_states.designator_iterator import IterateDesignator
import robot_smach_states.util.designators as ds
if __name__ == "__main__":
collection = ['a', 'b', 'c']
collection_des = ds.Designator(collection)
element_des = ds.VariableDesignator(resolve_type=str)
iterator = IterateDesignator(collection_des, element_des.writeable)
for i in range(8):
rospy.loginfo("iterator.execute() \n {}".format(iterator.execute()))
rospy.loginfo("element_des.resolve() \n {} \n".format(element_des.resolve()))
def setUpClass(cls):
cls.collection_des = ds.Designator(['a', 'b', 'c'])
cls.element_des = ds.VariableDesignator(resolve_type=str).writeable
cls.iterator = IterateDesignator(cls.collection_des, cls.element_des)
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 setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
# Designators
# Room to search through
roomr = ds.VariableDesignator(resolve_type=str)
roomw = roomr.writeable
# Cleanup location as defined in local knowledge
cleanup_locationsr = ds.VariableDesignator([{'1': '2', '3': '4'}])
cleanup_locationsw = cleanup_locationsr.writeable
location_des = ds.VariableDesignator(resolve_type=dict)
with sm:
smach.StateMachine.add("START_ROBUST",
robot_smach_states.StartChallengeRobust(
robot, challenge_knowledge.starting_point),
transitions={
"Done": "GO_TO_WAITING_POINT",
"Aborted": "GO_TO_WAITING_POINT",
"Failed": "GO_TO_WAITING_POINT"
})
smach.StateMachine.add("GO_TO_WAITING_POINT",
robot_smach_states.NavigateToWaypoint(
robot,
ds.EntityByIdDesignator(
robot,
challenge_knowledge.waiting_point)),
transitions={
"arrived": "INQUIRE_ROOM",
"unreachable": "GO_TO_WAITING_POINT1",
"goal_not_defined": "GO_TO_WAITING_POINT1"
})
smach.StateMachine.add("GO_TO_WAITING_POINT1",
robot_smach_states.NavigateToWaypoint(
robot,
ds.EntityByIdDesignator(
robot,
challenge_knowledge.waiting_point),
radius=0.3),
transitions={
"arrived": "INQUIRE_ROOM",
"unreachable": "INQUIRE_ROOM",
"goal_not_defined": "INQUIRE_ROOM"
})
smach.StateMachine.add("INQUIRE_ROOM",
AskWhichRoomToClean(
robot,
ds.Designator(challenge_knowledge.grammar),
roomw, cleanup_locationsw),
transitions={"done": "VERIFY"})
smach.StateMachine.add('VERIFY',
VerifyWorldModelInfo(robot),
transitions={
"done": "SAY_START_CHALLENGE",
"failed": "SAY_KNOWLEDGE_NOT_COMPLETE"
})
smach.StateMachine.add(
'SAY_KNOWLEDGE_NOT_COMPLETE',
robot_smach_states.Say(robot, [
"My knowledge of the world is not complete!",
"Please give me some more information!"
],
block=False),
transitions={"spoken": "Aborted"})
smach.StateMachine.add('SAY_START_CHALLENGE',
robot_smach_states.Say(robot, [
"Starting the cleanup challenge",
"What a mess here, let's clean this room!",
"Let's see if I can find some garbage here",
"All I want to do is clean this mess up!"
],
block=False),
transitions={"spoken": "ITERATE_NEXT_LOC"})
smach.StateMachine.add('ITERATE_NEXT_LOC',
robot_smach_states.IterateDesignator(
cleanup_locationsr, location_des.writeable),
transitions={
"next": "INSPECT",
"stop_iteration": "RETURN_TO_OPERATOR"
})
smach.StateMachine.add("INSPECT",
CleanInspect(robot, location_des),
transitions={"done": "ITERATE_NEXT_LOC"})
smach.StateMachine.add("RETURN_TO_OPERATOR",
robot_smach_states.NavigateToWaypoint(
robot=robot,
waypoint_designator=ds.EntityByIdDesignator(
robot=robot,
id=challenge_knowledge.starting_point),
radius=0.3),
transitions={
"arrived": "SAY_CLEANED_ROOM",
"unreachable": "SAY_CLEANED_ROOM",
"goal_not_defined": "SAY_CLEANED_ROOM"
})
smach.StateMachine.add(
'SAY_CLEANED_ROOM',
robot_smach_states.Say(robot, [
"I successfully cleaned the {room}!",
"All done in the {room}. Am I a good robot now?",
"There, I cleaned up your mess in the {room}, are you happy now!"
],
room=roomr,
block=False),
transitions={"spoken": "Done"})
return sm
