#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