Exemplo n.º 1
0
    def send_tag_updates(self, tags):
        "Send location information given in the dictionary 'tags' (tag_id -> location)"

        # FIXME: Uses plain text interface. Should use proper LAT XML interface.

        result = {}

        # Add these tag updates to our history
        for tag_id, location in tags.iteritems():
            if not self.tags.has_key(tag_id):
                self.tags[tag_id] = []
            self.tags[tag_id].append(location)
            
        now = clock.get_time()
        if now - self.last_update_time > self.update_period:
            
            for tag_id, locations in self.tags.iteritems():
                x, y = locations[-1]
                result[tag_id] = x, y
                logging.info("Sending tag update: %d (%.2f, %.2f)" % (tag_id, x, y))
                if self.socket:
                    self.socket.send("%d %.2f %.2f\r\n" % (tag_id, x, y))
            
            # Reset for next time.
            self.tags = {}
            self.last_update_time = clock.get_time()
            
        return result
Exemplo n.º 2
0
    def loop(self):

        self.rdset = [self.socket]
        self.clients = []

        logging.info("Starting fake location server loop.")

        while True:

            # Deal with any incoming data first.
            (readers, writers, exceptors) = select.select(self.rdset + self.clients,[], [], 0.1)
            for reader in readers:
                if reader == self.socket:
                    # Accecpt a new connection from this socket.
                    conn, addr = self.socket.accept()
                    self.clients.append(conn)
                    logging.info("New client connection: %s" % (str(addr)))
                else:
                    # Service any data that has come from a client connection.
                    message = reader.recv(1024)
                    if message:
                        logging.info("Received message from client: %s" % message.strip())
                    else:
                        logging.info("Dropping client connection")
                        for i, client in enumerate(self.clients):
                            if client == reader:
                                del self.clients[i]

            # Then, see if it's time to push some data.
            if clock.get_time() > self.next_update_time:
                self.push_updates()
Exemplo n.º 3
0
    def push_updates(self):
        "Send distance readings to all the clients, update self.next_update_time."
        
        now = clock.get_time()
        updates = ""

        reading = self.readings[self.next_reading]
        while reading["timestamp"] < now:
            
            distance_reading = Reading(distance=reading["distance"], tag_id=reading["tag_id"], anchor_id=reading["anchor_id"], error_code=0)
            updates += "%s%s" % (str(distance_reading), LocationServer.line_separator)
            
            self.next_reading += 1

            if self.next_reading >= len(self.readings):
                if self.loop:
                    # Set the next reading to the first, and set the time three seconds before the first reading.
                    self.next_reading = 0
                    first_timestamp = self.readings[0]["timestamp"]
                    clock.set_time(first_timestamp - 3.0)
                    self.next_update_time = first_timestamp
                break
            
            reading = self.readings[self.next_reading]

        if updates:
            logging.info("Pushing updates of length %d" % len(updates))
            logging.debug("Updates:\n%s" % updates)
            for client in self.clients:
                client.send(updates)
Exemplo n.º 4
0
 def ground_truth(self, tag_id, timestamp=None):
     
     if timestamp == None:
         timestamp = clock.get_time()
     
     cursor = self.connection.cursor()
     
     cursor.execute(self.ground_truth_sql, (tag_id, timestamp, timestamp))
     rows = cursor.fetchall()
     
     if not rows:
         return None
     
     if len(rows) > 1:
         print rows
         raise Exception("Tag %d at %.2f matches more than one ground truth (%d)" % (tag_id, timestamp, len(rows)))
         
     start_x, start_y, end_x, end_y, start_time, end_time = rows[0]
     
     # Static point
     if end_x == None and end_y == None:
         return start_x, start_y
     
     # Linear interpolation between the start and end points.
     alpha = (timestamp - start_time) / (end_time - start_time)
     dx = alpha * (end_x - start_x)
     dy = alpha * (end_y - start_y)
     x = start_x + dx
     y = start_y + dy
     
     return x, y
Exemplo n.º 5
0
 def start_ground_truth(self, tag_id, label, location):
     
     x, y = location
     self.cursor.execute(self.start_ground_truth_sql, (label, tag_id, clock.get_time(), x, y))
     ground_truth_id = self.last_rowid()
     self.connection.commit()
     
     return ground_truth_id
Exemplo n.º 6
0
 def perturb(self):
     """Perturb each of the particles in the cloud according to the given perturbation function"""
     
     now = clock.get_time()
     
     period = now - self.last_perturb_time
     for particle in self.particles:
         particle.perturb(period)
Exemplo n.º 7
0
 def add_estimate(self, tag_id, x, y):
     "Add an estimate by the current location module."
     
     if self.configuration_id:
         sql = "INSERT INTO estimate(tag_id, x, y, timestamp, configuration_id) VALUES (?, ?, ?, ?, ?)"
         self.insert(sql, (tag_id, x, y, clock.get_time(), self.configuration_id))
     else:
         logging.warning("Ignored distance estimate because current configuration ID is not set.")
Exemplo n.º 8
0
 def __init__(self, anchor_id, tag_id, distance, timestamp=None):
     super(DistanceReading, self).__init__()
     self.anchor_id = anchor_id
     self.tag_id = tag_id
     self.distance = distance
     if timestamp == None:
         timestamp = clock.get_time()
     self.timestamp = timestamp
Exemplo n.º 9
0
    def median_distance(self, distance_readings, max_age=2.0):
        "Return the median distance of all the readings within max_age"

        now = clock.get_time()
        distances = [r.distance for r in distance_readings if now - r.timestamp < max_age]
        if distances:
            logging.debug("Returning median of %d distances" % len(distances))
            return numpy.median(distances)
        else:
            return None
Exemplo n.º 10
0
 def end_ground_truth(self, ground_truth_id, location=None, label=None):
     if location == None:
         x, y = None, None
     else:
         x, y = location
     self.cursor.execute(self.end_ground_truth_sql, (clock.get_time(), x, y, ground_truth_id))
     if label != None:
         self.cursor.execute(self.update_ground_truth_label_sql, (label, ground_truth_id))
     
     self.connection.commit()
Exemplo n.º 11
0
 def cull_old(self):
     "Delete any position updates older than 'max_age'"
     oldest = clock.get_time() - self.max_age
     
     for tag_id, position_updates in self.tag_updates.iteritems():
         i = 0
         while i < len(position_updates):
             if position_updates[i].timestamp < oldest:
                 del position_updates[i]
             else:
                 i += 1
Exemplo n.º 12
0
 def ground_truth_id(self, tag_id, timestamp=None):
     
     if timestamp == None:
         timestamp = clock.get_time()
     
     sql = "SELECT id FROM ground_truth WHERE tag_id = ? AND start_time <= ? AND end_time > ?"
     rows = self.query(sql, (tag_id, timestamp, timestamp)).fetchall()
     
     if not rows:
         return None
         
     if len(rows) > 1:
         raise Exception("Tag %d at %.2f matches more than one ground truth (%d)" % (tag_id, timestamp, len(rows)))
         
     return rows[0][0]
Exemplo n.º 13
0
    def tag_locations(self, tag_ids=[]):
        "A dictionary of tag locations: tag_id -> (x,y)"

        now = clock.get_time()
        result = {}
                        
        self.cull_old()
                        
        for tag_id in tag_ids:
            if self.update_rate and (now - self.last_updates[tag_id] < self.update_rate):
                continue
            position_updates = self.tag_updates.get(tag_id)
            if position_updates:
                result[tag_id] = self.filter_function(position_updates)
                self.last_updates[tag_id] = now
            
        return result
Exemplo n.º 14
0
def run_locmod(experiment, locmod, config):
    "Run the locmod for a particular configuration against the distance readings to generate a new set of estimates."

    last_anchor_id = 0

    logging.info("Running location module: %s" % (config.locmod_filename))

    experiment.register_configuration(config.filename, config.text, config.locmod_filename, config.locmod_text)


    sql = "SELECT anchor_id, tag_id, distance, timestamp FROM distance_reading ORDER BY timestamp"
    for anchor_id, tag_id, distance, timestamp in experiment.query(sql):

        # Skip if the reading is not relavent to this configuration
        if not anchor_id in config.anchors:
            logging.debug("Ignoring reading from unknown anchor: %d" % anchor_id)
            continue
        if not tag_id in config.tag_ids:
            logging.debug("Ignoring reading from unknown tag: %d" % tag_id)
            continue

        # Anchor IDs come in order, so if we roll back to an earlier one it means we have all the reading for this update, and it's time to do the location update.
        if anchor_id < last_anchor_id:
            update = locmod.update_locations([tag_id])
            if update.has_key(tag_id):
                location = update[tag_id]
                x, y = location
                ground_truth = experiment.ground_truth(tag_id)
                if ground_truth:
                    gx, gy = ground_truth
                    error = math.hypot(x - gx, y - gy)
                    ground_truth_id = experiment.ground_truth_id(tag_id)
                    sql = "INSERT INTO estimate(tag_id, x, y, timestamp, ground_truth_id, error, configuration_id) VALUES (?, ?, ?, ?, ?, ?, ?)"
                    experiment.cursor.execute(sql, (tag_id, x, y, clock.get_time(), ground_truth_id, error, experiment.configuration_id))
                    logging.debug("Inserted estimate: (%06.2f, %06.2f) error %05.2fm from (%06.2f, %06.2f)" % (x, y, error, gx, gy))
                else:
                    logging.debug("Not adding estimate, location not known.")

        clock.set_time(timestamp)
        locmod.add_reading(anchor_id, tag_id, distance)

        last_anchor_id = anchor_id

    experiment.connection.commit()
Exemplo n.º 15
0
 def add_readings(self, tag_id, distances, ground_truth_id=None):
     "Like add reading for many readings, but all with an identical timestamp."
     timestamp = clock.get_time()
     for anchor_id, distance in distances:
         self.cursor.execute(self.add_reading_sql, (anchor_id, tag_id, distance, ground_truth, timestamp))
Exemplo n.º 16
0
 def add_reading(self, anchor_id, tag_id, distance, ground_truth=None):
     "Add a reading, now. Don't worry about comparing to the ground truth distance."
     
     self.cursor.execute(self.add_reading_sql, (anchor_id, tag_id, distance, ground_truth, clock.get_time()))
     self.connection.commit()
Exemplo n.º 17
0
from Almada.clock import shared_clock as clock
import clock_test_b
import time

if __name__ == "__main__":
    b = clock_test_b.B()
    
    for i in range(10):
        start_time = clock.get_time()
        b.advance_time(i)
        time.sleep(1)
        wait_time = clock.get_time() - start_time
        print "Slept for %.2f seconds" % (wait_time)
        
Exemplo n.º 18
0
 def __init__(self, x, y):
     super(PositionUpdate, self).__init__()
     self.x = x
     self.y = y
     self.timestamp = clock.get_time()
Exemplo n.º 19
0
 def advance_time(self, interval):
     now = clock.get_time()
     should_be = now + interval
     clock.set_time(should_be)
     print "Error: %.2f" % (clock.get_time() - should_be)