Beispiel #1
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\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')
Beispiel #2
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
Beispiel #3
0
    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)
Beispiel #4
0
    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...')
Beispiel #6
0
    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)
Beispiel #7
0
    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""")
Beispiel #9
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""")
Beispiel #10
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)
Beispiel #12
0
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
Beispiel #13
0
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
Beispiel #14
0
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
Beispiel #15
0
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
Beispiel #16
0
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
Beispiel #17
0
 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)
Beispiel #18
0
 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)
Beispiel #23
0
    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)
Beispiel #24
0
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
Beispiel #25
0
 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
Beispiel #29
0
 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
Beispiel #34
0
    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)
Beispiel #36
0
 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)
Beispiel #39
0
 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
Beispiel #40
0
    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)
Beispiel #41
0
    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)
Beispiel #42
0
 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)
Beispiel #43
0
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
Beispiel #46
0
    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)
Beispiel #47
0
    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)
Beispiel #52
0
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)