def test_constructor(self):
msg1 = Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_WILDCARD],
status=Request.NEW)
rq1 = ResourceRequest(msg1)
self.assertIsNotNone(rq1)
self.assertEqual(str(rq1),
'id: 01234567-89ab-cdef-fedc-ba9876543210\n'
' priority: 0\n'
' resources: \n'
' linux.precise.ros.segbot.*/test_rapp\n'
' status: 0')
self.assertEqual(rq1.msg.status, Request.NEW)
self.assertEqual(rq1.msg.resources, [TEST_WILDCARD])
# why is this broken???
#self.assertEqual(rq1.get_uuid, TEST_UUID)
rq2 = ResourceRequest(Request(id=unique_id.toMsg(DIFF_UUID),
resources=[TEST_RESOURCE],
status=Request.NEW))
self.assertEqual(rq2.msg.status, Request.NEW)
self.assertEqual(rq2.msg.resources, [TEST_RESOURCE])
self.assertEqual(rq2.get_uuid(), DIFF_UUID)
self.assertEqual(str(rq2),
'id: 01234567-cdef-fedc-89ab-ba9876543210\n'
' priority: 0\n'
' resources: \n'
' linux.precise.ros.segbot.roberto/test_rapp\n'
' status: 0')
def behaviour_to_msg(behaviour):
new_behaviour = py_trees_msgs.Behaviour()
new_behaviour.name = behaviour.name
new_behaviour.class_name = str(behaviour.__module__) + '.' + str(
type(behaviour).__name__)
new_behaviour.own_id = unique_id.toMsg(behaviour.id)
new_behaviour.parent_id = unique_id.toMsg(
behaviour.parent.id) if behaviour.parent else uuid_msgs.UniqueID()
new_behaviour.child_ids = [
unique_id.toMsg(child.id)
for child in behaviour.iterate(direct_descendants=True)
if not child.id == behaviour.id
]
tip = behaviour.tip()
# tip_id is empty if the behaviour is invalid or if it is a valid
# leaf
if tip is not None and tip != behaviour:
new_behaviour.tip_id = unique_id.toMsg(tip.id)
new_behaviour.type = convert_type(behaviour)
new_behaviour.blackbox_level = convert_blackbox_level(
behaviour.blackbox_level)
new_behaviour.status = convert_status(behaviour.status)
new_behaviour.message = behaviour.feedback_message
return new_behaviour
def test_one_request_set(self):
msg1 = Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_WILDCARD],
status=Request.NEW)
rset = RequestSet([msg1], RQR_UUID, contents=ResourceReply)
self.assertEqual(len(rset), 1)
self.assertTrue(TEST_UUID in rset)
self.assertEqual(rset[TEST_UUID].msg, msg1)
self.assertEqual(rset.get(TEST_UUID), rset[TEST_UUID])
self.assertFalse(DIFF_UUID in rset)
self.assertIsNone(rset.get(DIFF_UUID))
self.assertEqual(rset.get(DIFF_UUID, 10), 10)
self.assertTrue(rset == RequestSet([msg1], RQR_UUID))
self.assertTrue(rset == RequestSet([msg1], RQR_UUID,
contents=ResourceReply))
rs2 = copy.deepcopy(rset)
self.assertTrue(rset == rs2)
rset_str = """requester_id: 01234567-89ab-cdef-0123-456789abcdef
requests:
id: 01234567-89ab-cdef-fedc-ba9876543210
priority: 0
resources:
linux.precise.ros.segbot.*/test_rapp
status: 0"""
self.assertEqual(str(rset), rset_str)
sch_msg = SchedulerRequests(requester=unique_id.toMsg(RQR_UUID),
requests=[msg1])
self.assertEqual(rset.to_msg(stamp=rospy.Time()), sch_msg)
def test_abort(self):
rq1 = ResourceReply(Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_RESOURCE],
status=Request.NEW))
rq1.abort()
self.assertEqual(rq1.msg.status, Request.ABORTED)
rq2 = ResourceReply(Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_RESOURCE],
status=Request.WAITING))
rq2.abort()
self.assertEqual(rq2.msg.status, Request.ABORTED)
rq3 = ResourceReply(Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_RESOURCE],
status=Request.PREEMPTING))
rq3.abort()
self.assertEqual(rq3.msg.status, Request.ABORTED)
rq4 = ResourceReply(Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_RESOURCE],
status=Request.PREEMPTED))
rq4.abort()
self.assertEqual(rq4.msg.status, Request.ABORTED)
rq5 = ResourceReply(Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_WILDCARD],
status=Request.GRANTED))
rq5.abort()
self.assertEqual(rq5.msg.status, Request.ABORTED)
def update_geo_path(self):
if (self.waypoints and self.zone and self.band):
waypoints_geo = []
for point in self.waypoints:
x, y = point
geo = UTMPoint(x, y, zone=self.zone, band=self.band).toMsg()
wp = WayPoint()
wp.id = unique_id.toMsg(unique_id.fromRandom())
wp.position = geo
waypoints_geo.append(wp)
segments_geo = []
last_wp = waypoints_geo[0].id
for wp in waypoints_geo:
seg = RouteSegment()
seg.id = unique_id.toMsg(unique_id.fromRandom())
seg.start = last_wp
seg.end = wp.id
last_wp = wp.id
segments_geo.append(seg)
self.route_network.points = waypoints_geo
self.route_network.segments = segments_geo
else:
rospy.logwarn('[update_geo_path] waypoints, zone, and/or band\n' +
'\thas not been set/updated.\n\tIgnoring...')
def test_reject(self):
rq1 = ResourceReply(Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_RESOURCE],
status=Request.NEW))
rq1.reject()
self.assertEqual(rq1.msg.status, Request.REJECTED)
rq2 = ResourceReply(Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_RESOURCE],
status=Request.WAITING))
rq2.reject()
self.assertEqual(rq2.msg.status, Request.REJECTED)
rq3 = ResourceReply(Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_RESOURCE],
status=Request.PREEMPTING))
rq3.reject()
self.assertEqual(rq3.msg.status, Request.REJECTED)
rq4 = ResourceReply(Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_RESOURCE],
status=Request.PREEMPTED))
rq4.reject()
self.assertEqual(rq4.msg.status, Request.REJECTED)
rq5 = ResourceReply(Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_WILDCARD],
status=Request.GRANTED))
self.assertRaises(TransitionError, rq5.reject)
def test_canceled_merge_plus_new_request(self):
msg1 = Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_RESOURCE],
status=Request.CANCELING)
msg2 = Request(id=unique_id.toMsg(DIFF_UUID),
resources=[TEST_WILDCARD],
status=Request.NEW)
rset = RequestSet([msg1, msg2], RQR_UUID)
self.assertEqual(len(rset), 2)
self.assertIn(TEST_UUID, rset)
self.assertIn(DIFF_UUID, rset)
self.assertEqual(rset[DIFF_UUID].msg.status, Request.NEW)
# merge a canceled request: TEST_UUID should be deleted, but
# DIFF_UUID should not
msg3 = Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_RESOURCE],
status=Request.CLOSED)
rel_rset = RequestSet([msg3], RQR_UUID)
rset.merge(rel_rset)
self.assertEqual(len(rset), 1)
self.assertNotIn(TEST_UUID, rset)
self.assertIn(DIFF_UUID, rset)
self.assertEqual(rset[DIFF_UUID].msg.status, Request.NEW)
# make a fresh object like the original msg2 for comparison
msg4 = Request(id=unique_id.toMsg(DIFF_UUID),
resources=[TEST_WILDCARD],
status=Request.NEW)
self.assertEqual(rset, RequestSet([msg4], RQR_UUID))
self.assertNotEqual(rset, rel_rset)
def test_constructor(self):
msg1 = Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_WILDCARD],
status=Request.NEW)
rq1 = ResourceRequest(msg1)
self.assertIsNotNone(rq1)
self.assertEqual(str(rq1),
"""id: 01234567-89ab-cdef-fedc-ba9876543210
priority: 0
resources:
rocon:/segbot#test_rapp
status: 0""")
self.assertEqual(rq1.msg.status, Request.NEW)
self.assertEqual(rq1.msg.resources, [TEST_WILDCARD])
self.assertEqual(rq1.uuid, TEST_UUID)
rq2 = ResourceRequest(Request(id=unique_id.toMsg(DIFF_UUID),
resources=[TEST_RESOURCE],
status=Request.NEW))
self.assertEqual(rq2.msg.status, Request.NEW)
self.assertEqual(rq2.msg.resources, [TEST_RESOURCE])
self.assertEqual(rq2.uuid, DIFF_UUID)
self.assertEqual(str(rq2),
"""id: 01234567-cdef-fedc-89ab-ba9876543210
priority: 0
resources:
rocon:/segbot/roberto#test_rapp
status: 0""")
def test_constructor(self):
msg1 = Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_WILDCARD],
status=Request.NEW)
rq1 = ResourceRequest(msg1)
self.assertIsNotNone(rq1)
self.assertEqual(
str(rq1), """id: 01234567-89ab-cdef-fedc-ba9876543210
priority: 0
resources:
rocon:/segbot#test_rapp
status: 0""")
self.assertEqual(rq1.msg.status, Request.NEW)
self.assertEqual(rq1.msg.resources, [TEST_WILDCARD])
self.assertEqual(rq1.uuid, TEST_UUID)
rq2 = ResourceRequest(
Request(id=unique_id.toMsg(DIFF_UUID),
resources=[TEST_RESOURCE],
status=Request.NEW))
self.assertEqual(rq2.msg.status, Request.NEW)
self.assertEqual(rq2.msg.resources, [TEST_RESOURCE])
self.assertEqual(rq2.uuid, DIFF_UUID)
self.assertEqual(
str(rq2), """id: 01234567-cdef-fedc-89ab-ba9876543210
priority: 0
resources:
rocon:/segbot/roberto#test_rapp
status: 0""")
def filter_by(self, world, uuids=[], names=[], types=[], keywords=[], relationships=[]):
'''
Reload annotations collection, filtered by new selection criteria.
@param world: Annotations in this collection belong to this world.
@param uuids: Filter annotations by their uuid.
@param names: Filter annotations by their name.
@param types: Filter annotations by their type.
@param keywords: Filter annotations by their keywords.
@param relationships: Filter annotations by their relationships.
@raise WCFError: If something went wrong.
'''
rospy.loginfo("Getting annotations for world '%s' and additional filter criteria", world)
get_anns_srv = self._get_service_handle('get_annotations', world_canvas_msgs.srv.GetAnnotations)
ids = [unique_id.toMsg(uuid.UUID('urn:uuid:' + id)) for id in uuids]
relationships = [unique_id.toMsg(uuid.UUID('urn:uuid:' + id)) for id in relationships]
response = get_anns_srv(world, ids, names, types, keywords, relationships)
if response.result:
if len(response.annotations) > 0:
rospy.loginfo("%d annotations found", len(response.annotations))
self._annotations = response.annotations
else:
rospy.loginfo("No annotations found for world '%s' with the given search criteria", world)
del self._annotations[:]
# All non-saved information gets invalidated after reload the collection
del self._annots_data[:]
del self._annots_to_delete[:]
self._saved = True
else:
message = "Server reported an error: %s" % response.message
rospy.logerr(message)
raise WCFError(message)
def test_canceled_merge_plus_new_request(self):
msg1 = Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_RESOURCE],
status=Request.CANCELING)
msg2 = Request(id=unique_id.toMsg(DIFF_UUID),
resources=[TEST_WILDCARD],
status=Request.NEW)
rset = RequestSet([msg1, msg2], RQR_UUID)
self.assertEqual(len(rset), 2)
self.assertIn(TEST_UUID, rset)
self.assertIn(DIFF_UUID, rset)
self.assertEqual(rset[DIFF_UUID].msg.status, Request.NEW)
# merge a canceled request: TEST_UUID should be deleted, but
# DIFF_UUID should not
msg3 = Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_RESOURCE],
status=Request.CLOSED)
rel_rset = RequestSet([msg3], RQR_UUID)
rset.merge(rel_rset)
self.assertEqual(len(rset), 1)
self.assertNotIn(TEST_UUID, rset)
self.assertIn(DIFF_UUID, rset)
self.assertEqual(rset[DIFF_UUID].msg.status, Request.NEW)
# make a fresh object like the original msg2 for comparison
msg4 = Request(id=unique_id.toMsg(DIFF_UUID),
resources=[TEST_WILDCARD],
status=Request.NEW)
self.assertEqual(rset, RequestSet([msg4], RQR_UUID))
self.assertNotEqual(rset, rel_rset)
def read(filename):
yaml_data = None
with open(filename) as f:
yaml_data = yaml.load(f)
anns_list = []
data_list = []
for t in yaml_data:
ann = Annotation()
ann.timestamp = rospy.Time.now()
ann.world_id = unique_id.toMsg(uuid.UUID('urn:uuid:' + world_id))
ann.data_id = unique_id.toMsg(unique_id.fromRandom())
ann.id = unique_id.toMsg(unique_id.fromRandom())
ann.name = t['name']
ann.type = 'yocs_msgs/Wall'
for i in range(0, random.randint(0,11)):
ann.keywords.append('kw'+str(random.randint(1,11)))
if 'prev_id' in vars():
ann.relationships.append(prev_id)
prev_id = ann.id
ann.shape = 1 # CUBE
ann.color.r = 0.8
ann.color.g = 0.2
ann.color.b = 0.2
ann.color.a = 0.4
ann.size.x = float(t['width'])
ann.size.y = float(t['length'])
ann.size.z = float(t['height'])
ann.pose.header.frame_id = t['frame_id']
ann.pose.header.stamp = rospy.Time.now()
ann.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose'])
# walls are assumed to lay on the floor, so z coordinate is zero;
# but WCF assumes that the annotation pose is the center of the object
ann.pose.pose.pose.position.z += ann.size.z/2.0
anns_list.append(ann)
object = Wall()
object.name = t['name']
object.length = float(t['length'])
object.width = float(t['width'])
object.height = float(t['height'])
object.pose.header.frame_id = t['frame_id']
object.pose.header.stamp = rospy.Time.now()
object.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose'])
data = AnnotationData()
data.id = ann.data_id
data.data = pickle.dumps(object)
data_list.append(data)
print ann, object, data
return anns_list, data_list
def read(file):
yaml_data = None
with open(file) as f:
yaml_data = yaml.load(f)
anns_list = []
data_list = []
for t in yaml_data:
ann = Annotation()
ann.timestamp = rospy.Time.now()
ann.data_id = unique_id.toMsg(unique_id.fromRandom())
ann.id = unique_id.toMsg(unique_id.fromRandom())
ann.world = world
ann.name = t['name']
ann.type = 'yocs_msgs/Table'
for i in range(0, random.randint(0, 11)):
ann.keywords.append('kw' + str(random.randint(1, 11)))
if 'prev_id' in vars():
ann.relationships.append(prev_id)
prev_id = ann.id
ann.shape = 3 #CYLINDER
ann.color.r = 0.2
ann.color.g = 0.2
ann.color.b = 0.8
ann.color.a = 0.5
ann.size.x = float(t['radius']) * 2
ann.size.y = float(t['radius']) * 2
ann.size.z = float(t['height'])
ann.pose.header.frame_id = t['frame_id']
ann.pose.header.stamp = rospy.Time.now()
ann.pose.pose.pose = message_converter.convert_dictionary_to_ros_message(
'geometry_msgs/Pose', t['pose'])
# tables are assumed to lay on the floor, so z coordinate is zero;
# but WCF assumes that the annotation pose is the center of the object
ann.pose.pose.pose.position.z += ann.size.z / 2.0
anns_list.append(ann)
object = Table()
object.name = t['name']
object.radius = float(t['radius'])
object.height = float(t['height'])
object.pose.header.frame_id = t['frame_id']
object.pose.header.stamp = rospy.Time.now()
object.pose.pose.pose = message_converter.convert_dictionary_to_ros_message(
'geometry_msgs/Pose', t['pose'])
data = AnnotationData()
data.id = ann.data_id
data.type = ann.type
data.data = serialize_msg(object)
data_list.append(data)
return anns_list, data_list
def read(file):
yaml_data = None
with open(file) as f:
yaml_data = yaml.load(f)
anns_list = []
data_list = []
for t in yaml_data:
ann = Annotation()
ann.timestamp = rospy.Time.now()
ann.data_id = unique_id.toMsg(unique_id.fromRandom())
ann.id = unique_id.toMsg(unique_id.fromRandom())
ann.world = world
ann.name = t['name']
ann.type = 'yocs_msgs/Table'
for i in range(0, random.randint(0,11)):
ann.keywords.append('kw'+str(random.randint(1,11)))
if 'prev_id' in vars():
ann.relationships.append(prev_id)
prev_id = ann.id
ann.shape = 3 #CYLINDER
ann.color.r = 0.2
ann.color.g = 0.2
ann.color.b = 0.8
ann.color.a = 0.5
ann.size.x = float(t['radius'])*2
ann.size.y = float(t['radius'])*2
ann.size.z = float(t['height'])
ann.pose.header.frame_id = t['frame_id']
ann.pose.header.stamp = rospy.Time.now()
ann.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose'])
# tables are assumed to lay on the floor, so z coordinate is zero;
# but WCF assumes that the annotation pose is the center of the object
ann.pose.pose.pose.position.z += ann.size.z/2.0
anns_list.append(ann)
object = Table()
object.name = t['name']
object.radius = float(t['radius'])
object.height = float(t['height'])
object.pose.header.frame_id = t['frame_id']
object.pose.header.stamp = rospy.Time.now()
object.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose'])
data = AnnotationData()
data.id = ann.data_id
data.type = ann.type
data.data = serialize_msg(object)
data_list.append(data)
return anns_list, data_list
def read(file):
yaml_data = None
with open(file) as f:
yaml_data = yaml.load(f)
anns_list = []
data_list = []
for t in yaml_data:
ann = Annotation()
ann.timestamp = rospy.Time.now()
ann.data_id = unique_id.toMsg(unique_id.fromRandom())
ann.id = unique_id.toMsg(unique_id.fromRandom())
ann.world = world
ann.name = t['name']
ann.type = 'ar_track_alvar_msgs/AlvarMarker'
for i in range(0, random.randint(0, 11)):
ann.keywords.append('kw' + str(random.randint(1, 11)))
# if 'prev_id' in vars():
# ann.relationships.append(prev_id)
# prev_id = ann.id
ann.shape = 1 # CUBE
ann.color.r = 1.0
ann.color.g = 1.0
ann.color.b = 1.0
ann.color.a = 1.0
ann.size.x = 0.18
ann.size.y = 0.18
ann.size.z = 0.01
ann.pose.header.frame_id = t['frame_id']
ann.pose.header.stamp = rospy.Time.now()
ann.pose.pose.pose = message_converter.convert_dictionary_to_ros_message(
'geometry_msgs/Pose', t['pose'])
anns_list.append(ann)
object = AlvarMarker()
object.id = t['id']
object.confidence = t['confidence']
object.pose.header.frame_id = t['frame_id']
object.pose.header.stamp = rospy.Time.now()
object.pose.pose = message_converter.convert_dictionary_to_ros_message(
'geometry_msgs/Pose', t['pose'])
data = AnnotationData()
data.id = ann.data_id
data.type = ann.type
data.data = serialize_msg(object)
data_list.append(data)
return anns_list, data_list
def read(filename):
yaml_data = None
with open(filename) as f:
yaml_data = yaml.load(f)
anns_list = []
data_list = []
for t in yaml_data:
ann = Annotation()
ann.timestamp = rospy.Time.now()
ann.world_id = unique_id.toMsg(uuid.UUID('urn:uuid:' + world_id))
ann.data_id = unique_id.toMsg(unique_id.fromRandom())
ann.id = unique_id.toMsg(unique_id.fromRandom())
ann.name = t['name']
ann.type = 'ar_track_alvar/AlvarMarker'
for i in range(0, random.randint(0,11)):
ann.keywords.append('kw'+str(random.randint(1,11)))
# if 'prev_id' in vars():
# ann.relationships.append(prev_id)
# prev_id = ann.id
ann.shape = 1 # CUBE
ann.color.r = 1.0
ann.color.g = 1.0
ann.color.b = 1.0
ann.color.a = 1.0
ann.size.x = 0.18
ann.size.y = 0.18
ann.size.z = 0.01
ann.pose.header.frame_id = t['frame_id']
ann.pose.header.stamp = rospy.Time.now()
ann.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose'])
anns_list.append(ann)
object = AlvarMarker()
object.id = t['id']
object.confidence = t['confidence']
object.pose.header.frame_id = t['frame_id']
object.pose.header.stamp = rospy.Time.now()
object.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',t['pose'])
data = AnnotationData()
data.id = ann.data_id
data.data = pickle.dumps(object)
data_list.append(data)
print ann, object, data
return anns_list, data_list
def test_allocate_four_resources_failure(self):
""" Similar to test_allocate_permutation_two_resources(), but
here there are more permutations, so the allocator gives up
after the initial failure.
"""
pool = ResourcePool(
KnownResources(resources=[
CurrentStatus(uri=DUDE1_NAME, rapps={TELEOP_RAPP}),
CurrentStatus(uri=DUDE2_NAME, rapps={TELEOP_RAPP}),
CurrentStatus(uri=DUDE3_NAME, rapps={TELEOP_RAPP}),
CurrentStatus(uri=DUDE4_NAME,
rapps={TELEOP_RAPP, EXAMPLE_RAPP})
]))
rq = ActiveRequest(
Request(id=unique_id.toMsg(RQ_UUID),
resources=[
Resource(rapp=TELEOP_RAPP, uri=ANY_NAME),
Resource(rapp=EXAMPLE_RAPP, uri=DUDE4_NAME),
Resource(rapp=TELEOP_RAPP, uri=DUDE2_NAME),
Resource(rapp=TELEOP_RAPP, uri=DUDE3_NAME)
]))
self.assertRaises(InvalidRequestError, pool.allocate, rq)
for name in [DUDE1_NAME, DUDE2_NAME, DUDE3_NAME, DUDE4_NAME]:
self.assertEqual(pool[name].status, CurrentStatus.AVAILABLE)
self.assertIsNone(pool[name].owner)
def test_allocate_permutation_two_resources(self):
""" Request a regexp allocation followed by an exact
allocation. Initially the exact resource gets assigned to the
regexp, so the second part of the request fails. The
allocator must try the other permutation for it to succeed.
"""
pool = ResourcePool(
KnownResources(resources=[
CurrentStatus(uri=MARVIN_NAME,
rapps={TELEOP_RAPP, EXAMPLE_RAPP}),
CurrentStatus(uri=ROBERTO_NAME, rapps={TELEOP_RAPP})
]))
rq = ActiveRequest(
Request(id=unique_id.toMsg(RQ_UUID),
resources=[
Resource(rapp=TELEOP_RAPP, uri=ANY_NAME),
Resource(rapp=EXAMPLE_RAPP, uri=MARVIN_NAME)
]))
alloc = pool.allocate(rq)
self.assertTrue(alloc)
self.assertEqual(len(alloc), 2)
self.assertEqual(pool[MARVIN_NAME].status, CurrentStatus.ALLOCATED)
self.assertEqual(pool[MARVIN_NAME].owner, RQ_UUID)
self.assertEqual(pool[ROBERTO_NAME].status, CurrentStatus.ALLOCATED)
self.assertEqual(pool[ROBERTO_NAME].owner, RQ_UUID)
self.assertEqual(alloc[0], Resource(rapp=TELEOP_RAPP,
uri=ROBERTO_NAME))
self.assertEqual(alloc[1], Resource(rapp=EXAMPLE_RAPP,
uri=MARVIN_NAME))
def test_validate(self):
rq1 = ResourceRequest(Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_RESOURCE],
status=Request.NEW))
self.assertTrue(rq1._validate(Request.GRANTED))
self.assertTrue(rq1._validate(Request.PREEMPTING))
self.assertFalse(rq1._validate(Request.CLOSED))
def __call__(self, msg, timeout=None, callback=None, error_callback=None):
'''
Initiates and executes the client request to the server. The type of arguments
supplied determines whether to apply blocking or non-blocking behaviour.
@param msg : the request message
@type <name>Request
@param timeout : time to wait for data
@type rospy.Duration
@param callback : user callback invoked for responses of non-blocking calls
@type method with arguments (uuid_msgs.UniqueID, <name>Response)
@return msg/id : for blocking calls it is the response message, for non-blocking it is the unique id
@rtype <name>Response/uuid_msgs.UniqueID
'''
pair_request_msg = self.ServicePairRequest()
pair_request_msg.id = unique_id.toMsg(unique_id.fromRandom())
pair_request_msg.request = msg
key = unique_id.toHexString(pair_request_msg.id)
if callback == None and error_callback == None:
self._request_handlers[key] = BlockingRequestHandler(key)
return self._make_blocking_call(self._request_handlers[key],
pair_request_msg, timeout)
else:
request_handler = NonBlockingRequestHandler(
key, callback, error_callback)
self._request_handlers[key] = request_handler.copy()
self._make_non_blocking_call(request_handler, pair_request_msg,
timeout)
return pair_request_msg.id
def test_allocate_permutation_two_resources(self):
""" Request a regexp allocation followed by an exact
allocation. Initially the exact resource gets assigned to the
regexp, so the second part of the request fails. The
allocator must try the other permutation for it to succeed.
"""
pool = ResourcePool(KnownResources(resources=[
CurrentStatus(uri=MARVIN_NAME,
rapps={TELEOP_RAPP, EXAMPLE_RAPP}),
CurrentStatus(uri=ROBERTO_NAME,
rapps={TELEOP_RAPP})]))
rq = ActiveRequest(Request(
id=unique_id.toMsg(RQ_UUID),
resources=[Resource(rapp=TELEOP_RAPP, uri=ANY_NAME),
Resource(rapp=EXAMPLE_RAPP, uri=MARVIN_NAME)]))
alloc = pool.allocate(rq)
self.assertTrue(alloc)
self.assertEqual(len(alloc), 2)
self.assertEqual(pool[MARVIN_NAME].status, CurrentStatus.ALLOCATED)
self.assertEqual(pool[MARVIN_NAME].owner, RQ_UUID)
self.assertEqual(pool[ROBERTO_NAME].status, CurrentStatus.ALLOCATED)
self.assertEqual(pool[ROBERTO_NAME].owner, RQ_UUID)
self.assertEqual(alloc[0],
Resource(rapp=TELEOP_RAPP, uri=ROBERTO_NAME))
self.assertEqual(alloc[1],
Resource(rapp=EXAMPLE_RAPP, uri=MARVIN_NAME))
def test_matching_allocation_one_resource(self):
pool = ResourcePool(SINGLETON_POOL)
self.assertEqual(len(pool), 1)
self.assertEqual(pool.known_resources(), SINGLETON_POOL)
self.assertFalse(pool.changed)
res = Resource(rapp=TELEOP_RAPP, uri=ANY_NAME)
subset = pool._match_subset(res, {CurrentStatus.AVAILABLE})
self.assertNotIn(MARVIN_NAME, subset)
self.assertIn(ROBERTO_NAME, subset)
self.assertEqual(subset, set([ROBERTO_NAME]))
self.assertEqual(pool.match_list([ROBERTO_RESOURCE],
{CurrentStatus.AVAILABLE}),
[set([ROBERTO_NAME])])
rq = copy.deepcopy(ANY_REQUEST)
alloc = pool.allocate(rq)
self.assertTrue(alloc)
self.assertTrue(pool.changed)
self.assertEqual(alloc[0], ROBERTO_RESOURCE)
self.assertEqual(pool[ROBERTO_NAME].status, CurrentStatus.ALLOCATED)
self.assertEqual(pool[ROBERTO_NAME].owner, RQ_UUID)
self.assertTrue(pool.changed)
self.assertEqual(
pool.known_resources(),
KnownResources(resources=[
CurrentStatus(uri=ROBERTO_NAME, rapps=TEST_RAPPS,
status=CurrentStatus.ALLOCATED,
owner=unique_id.toMsg(RQ_UUID))]))
self.assertFalse(pool.changed)
def test_empty_merge(self):
msg1 = Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_WILDCARD],
status=Request.NEW)
rset = RequestSet([msg1], RQR_UUID)
self.assertEqual(len(rset), 1)
self.assertIn(TEST_UUID, rset)
sch_msg = SchedulerRequests(requester=unique_id.toMsg(RQR_UUID),
requests=[msg1])
self.assertEqual(rset.to_msg(stamp=rospy.Time()), sch_msg)
# merge an empty request set: rset should remain the same
rset.merge(RequestSet([], RQR_UUID, contents=ActiveRequest))
self.assertEqual(len(rset), 1)
self.assertIn(TEST_UUID, rset)
self.assertEqual(rset.to_msg(stamp=rospy.Time()), sch_msg)
def create_alvar_marker_from_info(annotation_info, world, frame_id):
ann = Annotation()
ann.timestamp = rospy.Time.now()
ann.id = unique_id.toMsg(unique_id.fromRandom())
ann.world = world
ann.name = "marker " + str(annotation_info['name'])
ann.type = 'ar_track_alvar_msgs/AlvarMarker'
ann.keywords.append(str(world))
ann.shape = 0 # Cylinder
ann.color.r = 1.0
ann.color.g = 1.0
ann.color.b = 1.0
ann.color.a = 1.0
ann.size.x = 0.18
ann.size.y = 0.18
ann.size.z = 0.01
ann.pose.header.frame_id = frame_id
ann.pose.header.stamp = rospy.Time.now()
ann.pose.pose.pose.position.x = annotation_info['x']
ann.pose.pose.pose.position.y = annotation_info['y']
ann.pose.pose.pose.position.z = annotation_info['height']
(ann.pose.pose.pose.orientation.x, ann.pose.pose.pose.orientation.y,
ann.pose.pose.pose.orientation.z, ann.pose.pose.pose.orientation.w
) = tf.transformations.quaternion_from_euler(
radians(annotation_info['roll']), radians(annotation_info['pitch']),
radians(annotation_info['yaw']))
obj = ar_msgs.AlvarMarker()
obj.id = int(annotation_info['name'])
obj.confidence = 80
obj.pose.header = ann.pose.header
obj.pose.pose = ann.pose.pose.pose
return ann, obj
def test_matching_allocation_one_resource(self):
pool = ResourcePool(SINGLETON_POOL)
self.assertEqual(len(pool), 1)
self.assertEqual(pool.known_resources(), SINGLETON_POOL)
self.assertFalse(pool.changed)
res = Resource(rapp=TELEOP_RAPP, uri=ANY_NAME)
subset = pool._match_subset(res, {CurrentStatus.AVAILABLE})
self.assertNotIn(MARVIN_NAME, subset)
self.assertIn(ROBERTO_NAME, subset)
self.assertEqual(subset, set([ROBERTO_NAME]))
self.assertEqual(
pool.match_list([ROBERTO_RESOURCE], {CurrentStatus.AVAILABLE}),
[set([ROBERTO_NAME])])
rq = copy.deepcopy(ANY_REQUEST)
alloc = pool.allocate(rq)
self.assertTrue(alloc)
self.assertTrue(pool.changed)
self.assertEqual(alloc[0], ROBERTO_RESOURCE)
self.assertEqual(pool[ROBERTO_NAME].status, CurrentStatus.ALLOCATED)
self.assertEqual(pool[ROBERTO_NAME].owner, RQ_UUID)
self.assertTrue(pool.changed)
self.assertEqual(
pool.known_resources(),
KnownResources(resources=[
CurrentStatus(uri=ROBERTO_NAME,
rapps=TEST_RAPPS,
status=CurrentStatus.ALLOCATED,
owner=unique_id.toMsg(RQ_UUID))
]))
self.assertFalse(pool.changed)
def new_request(self, resources, priority=None, uuid=None):
""" Add a new scheduler request.
Call this method for each desired new request, then invoke
:py:meth:`.send_requests` to notify the scheduler.
:param resources: ROCON resources requested
:type resources: list of scheduler_msgs/Resource
:param priority: Scheduling priority of this request. If
``None`` provided, use this requester's priority.
:type priority: int
:param uuid: UUID_ of this request. If ``None`` provided, a
random UUID will be assigned.
:type uuid: :class:`uuid.UUID` or ``None``
:returns: UUID (:class:`uuid.UUID`) assigned.
:raises: :exc:`.WrongRequestError` if request already exists.
"""
if priority is None:
priority = self.priority
if uuid is None:
uuid = unique_id.fromRandom()
if uuid in self.rset:
raise WrongRequestError('UUID already in use.')
msg = Request(id=unique_id.toMsg(uuid),
priority=priority,
resources=resources,
status=Request.NEW)
self.rset[uuid] = ResourceRequest(msg)
return uuid
def test_validate(self):
rq1 = ResourceRequest(Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_RESOURCE],
status=Request.NEW))
self.assertTrue(rq1._validate(Request.GRANTED))
self.assertTrue(rq1._validate(Request.PREEMPTING))
self.assertFalse(rq1._validate(Request.CLOSED))
def new_request(self, resources, priority=None, uuid=None):
""" Add a new scheduler request.
Call this method for each desired new request, then invoke
:py:meth:`.send_requests` to notify the scheduler.
:param resources: ROCON resources requested
:type resources: list of scheduler_msgs/Resource
:param priority: Scheduling priority of this request. If
``None`` provided, use this requester's priority.
:type priority: int
:param uuid: UUID_ of this request. If ``None`` provided, a
random UUID will be assigned.
:type uuid: :class:`uuid.UUID` or ``None``
:returns: UUID (:class:`uuid.UUID`) assigned.
:raises: :exc:`.WrongRequestError` if request already exists.
"""
if priority is None:
priority = self.priority
if uuid is None:
uuid = unique_id.fromRandom()
if uuid in self.rset:
raise WrongRequestError('UUID already in use.')
msg = Request(id=unique_id.toMsg(uuid),
priority=priority,
resources=resources,
status=Request.NEW)
self.rset[uuid] = ResourceRequest(msg)
return uuid
def test_allocate_four_resources_success(self):
""" Similar to test_allocate_four_resources_failure(), but the
order of the request is different, so the allocator succeeds.
"""
pool = ResourcePool(
KnownResources(resources=[
CurrentStatus(uri=DUDE1_NAME, rapps={TELEOP_RAPP}),
CurrentStatus(uri=DUDE2_NAME, rapps={TELEOP_RAPP}),
CurrentStatus(uri=DUDE3_NAME, rapps={TELEOP_RAPP}),
CurrentStatus(uri=DUDE4_NAME,
rapps={TELEOP_RAPP, EXAMPLE_RAPP})
]))
rq = ActiveRequest(
Request(id=unique_id.toMsg(RQ_UUID),
resources=[
Resource(rapp=EXAMPLE_RAPP, uri=DUDE4_NAME),
Resource(rapp=TELEOP_RAPP, uri=DUDE2_NAME),
Resource(rapp=TELEOP_RAPP, uri=DUDE3_NAME),
Resource(rapp=TELEOP_RAPP, uri=ANY_NAME)
]))
alloc = pool.allocate(rq)
self.assertTrue(alloc)
bot_names = [DUDE4_NAME, DUDE2_NAME, DUDE3_NAME, DUDE1_NAME]
for name, i in zip(bot_names, range(4)):
self.assertEqual(pool[name].status, CurrentStatus.ALLOCATED)
self.assertEqual(alloc[i].uri, name)
def __call__(self, msg, timeout=None, callback=None, error_callback=None):
'''
Initiates and executes the client request to the server. The type of arguments
supplied determines whether to apply blocking or non-blocking behaviour.
If no callback is supplied, the mode is blocking, otherwise non-blocking.
If no timeout is specified, then a return of None indicates that the
operation timed out.
:param msg: the request message
:type msg: <name>Request
:param rospy.Duration timeout: time to wait for data
:param callback: user callback invoked for responses of non-blocking calls
:type callback: method with arguments (uuid_msgs.UniqueID, <name>Response)
:returns: msg/id for blocking calls it is the response message, for non-blocking it is the unique id
:rtype: <name>Response/uuid_msgs.UniqueID or None (if timed out)
'''
pair_request_msg = self.ServicePairRequest()
pair_request_msg.id = unique_id.toMsg(unique_id.fromRandom())
pair_request_msg.request = msg
key = unique_id.toHexString(pair_request_msg.id)
if callback == None and error_callback == None:
self._request_handlers[key] = BlockingRequestHandler(key)
return self._make_blocking_call(self._request_handlers[key], pair_request_msg, timeout)
else:
request_handler = NonBlockingRequestHandler(key, callback, error_callback)
self._request_handlers[key] = request_handler.copy()
self._make_non_blocking_call(request_handler, pair_request_msg, timeout)
return pair_request_msg.id
def current_status(self):
""" :returns: ``scheduler_msgs/CurrentStatus`` for this resource. """
msg = CurrentStatus(uri=self.uri, status=self.status, rapps=list(self.rapps))
if self.status == CurrentStatus.ALLOCATED:
msg.owner = unique_id.toMsg(self.owner)
msg.priority = self.priority
return msg
def test_empty_merge(self):
msg1 = Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_WILDCARD],
status=Request.NEW)
rset = RequestSet([msg1], RQR_UUID)
self.assertEqual(len(rset), 1)
self.assertIn(TEST_UUID, rset)
sch_msg = SchedulerRequests(requester=unique_id.toMsg(RQR_UUID),
requests=[msg1])
self.assertEqual(rset.to_msg(stamp=rospy.Time()), sch_msg)
# merge an empty request set: rset should remain the same
rset.merge(RequestSet([], RQR_UUID, contents=ActiveRequest))
self.assertEqual(len(rset), 1)
self.assertIn(TEST_UUID, rset)
self.assertEqual(rset.to_msg(stamp=rospy.Time()), sch_msg)
def enable(self, unique_identifier, interactions_loader):
'''
We don't need particularly complicated error codes and messages here as we don't do
any decision making (yet) on the result. Just true/false.
@param unique_identifier : unique id for this instance
@type : uuid.UUID
@param interactions_loader : used to load interactions
@type rocon_interactions.InteractionsLoader
'''
success = False
self._lock.acquire()
try:
# load up parameters first so that when start runs, it can find the params immediately
if self.msg.parameters != '':
namespace = concert_msgs.Strings.SERVICE_NAMESPACE + '/' + self.msg.name
load_parameters_from_file(self.msg.parameters, namespace, self.msg.name, load=True)
# Refresh the unique id
self.msg.uuid = unique_id.toMsg(unique_identifier)
self._start()
if self.msg.interactions != '':
# Can raise YamlResourceNotFoundException, MalformedInteractionsYaml
interactions_loader.load(self.msg.interactions, namespace=self._namespace, load=True)
# if there's a failure point, it will have thrown an exception before here.
success = True
self._update_callback()
self.loginfo("service enabled [%s]" % self.msg.name)
message = "success"
except (rocon_interactions.YamlResourceNotFoundException, rocon_interactions.MalformedInteractionsYaml) as e:
message = "failed to enable service [%s][%s]" % (self.msg.name, str(e))
self.logwarn(message)
self._lock.release()
return success, message
def test_one_request_set(self):
msg1 = Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_WILDCARD],
status=Request.NEW)
rset = RequestSet([msg1], RQR_UUID, contents=ActiveRequest)
self.assertEqual(len(rset), 1)
self.assertIn(TEST_UUID, rset)
self.assertEqual(rset[TEST_UUID].msg, msg1)
self.assertEqual(rset.get(TEST_UUID), rset[TEST_UUID])
self.assertNotIn(DIFF_UUID, rset)
self.assertIsNone(rset.get(DIFF_UUID))
self.assertEqual(rset.get(DIFF_UUID, 10), 10)
self.assertEqual(rset, RequestSet([msg1], RQR_UUID))
self.assertEqual(rset,
RequestSet([msg1], RQR_UUID, contents=ActiveRequest))
rset_str = """requester_id: 01234567-89ab-cdef-0123-456789abcdef
requests:
id: 01234567-89ab-cdef-fedc-ba9876543210
priority: 0
resources:
rocon:/segbot#test_rapp
status: 0"""
self.assertEqual(str(rset), rset_str)
sch_msg = SchedulerRequests(requester=unique_id.toMsg(RQR_UUID),
requests=[msg1])
self.assertEqual(rset.to_msg(stamp=rospy.Time()), sch_msg)
# vary message contents:
rs2 = copy.deepcopy(rset)
self.assertEqual(rset, rs2)
rq = rs2.get(TEST_UUID)
rq.msg.priority = 10
self.assertNotEqual(rset, rs2)
rq.msg.priority = 0
self.assertEqual(rset, rs2)
rq.msg.availability = rospy.Time(1000.0)
self.assertNotEqual(rset, rs2)
rq.msg.availability = rospy.Time()
self.assertEqual(rset, rs2)
rq.msg.hold_time = rospy.Duration(1000.0)
self.assertNotEqual(rset, rs2)
rq.msg.hold_time = rospy.Duration()
self.assertEqual(rset, rs2)
rq.msg.resources = [TEST_RESOURCE]
self.assertNotEqual(rset, rs2)
rq.msg.resources = [TEST_WILDCARD]
self.assertEqual(rset, rs2)
def test_one_request_set(self):
msg1 = Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_WILDCARD],
status=Request.NEW)
rset = RequestSet([msg1], RQR_UUID, contents=ActiveRequest)
self.assertEqual(len(rset), 1)
self.assertIn(TEST_UUID, rset)
self.assertEqual(rset[TEST_UUID].msg, msg1)
self.assertEqual(rset.get(TEST_UUID), rset[TEST_UUID])
self.assertNotIn(DIFF_UUID, rset)
self.assertIsNone(rset.get(DIFF_UUID))
self.assertEqual(rset.get(DIFF_UUID, 10), 10)
self.assertEqual(rset, RequestSet([msg1], RQR_UUID))
self.assertEqual(rset, RequestSet([msg1], RQR_UUID,
contents=ActiveRequest))
rset_str = """requester_id: 01234567-89ab-cdef-0123-456789abcdef
requests:
id: 01234567-89ab-cdef-fedc-ba9876543210
priority: 0
resources:
rocon:/segbot#test_rapp
status: 0"""
self.assertEqual(str(rset), rset_str)
sch_msg = SchedulerRequests(requester=unique_id.toMsg(RQR_UUID),
requests=[msg1])
self.assertEqual(rset.to_msg(stamp=rospy.Time()), sch_msg)
# vary message contents:
rs2 = copy.deepcopy(rset)
self.assertEqual(rset, rs2)
rq = rs2.get(TEST_UUID)
rq.msg.priority = 10
self.assertNotEqual(rset, rs2)
rq.msg.priority = 0
self.assertEqual(rset, rs2)
rq.msg.availability = rospy.Time(1000.0)
self.assertNotEqual(rset, rs2)
rq.msg.availability = rospy.Time()
self.assertEqual(rset, rs2)
rq.msg.hold_time = rospy.Duration(1000.0)
self.assertNotEqual(rset, rs2)
rq.msg.hold_time = rospy.Duration()
self.assertEqual(rset, rs2)
rq.msg.resources = [TEST_RESOURCE]
self.assertNotEqual(rset, rs2)
rq.msg.resources = [TEST_WILDCARD]
self.assertEqual(rset, rs2)
def current_status(self):
""" :returns: ``scheduler_msgs/CurrentStatus`` for this resource. """
msg = CurrentStatus(uri=self.uri, status=self.status,
rapps=list(self.rapps))
if self.status == CurrentStatus.ALLOCATED:
msg.owner = unique_id.toMsg(self.owner)
msg.priority = self.priority
return msg
def add(self, annotation, msg=None, gen_uuid=True):
'''
Add a new annotation with a new associated data or for an existing data.
:param annotation: The new annotation.
:param msg: Its associated data. If None, we assume that we are adding an annotation to existing data.
:param gen_uuid: Generate an unique id for the new annotation or use the received one.
:raises WCFError: If something went wrong.
'''
if gen_uuid:
annotation.id = unique_id.toMsg(unique_id.fromRandom())
else:
for a in self._annotations:
if a.id == annotation.id:
message = "Duplicated annotation with uuid '%s'" % unique_id.toHexString(
annotation.id)
rospy.logerr(message)
raise WCFError(message)
if msg is None:
# Msg not provided, so we assume that we are adding an annotation to existing data; find it by its id
msg_found = False
for d in self._annots_data:
if d.id == annotation.data_id:
rospy.logdebug("Annotation data with uuid '%s' found",
unique_id.toHexString(annotation.data_id))
msg_found = True
break
if not msg_found:
message = "Annotation data with uuid '%s' not found" % unique_id.toHexString(
annotation.data_id)
rospy.logerr(message)
raise WCFError(message)
else:
# Annotation comes with its data; create a common unique id to link both
annotation.data_id = unique_id.toMsg(unique_id.fromRandom())
annot_data = world_canvas_msgs.msg.AnnotationData()
annot_data.id = annotation.data_id
annot_data.type = annotation.type
annot_data.data = serialize_msg(msg)
self._annots_data.append(annot_data)
self._annotations.append(annotation)
self._saved = False
def test_closed_merge(self):
msg1 = Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_WILDCARD],
status=Request.CLOSED)
rset = RequestSet([msg1], RQR_UUID, contents=ActiveRequest)
self.assertEqual(len(rset), 1)
self.assertIn(TEST_UUID, rset)
sch_msg = SchedulerRequests(requester=unique_id.toMsg(RQR_UUID),
requests=[msg1])
self.assertEqual(rset.to_msg(stamp=rospy.Time()), sch_msg)
# merge an empty request set: TEST_UUID should be deleted
empty_rset = RequestSet([], RQR_UUID)
rset.merge(empty_rset)
self.assertEqual(len(rset), 0)
self.assertNotIn(TEST_UUID, rset)
self.assertNotEqual(rset.to_msg(stamp=rospy.Time()), sch_msg)
self.assertEqual(rset, empty_rset)
def _create_resource(self, uri):
# Create a resource to request
resource = scheduler_msgs.Resource()
resource.id = unique_id.toMsg(unique_id.fromRandom())
resource.rapp = self.resource_type
resource.uri = uri
compressed_image_topic_remapped = self._get_remapped_topic(rocon_uri.parse(resource.uri).name.string)
resource.remappings = [rocon_std_msgs.Remapping(self._default_compressed_image_topic, compressed_image_topic_remapped)]
return resource
def test_closed_merge(self):
msg1 = Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_WILDCARD],
status=Request.CLOSED)
rset = RequestSet([msg1], RQR_UUID, contents=ActiveRequest)
self.assertEqual(len(rset), 1)
self.assertIn(TEST_UUID, rset)
sch_msg = SchedulerRequests(requester=unique_id.toMsg(RQR_UUID),
requests=[msg1])
self.assertEqual(rset.to_msg(stamp=rospy.Time()), sch_msg)
# merge an empty request set: TEST_UUID should be deleted
empty_rset = RequestSet([], RQR_UUID)
rset.merge(empty_rset)
self.assertEqual(len(rset), 0)
self.assertNotIn(TEST_UUID, rset)
self.assertNotEqual(rset.to_msg(stamp=rospy.Time()), sch_msg)
self.assertEqual(rset, empty_rset)
def pub_resource_alloc(self, uri, options):
self.resource_alloc_flag = False
data = adapter_msgs.Adapter()
data.resource.id = unique_id.toMsg(unique_id.fromRandom())
data.resource.uri = uri
data.resource.rapp = self.rapp_parse(uri)
data.command = options
time.sleep(1)
self.resource_alloc_pub.publish(data)
def test_insufficient_resources(self):
# ask for two when there's only one
pool = ResourcePool(KnownResources(resources=[ROBERTO]))
rq1 = ActiveRequest(
Request(id=unique_id.toMsg(RQ_UUID),
resources=[ANY_RESOURCE, ANY_RESOURCE]))
alloc1 = pool.allocate(rq1)
self.assertFalse(alloc1)
self.assertEqual(pool[ROBERTO_NAME].status, CurrentStatus.AVAILABLE)
self.assertIsNone(pool[ROBERTO_NAME].owner)
def makeUniqueID(url, id=None):
"""Create a UniqueID message for *id* number in name space *ns*.
:param url: URL indicating generating source
:param id: (optional) identifier, unique within URL name space
:type id: int or string convertible to int
:returns: `geographic_msgs/UniqueID`_ message
:raises: :exc:`ValueError` if *id* not convertible to int.
"""
return unique_id.toMsg(generate(url, id))
def test_insufficient_resources(self):
# ask for two when there's only one
pool = ResourcePool(KnownResources(resources=[ROBERTO]))
rq1 = ActiveRequest(Request(
id=unique_id.toMsg(RQ_UUID),
resources=[ANY_RESOURCE, ANY_RESOURCE]))
alloc1 = pool.allocate(rq1)
self.assertFalse(alloc1)
self.assertEqual(pool[ROBERTO_NAME].status, CurrentStatus.AVAILABLE)
self.assertIsNone(pool[ROBERTO_NAME].owner)
def add(self, annotation, msg=None, gen_uuid=True):
'''
Add a new annotation with a new associated data or for an existing data.
@param annotation: The new annotation.
@param msg: Its associated data. If None, we assume that we are adding an annotation to existing data.
@param gen_uuid: Generate an unique id for the new annotation or use the received one.
@raise WCFError: If something went wrong.
'''
if gen_uuid:
annotation.id = unique_id.toMsg(unique_id.fromRandom())
else:
for a in self._annotations:
if a.id == annotation.id:
message = "Duplicated annotation with uuid '%s'" % unique_id.toHexString(annotation.id)
rospy.logerr(message)
raise WCFError(message)
if msg is None:
# Msg not provided, so we assume that we are adding an annotation to existing data; find it by its id
msg_found = False
for d in self._annots_data:
if d.id == annotation.data_id:
rospy.logdebug("Annotation data with uuid '%s' found", unique_id.toHexString(annotation.data_id))
msg_found = True
break
if not msg_found:
message = "Annotation data with uuid '%s' not found" % unique_id.toHexString(annotation.data_id)
rospy.logerr(message)
raise WCFError(message)
else:
# Annotation comes with its data; create a common unique id to link both
annotation.data_id = unique_id.toMsg(unique_id.fromRandom())
annot_data = world_canvas_msgs.msg.AnnotationData()
annot_data.id = annotation.data_id
annot_data.type = annotation.type
annot_data.data = serialize_msg(msg)
self._annots_data.append(annot_data)
self._annotations.append(annotation)
self._saved = False
def pub_command(self, duration, options):
self.command_flag = False
data = adapter_msgs.Adapter()
data.resource.id = unique_id.toMsg(unique_id.fromRandom())
data.command_duration = int(duration)
data.command = options
print data
time.sleep(1)
self.command_pub.publish(data)
def pub_command(self, duration, options):
self.command_flag = False
data = adapter_msgs.Adapter()
data.resource.id = unique_id.toMsg(unique_id.fromRandom())
data.command_duration = int(duration)
data.command = options
print data
time.sleep(1)
self.command_pub.publish(data)
def assert_invalid(self, request_type, old_status,
operation, exception, *args):
"""
Assert that *request_type* with *old_status* rejects named
*operation*, raising *exception*.
"""
rq = request_type(Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_WILDCARD],
status=old_status))
op_method = getattr(rq, operation)
self.assertRaises(exception, op_method, *args)
def assert_invalid(self, request_type, old_status,
operation, exception, *args):
"""
Assert that *request_type* with *old_status* rejects named
*operation*, raising *exception*.
"""
rq = request_type(Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_WILDCARD],
status=old_status))
op_method = getattr(rq, operation)
self.assertRaises(exception, op_method, *args)
def test_heap_queue(self):
qe1 = QueueElement(ActiveRequest(
Request(id=unique_id.toMsg(RQ1_UUID),
resources=[ROBERTO_RESOURCE],
priority=10)
), RQR_ID)
qe2 = QueueElement(ActiveRequest(
Request(id=unique_id.toMsg(RQ1_UUID),
resources=[MARVIN_RESOURCE],
priority=0)
), RQR_ID)
self.assertLess(qe1, qe2) # due to higher priority
h = []
heapq.heappush(h, qe2)
heapq.heappush(h, qe1)
self.assertEqual(len(h), 2)
self.assertEqual(heapq.heappop(h), qe1)
self.assertEqual(len(h), 1)
heapq.heappush(h, qe1)
self.assertEqual(len(h), 2)
self.assertEqual(heapq.heappop(h), qe1)
qe3 = QueueElement(ActiveRequest(
Request(id=unique_id.toMsg(RQ1_UUID),
resources=[ROBERTO_RESOURCE])
), RQR_ID)
qe4 = QueueElement(ActiveRequest(
Request(id=unique_id.toMsg(RQ1_UUID),
resources=[MARVIN_RESOURCE])
), RQR_ID)
self.assertLess(qe3, qe4) # due to sequence number
heapq.heappush(h, qe4)
heapq.heappush(h, qe3)
heapq.heappush(h, qe1)
self.assertEqual(len(h), 4)
self.assertEqual(heapq.heappop(h), qe1)
self.assertEqual(heapq.heappop(h), qe2)
self.assertEqual(heapq.heappop(h), qe3)
self.assertEqual(heapq.heappop(h), qe4)
self.assertEqual(len(h), 0)
self.assertRaises(IndexError, heapq.heappop, h)
def test_single_merge(self):
msg1 = Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_WILDCARD],
status=Request.NEW)
rset = RequestSet([msg1], RQR_UUID)
self.assertEqual(rset[TEST_UUID].msg.status, Request.NEW)
self.assertEqual(rset[TEST_UUID].msg.resources, [TEST_WILDCARD])
self.assertEqual(rset[TEST_UUID].msg.id, unique_id.toMsg(TEST_UUID))
# merge an updated request set: resource list should change
msg2 = Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_RESOURCE],
status=Request.GRANTED)
rset.merge(RequestSet([msg2], RQR_UUID, contents=ActiveRequest))
self.assertEqual(len(rset), 1)
self.assertIn(TEST_UUID, rset)
self.assertEqual(rset[TEST_UUID].msg.status, Request.GRANTED)
self.assertEqual(rset[TEST_UUID].msg.resources, [TEST_RESOURCE])
sch_msg = SchedulerRequests(requester=unique_id.toMsg(RQR_UUID),
requests=[msg2])
self.assertEqual(rset.to_msg(stamp=rospy.Time()), sch_msg)
def read(filename):
'''
Parse a yaml file containing a single map message
@param filename Target file path
'''
yaml_data = None
with open(filename) as f:
yaml_data = yaml.load(f)
ann = Annotation()
ann.timestamp = rospy.Time.now()
ann.world_id = unique_id.toMsg(uuid.UUID('urn:uuid:' + world_id))
ann.data_id = unique_id.toMsg(unique_id.fromRandom())
ann.id = unique_id.toMsg(unique_id.fromRandom())
ann.name = map_name
ann.type = 'nav_msgs/OccupancyGrid'
ann.keywords.append(map_name)
ann.shape = 1 # CUBE
ann.color.r = 0.2
ann.color.g = 0.2
ann.color.b = 0.2
ann.color.a = 1.0
ann.size.x = yaml_data['info']['width'] * yaml_data['info']['resolution']
ann.size.y = yaml_data['info']['height'] * yaml_data['info']['resolution']
ann.size.z = 0.000001
ann.pose.header.frame_id = '/map'
ann.pose.header.stamp = rospy.Time.now()
ann.pose.pose.pose = message_converter.convert_dictionary_to_ros_message('geometry_msgs/Pose',
yaml_data['info']['origin'])
object = OccupancyGrid()
genpy.message.fill_message_args(object, yaml_data)
map = AnnotationData()
map.id = ann.data_id
map.data = pickle.dumps(object)
print ann
return [ann], [map] # return as lists, as is what expects save_annotations_data service
def request_turtle(self, x_vel, z_vel, scale):
'''
Request a turtle.
'''
resource = scheduler_msgs.Resource()
resource.id = unique_id.toMsg(unique_id.fromRandom())
resource.rapp = 'turtle_concert/turtle_stroll'
resource.uri = 'rocon:/'
resource_request_id = self.requester.new_request([resource], priority=self.service_priority)
resource.parameters = [rocon_std_msgs.KeyValue('turtle_x_vel', str(x_vel)), rocon_std_msgs.KeyValue('turtle_z_vel', str(z_vel)), rocon_std_msgs.KeyValue('square_scale', str(scale))]
self.pending_requests.append(resource_request_id)
self.requester.send_requests()
def test_two_request_set(self):
msg1 = Request(id=unique_id.toMsg(TEST_UUID),
resources=[TEST_WILDCARD])
msg2 = Request(id=unique_id.toMsg(DIFF_UUID),
resources=[TEST_RESOURCE])
rset = RequestSet([msg1, msg2], RQR_UUID)
self.assertEqual(len(rset), 2)
self.assertIn(TEST_UUID, rset)
self.assertIn(DIFF_UUID, rset)
self.assertEqual(rset[TEST_UUID].msg, msg1)
self.assertEqual(rset[DIFF_UUID].msg, msg2)
self.assertEqual(rset.get(TEST_UUID), rset[TEST_UUID])
self.assertEqual(rset.get(DIFF_UUID), rset[DIFF_UUID])
self.assertEqual(rset, RequestSet([msg1, msg2], RQR_UUID))
self.assertEqual(rset, RequestSet([msg2, msg1], RQR_UUID))
self.assertNotEqual(rset, RequestSet([msg1], RQR_UUID))
self.assertNotEqual(rset, RequestSet([msg2], RQR_UUID))
self.assertNotEqual(rset, RequestSet([], RQR_UUID))
rs2 = copy.deepcopy(rset)
self.assertEqual(rset, rs2)
rs2[TEST_UUID]._transition(EVENT_GRANT)
self.assertNotEqual(rset, rs2)
