def main():
    
    try:
        sWeather = fetchJSON("http://api.openweathermap.org/data/2.5/weather?q=" + config['location'] + "&units=metric&APPID=" + config['api'])
        jWeather = json.loads(sWeather)

        data = processData(jWeather)

        saveSQLite(data)
    except urllib2.HTTPError:
        print "Failed to connect to OpenWeathcerMap"
    
    if (sensor_config['attiny_light_meter'] == True):
        try:
            bus = smbus.SMBus(I2C_BUS_NUMBER)
            light_level = bus.read_byte(LIGHT_SENSOR_ADDR) * 256 + bus.read_byte(LIGHT_SENSOR_ADDR);
        
            sensor_handler = sensor.sensor()
            sensor_handler.save_value(7, light_level)
        except IOError as e:
            print "Failed to fetch light level"
    
    elif (sensor_config['BH1750FVI_light_meter'] == True):
        try:
            light_level = readLight();
        
            sensor_handler = sensor.sensor()
            sensor_handler.save_value(8, light_level)
        except IOError as e:
            print "Failed to fetch light level"
예제 #2
0
def main():

    sensor_handler = sensor.sensor()

    global db_connection

    db_connection = sensor_handler.get_db_connection()

    c = db_connection.cursor()
    c.execute('DROP TABLE IF EXISTS sensor_values')
    c.execute('CREATE TABLE IF NOT EXISTS sensor_values(`Date` integer, `Sensor` integer, `Value` real)')

    source_connection = sqlite3.connect(os.path.dirname(os.path.realpath(__file__)) + '/data.db')

    cursor_source = source_connection.cursor()
    result = cursor_source.execute('SELECT * FROM readouts_external')

    for row in result:
        sensor_handler.insert_no_commit(row[0], 0, row[1]) # Temperature
        sensor_handler.insert_no_commit(row[0], 1, row[2]) # Humidity
        # print row

    result = cursor_source.execute('SELECT * FROM external_data')

    for row in result:
        sensor_handler.insert_no_commit(row[0], 2, row[1])  # Real Pressure
        sensor_handler.insert_no_commit(row[0], 4, row[2])  # Wind Speed
        sensor_handler.insert_no_commit(row[0], 5, row[3])  # Wind direction
        # print row

    c.execute('CREATE INDEX IF NOT EXISTS SENSOR_A ON sensor_values(`Date`, `Sensor`)')

    db_connection.commit()
예제 #3
0
 def __init__(self):
     """ This will read the config files and set up the different
     listeners and what not
     """
     self.config = read_config()
     self.buildMap()
     self.runMDP()
     self.sensor = sensor(self.config)
     self.moveDict = self.sensor.moveSensor.moveDict
     self.probDict = dict()
     self.probDict[0] = self.config["prob_move_forward"]
     self.probDict[1] = self.config["prob_move_backward"]
     self.probDict[2] = self.config["prob_move_left"]
     self.probDict[3] = self.config["prob_move_right"]
     self.count = 0
     row, col = self.config["map_size"]
     for x in xrange(row):
         for y in xrange(col):
             if not (self.grid[x][y] == "W" or self.grid[x][y] == "P"):
                 self.count += 1
     avg = 1.0 / float(self.count)
     self.probGrid = [[
         avg if not (cell == "W" or cell == "P") else 0 for cell in line
     ] for line in self.grid]
     util.print_2d_floats(self.probGrid)
     self.getToGoal()
예제 #4
0
def main(argv):
    #If True, data will print to shell as well as write to file
    #If False, data will only write to file
    verb = ''
    if len(sys.argv) == 2:
        if sys.argv[1] == '-v' or sys.argv[1] == '-verb' or sys.argv[
                1] == '-verbose':
            verb = True
        else:
            print("Invalid argument. Valid argument(s): -v[erbose]")
    else:
        verb = False

    #Initialize file path for readings
    fpath = "/home/pi/MSD-P21422-BSF/Readings/"

    #Initialize data variables
    in_temp_f, in_temp_c, out_temp_f, out_temp_c, in_hum, out_hum, co2 = 0, 0, 0, 0, 0, 0, 0
    today, now = '0', '0'

    dbx = 0
    db_connect = False

    #Check if connected to internet
    if check_connection():
        #If connected to internet, establish dropbox connection
        dbx, db_connect = db_access()

    while True:
        today, now = cur_date_time(today, now, verb)
        file_name = date.strftime(date.today(), '%Y%m%d.csv')
        full_path = fpath + file_name
        in_temp_f, in_temp_c, out_temp_f, out_temp_c, in_hum, out_hum, co2 = \
                    sensor.sensor(in_temp_f, in_temp_c, out_temp_f, out_temp_c, in_hum, out_hum, co2, verb)
        heat_stat, hum_stat, fan_stat, light_stat = relay.relay(
            in_temp_f, in_hum, co2, verb)
        write_to_csv(in_temp_f, in_temp_c, out_temp_f, out_temp_c, in_hum,
                     out_hum, co2, today, now, heat_stat, hum_stat, fan_stat,
                     light_stat, full_path)

        # Check the internet connection
        if check_connection():
            #If connected to internet but not dropbox
            if db_connect == False:
                #Establish dropbox connection
                dbx, db_connect = db_access()

            #If connected to internet and to dropbox
            if db_connect == True:
                #Upload file to dropbox
                print("Uploading the file...")
                upload('/' + file_name, full_path, dbx)
                print("Upload successful")
        #TO-DO:
        #If connection is down for more than a day, add a case to upload files that were missed

        #sleep in seconds. 60 = 1 minute, 300 = 5 minutes, 1800 = 30 minutes
        time.sleep(599.0)
    return
예제 #5
0
 def start(self):
     if self.interval != 0:
         logger.debug('Sensor Thread init - ' + self.name)
         self.obj = sensor.sensor(self.no, self.name, self.interval)
         #self.obj.setName(self.name)
         logger.debug('Starting Sensor Thread - ' + self.name)
         self.obj.start()
         self.type_setup()
예제 #6
0
 def start(self):
     if self.interval != 0:
         logger.debug('Sensor Thread init - ' + self.name)
         self.obj = sensor.sensor(self.no, self.name, self.interval)
         #self.obj.setName(self.name)
         logger.debug('Starting Sensor Thread - ' + self.name)
         self.obj.start()
         self.type_setup()
예제 #7
0
 def __init__(self):
     threading.Thread.__init__(self)
     thread = threading.Thread(target=self.run)
     thread.daemon = True
     for i in range(len(self.sensors_list)):
         self.sensors.append(
             sensor(self.sensors_list[i], self.sensors_pins[i],
                    self.sensors_thresholds[i]))
     thread.start()
예제 #8
0
파일: window.py 프로젝트: mastergreg/os-lab
def main():
    global threads
    nsensors = 2
    for i in xrange(nsensors):
        threads.append(sensor(i,'batt'))
        threads.append(sensor(i,'light'))
        threads.append(sensor(i,'temp'))
    map( lambda x : x.start(),threads )

    myscreen = curses.initscr()
    myscreen.border(0)

    while True:
        myscreen.refresh()   
        for i,j in enumerate(threads):
            myscreen.addstr(i+2, 2, str(j))
        sleep(1)

    curses.endwin()
예제 #9
0
def main():

    sensor_handler = sensor.sensor()

    FORMAT = '%(asctime)-15s %(message)s'
    logging.basicConfig(filename=os.path.dirname(os.path.realpath(__file__)) +
                        '/dht22.log',
                        level=logging.DEBUG,
                        format=FORMAT)
    logger = logging.getLogger('dht22')

    print "DHT22 Sensor:"

    readout = None

    counter = 0

    try:
        pi = pigpio.pi()
    except ValueError:
        print "Failed to connect to PIGPIO (%s)"
        logger.error('Failed to connect to PIGPIO (%s)', ValueError)

    try:
        sensor_data = DHT22.sensor(pi, 17)
    except ValueError:
        print "Failed to connect to DHT22"
        logger.error('Failed to connect to DHT22 (%s)', ValueError)

    while (readout == None and counter < 5):

        counter += 1

        # Get data from sensor
        sensor_data.trigger()
        time.sleep(0.2)

        humidity = sensor_data.humidity()
        temperature = sensor_data.temperature()

        if humidity != None and temperature != None and humidity >= 0 and humidity <= 100:

            readout = [humidity, temperature]

            saveSQLite(readout)

            sensor_handler.save_value(0, temperature)
            sensor_handler.save_value(1, humidity)

            print "Humidity: " + str(humidity)
            print "Temperature: " + str(temperature)
            counter = 10
        else:
            time.sleep(5)
def STA_generation(amount, radius, RTS_enable, CWmin, CWmax, system_AP):
    STA_list = []
    import math, random
    for i in range(1, amount + 1):
        alpha = random.random() * 2 * math.pi
        r = math.sqrt(random.random()) * radius
        x = r * math.cos(alpha)
        y = r * math.sin(alpha)
        STA_list.append(
            sensor.sensor(i, CWmin, CWmax, [x, y], RTS_enable, False,
                          system_AP))
    return STA_list
예제 #11
0
def main():

    s = sensor.sensor()
    dist = s.get_distance()

    if dist != None:
        level = tank_height - dist
        print "level=" + str(level)

        logger.log_level(level)
    else:
        print "Error in monitor. Failed to get level"
예제 #12
0
def main():

    sensor_handler = sensor.sensor();

    FORMAT = '%(asctime)-15s %(message)s'
    logging.basicConfig(filename=os.path.dirname(os.path.realpath(__file__)) + '/dht22.log', level=logging.DEBUG,
                        format=FORMAT)
    logger = logging.getLogger('dht22')

    print "DHT22 Sensor:"

    readout = None

    counter = 0

    try:
        pi = pigpio.pi()
    except ValueError:
        print "Failed to connect to PIGPIO (%s)"
        logger.error('Failed to connect to PIGPIO (%s)', ValueError);

    try:
        sensor_data = DHT22.sensor(pi, 17)
    except ValueError:
        print "Failed to connect to DHT22"
        logger.error('Failed to connect to DHT22 (%s)', ValueError);

    while (readout == None and counter < 5):

        counter += 1

        # Get data from sensor
        sensor_data.trigger()
        time.sleep(0.2)

        humidity = sensor_data.humidity()
        temperature = sensor_data.temperature()

        if humidity != None and temperature != None and humidity >= 0 and humidity <= 100:

            readout = [humidity, temperature]

            saveSQLite(readout)

            sensor_handler.save_value(0, temperature)
            sensor_handler.save_value(1, humidity)

            print "Humidity: " + str(humidity)
            print "Temperature: " + str(temperature)
            counter = 10
        else:
            time.sleep(5)
예제 #13
0
def main():

    sensor_handler = sensor.sensor();

    temperature = sensor_handler.get_last_value(0)
    humidity = sensor_handler.get_last_value(1)
    pressure = sensor_handler.get_last_value(2)

    callScript = "curl -d 'pressure="+str(pressure)+"&humidity="+str(humidity)+"&temp="+str(temperature)+"&"+config['coords']+"' --user '"+config['user']+":"+config['password']+"' http://openweathermap.org/data/post"

    # print callScript

    p = subprocess.Popen(callScript, shell=True, stdout=subprocess.PIPE, stderr=subprocess.STDOUT)	
예제 #14
0
    def __init__( self ):
        Thread.__init__( self );

        self.send_dt = 0.1;
        self.is_to_break = False;
        self.s = sensor(  );
        self.sock = bt.BluetoothSocket( bt.RFCOMM );
        
        with open( 'bt_address.txt', 'r' ) as f:
            for line in f:
                self.hostaddr = line.rstrip();
            #
        #
        return;
예제 #15
0
    def __init__(self):
        Thread.__init__(self)

        self.send_dt = 0.1
        self.is_to_break = False
        self.s = sensor()
        self.sock = bt.BluetoothSocket(bt.RFCOMM)

        with open('bt_address.txt', 'r') as f:
            for line in f:
                self.hostaddr = line.rstrip()
            #
        #
        return
예제 #16
0
 def long_press( self ):
     if button.is_blocked:
         return;
     else:
         button.is_blocked = True;
     #
     try:
         button.indicator.set_ratio_period( 1. );
         s = sensor(  );
         s.calib(  );
     except:
         pass;
     #
     button.indicator.set_ratio_period( 0.0 );
     button.is_blocked = False;
예제 #17
0
def main():

    sensor_handler = sensor.sensor();

    temperature = sensor_handler.get_last_value(0)
    humidity = sensor_handler.get_last_value(1)
    pressure = sensor_handler.get_last_value(2)
     
    encodedAttributes = {
        'key': config['api'],
        'field1': temperature,
        'field2': humidity,
        'field3': pressure
    }
    req = urllib2.Request(config['url'] + "?" + urllib.urlencode(encodedAttributes))
    response=urllib2.urlopen(req)
    print "Posted!"
def processData(json):
    out = {}

    sensor_handler = sensor.sensor()

    bmp_180_sensor = BMP085.BMP085()

    out['pressure'] = round(bmp_180_sensor.read_sealevel_pressure(35) / 100, 1)
    out['wind-direction'] = json["wind"]["deg"]
    out['wind-speed'] = json["wind"]["speed"]

    # FIXME this is a hack, whole process should be rewritten
    sensor_handler.save_value(2, round(bmp_180_sensor.read_sealevel_pressure(35) / 100, 1))
    sensor_handler.save_value(3, json["main"]["pressure"])
    sensor_handler.save_value(4, json["wind"]["speed"])
    sensor_handler.save_value(5, json["wind"]["deg"])
    sensor_handler.save_value(6, bmp_180_sensor.read_temperature()) # Save internal temperature readout
    return out
예제 #19
0
def main():

    sensor_handler = sensor.sensor()

    temperature = sensor_handler.get_last_value(0)
    humidity = sensor_handler.get_last_value(1)
    pressure = sensor_handler.get_last_value(2)

    callScript = "curl -d 'pressure=" + str(pressure) + "&humidity=" + str(
        humidity) + "&temp=" + str(temperature) + "&" + config[
            'coords'] + "' --user '" + config['user'] + ":" + config[
                'password'] + "' http://openweathermap.org/data/post"

    # print callScript

    p = subprocess.Popen(callScript,
                         shell=True,
                         stdout=subprocess.PIPE,
                         stderr=subprocess.STDOUT)
예제 #20
0
    def __init__(self, room_id, numRows, *attrs):
        self.room_mod = None
        self.sensors = {}

        if attrs:
            attrs = attrs[0]
            for i, sensorID in enumerate(attrs['info']):
                sensor_attributes = {
                    'id': sensorID,
                    'info': attrs['info'][sensorID]
                }
                self.sensors[i] = sensor('thermostat', numRows,
                                         sensor_attributes)
                self.id = attrs['id']
        else:
            self.id = np.int64(str(uuid.uuid4().int)[:7])

            num_thermostat = rd.randint(1, 2)
            num_co2 = rd.randint(1, 3)

            self.add_sensors(num_thermostat, num_co2, numRows)
예제 #21
0
파일: comm.py 프로젝트: Sijia233/check
    def main(self, n, node_id):

        self.node_id = node_id

        # Initialize and start the listener thread
        listener_thread = listener.listener()
        listener_thread.set_node_id(node_id)
        listener_thread.set_node_count(n)
        listener_thread.start()

        # Initialize and start the protocol handler thread
        aodv_thread = aodv.aodv()
        aodv_thread.set_node_id(node_id)
        aodv_thread.set_node_count(n)
        aodv_thread.start()

        # Initialize and start the vehicle simulation thread
        sensor_thread = sensor.sensor()
        sensor_thread.set_node_id(node_id)
        sensor_thread.daemon = True
        sensor_thread.start()
예제 #22
0
def main():
    api = tweetpony.API(consumer_key=config['consumer_key'],
                        consumer_secret=config['consumer_secret'],
                        access_token=config['access_token'],
                        access_token_secret=config['access_token_secret'])
    user = api.user

    sensor_handler = sensor.sensor()

    temperature = sensor_handler.get_last_value(0)
    humidity = sensor_handler.get_last_value(1)
    pressure = sensor_handler.get_last_value(2)

    try:
        api.update_status(status=u'Witaj Dobra, mamy właśnie ' +
                          unicode(str(temperature)) + u'C i ' +
                          unicode(str(humidity)) + u'% wilgotności')
    except tweetpony.APIError as err:
        print "Oops, something went wrong! Twitter returned error #%i and said: %s" % (
            err.code, err.description)
    else:
        print "Yay! Your tweet has been sent!"
예제 #23
0
def main():

    sensor_handler = sensor.sensor()

    global db_connection

    db_connection = sensor_handler.get_db_connection()

    c = db_connection.cursor()
    c.execute('DROP TABLE IF EXISTS sensor_values')
    c.execute(
        'CREATE TABLE IF NOT EXISTS sensor_values(`Date` integer, `Sensor` integer, `Value` real)'
    )

    source_connection = sqlite3.connect(
        os.path.dirname(os.path.realpath(__file__)) + '/data.db')

    cursor_source = source_connection.cursor()
    result = cursor_source.execute('SELECT * FROM readouts_external')

    for row in result:
        sensor_handler.insert_no_commit(row[0], 0, row[1])  # Temperature
        sensor_handler.insert_no_commit(row[0], 1, row[2])  # Humidity
        # print row

    result = cursor_source.execute('SELECT * FROM external_data')

    for row in result:
        sensor_handler.insert_no_commit(row[0], 2, row[1])  # Real Pressure
        sensor_handler.insert_no_commit(row[0], 4, row[2])  # Wind Speed
        sensor_handler.insert_no_commit(row[0], 5, row[3])  # Wind direction
        # print row

    c.execute(
        'CREATE INDEX IF NOT EXISTS SENSOR_A ON sensor_values(`Date`, `Sensor`)'
    )

    db_connection.commit()
예제 #24
0
    def __init__(self,
                 name,
                 pin0=18,
                 pin1=23,
                 pin2=24,
                 pin3=25,
                 simulation=True):

        #GPIO: 18 23 24 25
        #pin : 12 16 18 22

        self.logger = logging.getLogger('myQ.quadcptr')
        self.name = name
        self.simulation = simulation

        self.version = 1

        self.motor = [motor('M' + str(i), 0) for i in xrange(4)]
        self.motor[0] = motor('M0',
                              pin0,
                              kv=1000,
                              WMin=0,
                              WMax=100,
                              simulation=self.simulation)
        self.motor[1] = motor('M1',
                              pin1,
                              kv=1000,
                              WMin=0,
                              WMax=100,
                              simulation=self.simulation)
        self.motor[2] = motor('M2',
                              pin2,
                              kv=1000,
                              WMin=0,
                              WMax=100,
                              simulation=self.simulation)
        self.motor[3] = motor('M3',
                              pin3,
                              kv=1000,
                              WMin=0,
                              WMax=100,
                              simulation=self.simulation)

        self.sensor = sensor(simulation=self.simulation)

        self.pidR = pid()
        self.pidP = pid()
        self.pidY = pid()
        self.pidR_rate = pid()
        self.pidP_rate = pid()
        self.pidY_rate = pid()

        self.ip = '192.168.1.1'

        self.netscan = netscan(self.ip)

        self.webserver = webserver(self)

        self.display = display(self)

        self.rc = rc(self.display.screen)

        self.imulog = False
        self.savelog = False
        self.calibIMU = False
        self.debuglev = 0
        self.netscanning = False

        #for quadricopter phisics calculations- not used yet
        self.prop = prop(9, 4.7, 1)
        self.voltage = 12  # [V]
        self.mass = 2  # [Kg]
        self.barLenght = 0.23  # [mm]
        self.barMass = 0.15  # [kg]

        self.datalog = ''
        self.initLog()
def run(args):
#if __name__=="__main__":
    # initialize parameters of interest
    # Method:
    # 0: linear policy
    # 1: RBF policy
    # 2: MLP policy

    #method = args[0]
    #RBF_components = args[1]
    #MLP_neurons = args[2]
    process_index = args[3]
    folder_name = args[4]
    np.random.seed(process_index+100)
    #process_index = 0
    #np.random.seed(process_index + 100)
    #vel_var = args[5]
    #num_targets = args[6]

    method = 0
    RBF_components = 20
    MLP_neurons = 50
    vel_var = .001
    num_targets = min(6,max(2,np.random.poisson(3)))
    num_targets = np.random.randint(2,10)
    #num_targets = 4


    print("Starting Thread:" + str(process_index))

    #Initialize all the parameters
    params ={0:{},1:{},2:{}}
    if method==0:
        params[0]["weight2"] = np.random.normal(0, .3, [2, num_states_layer2])
        #params[0]["weight2"] = np.array([[  3.97573312,   0.4639474 ,   2.27280486,  12.9085868 ,
         #   3.45722461,   6.36735166],
         #[-11.87940874,   2.59549414,  -5.68556954,   2.87746786,
          #  7.08059984,   5.5631133 ]])

        params[0]["weight"] = np.array([[7.18777985, -13.68815256, 1.69010242, -5.62483187,
                           -4.30451483, 10.09592853],
                         [13.33104057, 13.60537864, 3.46939294, 0.8446329,
                         -14.79733566, -4.78599648]])

        #params[0]["weight"] = np.array([[ 1.45702249, -1.17664153, -0.11593174,  1.02967173, -0.25321044,
         #0.09052774],
       #[ 0.67730786,  0.3213561 ,  0.99580938, -2.39007038, -1.16340594,
        #-1.77515938]])
    elif method==1:
        featurizer = sklearn.pipeline.FeatureUnion([("rbf1", RBFSampler(gamma=rbf_var, n_components=RBF_components, random_state=1))])
        featurizer.fit(np.array(list_of_states))  # Use this featurizer for normalization
        params[1]["weight"] = np.random.normal(0, 1, [2, RBF_components])
    elif method==2:
        params[2]["weigh1"] = np.random.normal(0, 1, [MLP_neurons, num_states])
        params[2]["bias1"] = np.random.normal(0,1,[MLP_neurons,1])
        params[2]["weigh2"] = np.random.normal(0, 1, [2, MLP_neurons])
        params[2]["bias2"] = np.random.normal(0, 1, [2, 1])

    return_saver = []
    error_saver = []
    episode_counter = 0
    weight_saver1 = []
    weight_saver2 = []
    weight_saver2_1 = []
    weight_saver2_2 = []
    #for episode_counter in range(0,N_max):
    #Training parameters
    avg_reward = []
    avg_error = []
    var_reward = []
    training = True


    result_folder = base_path+folder_name+"/"
    reward_file = open(result_folder+"reward_noise:"+str(vel_var)+"_"+str(process_index)+  "_linear_6states.txt","a")
    error_file = open(result_folder + "error_noise:" + str(vel_var) +"_"+str(process_index)+ "_linear_6states.txt", "a")
    error_file_median = open(result_folder + "error_median_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt",
                      "a")
    var_file = open(result_folder + "var_noise:" + str(vel_var) +"_"+str(process_index)+ "_linear_6states.txt", "a")
    var_error_file = open(result_folder + "var_error_noise:" + str(vel_var) +"_"+str(process_index)+ "_linear_6states.txt", "a")
    weight_file = open(result_folder + "weight_noise:" + str(vel_var) +"_"+str(process_index)+ "_linear_6states.txt", "a")

    #flatten initial weight and store the values
    if method==0:
        weight = params[0]['weight']
        flatted_weights = list(weight[0, :]) + list(weight[1, :])
        temp = []
        [temp.append(str(x)) for x in flatted_weights]
        weight_file.write("\t".join(temp)+"\n")
    elif method==1:
        weight = params[1]['weight']
        flatted_weights = list(weight[0, :]) + list(weight[1, :])
        temp = []
        [temp.append(str(x)) for x in flatted_weights]
        weight_file.write("\t".join(temp) + "\n")
    elif method==2:
        pass

    #weight = np.reshape(np.array(weights[0]), [2, 6])
    init_max_target = 3
    num_targets = init_max_target
    while episode_counter<N_max:
        if episode_counter%1000==0 and episode_counter>0:
            init_max_target +=1
            init_max_target = min(20,init_max_target)
        
        if episode_counter%100==0 and episode_counter>0:
            num_targets = np.random.randint(3,init_max_target+1)
        sigma = gen_learning_rate(episode_counter,sigma_max,.1,5000)
        sigma = sigma_max
        discounted_return = np.array([])
        discount_vector = np.array([])
        #print(episodes_counter)
        scen = scenario(1,1)
        bearing_var = 1E-2#variance of bearing measurement
        #Target information
        x = 10000*np.random.random([num_targets])-5000#initial x-location
        y = 10000 * np.random.random([num_targets]) - 5000#initial y-location
        xdot = 10*np.random.random([num_targets])-5#initial xdot-value
        ydot = 10 * np.random.random([num_targets]) - 5#initial ydot-value

        #TEMP
        #x = [2000,-2000]
        #y = [2000,2000]
        #xdot = [1,1]
        #ydot = [-1,-1]

        init_target_state = []
        init_for_smc = []
        for target_counter in range(0,num_targets):
            init_target_state.append([x[target_counter],y[target_counter],xdot[target_counter],ydot[target_counter]])#initialize target state
            init_for_smc.append([x[target_counter]+np.random.normal(0,5),y[target_counter]
                                 +np.random.normal(0,5),np.random.normal(0,5),np.random.normal(0,5)])#init state for the tracker (tracker doesn't know about the initial state)


        #temp_loc = np.array(init_target_state[0:2]).reshape(2,1)
        #init_location_estimate = temp_loc+0*np.random.normal(np.zeros([2,1]),10)
        #init_location_estimate = [init_location_estimate[0][0],init_location_estimate[1][0]]
        #init_velocity_estimate = [6*random.random()-3,6*random.random()-3]
        #init_velocity_estimate = [init_target_state[2],init_target_state[3]]
        #init_estimate = init_location_estimate+init_velocity_estimate
        init_covariance = np.diag([MAX_UNCERTAINTY,MAX_UNCERTAINTY,MAX_UNCERTAINTY,MAX_UNCERTAINTY])#initial covariance of state estimation
        t = []
        for i in range(0,num_targets):
            t.append(target(init_target_state[i][0:2], init_target_state[i][2],
                            init_target_state[i][3], vel_var, vel_var, "CONS_V"))#constant-velocity model for target motion
        A, B = t[0].constant_velocity(1E-10)#Get motion model
        x_var = t[0].x_var
        y_var = t[0].y_var

        tracker_object = []
        for i in range(0,num_targets):
            tracker_object.append(EKF_tracker(init_for_smc[i], np.array(init_covariance), A,B,x_var,y_var,bearing_var))#create tracker object
            #smc_object = smc_tracker(A,B,x_var,y_var,bearing_var,1000,np.array(init_for_smc))

        #Initialize sensor object
        if method==0:
            s = sensor("POLICY_COMM_LINEAR")#create sensor object (stochastic policy)
        elif method==1:
            s = sensor("POLICY_COMM_RBF")
        elif method==2:
            s = sensor("POLICY_COMM_MLP")
        measure = measurement(bearing_var)#create measurement object

        m = []
        x_est = []; y_est = []; x_vel_est = []; y_vel_est = []
        x_truth = [];
        y_truth = [];
        x_vel_truth = [];
        y_vel_truth = []
        uncertainty = []
        vel_error = []
        pos_error = []
        iteration = []
        innovation = []
        for i in range(0,num_targets):
            x_truth.append([])
            y_truth.append([])
            x_vel_truth.append([])
            y_vel_truth.append([])
            uncertainty.append([])
            vel_error.append([])
            x_est.append([])
            y_est.append([])
            x_vel_est.append([])
            y_vel_est.append([])
            pos_error.append([])
            innovation.append([])
        reward = []
        episode_condition = True
        n=0
        violation = 0
        #store required information
        episode_state = []
        episode_state_out_layer = []
        episode_MLP_state = []
        episode_actions = []
        avg_uncertainty= []
        max_uncertainty = []

        while episode_condition:
            temp_m = []
            input_state_temp = []
            for i in range(0,num_targets):
                t[i].update_location()
                temp_m.append(measure.generate_bearing(t[i].current_location,s.current_location))

            m.append(temp_m)
            temp_reward = []
            target_actions = []
            for i in range(0,num_targets):
                tracker_object[i].update_states(s.current_location, m[-1][i])
                normalized_innovation = (tracker_object[i].innovation_list[-1])/tracker_object[i].innovation_var[-1]
                #print(normalized_innovation)
                #if (normalized_innovation<1E-4 or n<10) and n<200:
                    #end of episode
                current_state = list(tracker_object[i].x_k_k.reshape(len(tracker_object[i].x_k_k))) + list(s.current_location)

                #print(current_state)
                #state normalization
                x_slope = 2.0/(scen.x_max-scen.x_min)
                y_slope = 2.0 / (scen.y_max - scen.y_min)

                x_slope_sensor = 2.0 / (40000)
                y_slope_sensor = 2.0 / (40000)

                vel_slope = 2.0/(scen.vel_max-scen.vel_min)
                #normalization
                current_state[0] = -1+x_slope*(current_state[0]-scen.x_min)
                current_state[1] = -1 + y_slope * (current_state[1] - scen.y_min)
                current_state[2] = -1 + vel_slope * (current_state[2] - scen.vel_min)
                current_state[3] = -1 + vel_slope * (current_state[3] - scen.vel_min)
                current_state[4] = -1 + x_slope * (current_state[4] -scen.x_min)
                current_state[5] = -1 + y_slope * (current_state[5] - scen.y_min)


                #Refactor states based on the usage
                if method==0 or method==2:
                    input_state = current_state
                    input_state_temp.append(input_state) #store input-sates
                elif method==1:
                    #Generate states for the RBF input
                    input_state =  featurizer.transform(np.array(current_state).reshape(1,len(current_state)))
                    input_state = list(input_state[0])


                target_actions.append(s.generate_action(params,input_state,.01))
                estimate = tracker_object[i].x_k_k
                episode_state.append(input_state) ####Neeed to get modified
                if method==2: episode_MLP_state.append(extra_information) #need to get modified
                truth = t[i].current_location
                x_est[i].append(estimate[0])
                y_est[i].append(estimate[1])
                x_vel_est[i].append(estimate[2])
                y_vel_est[i].append(estimate[3])
                x_truth[i].append(truth[0])
                y_truth[i].append(truth[1])
                x_vel_truth[i].append(t[i].current_velocity[0])
                y_vel_truth[i].append(t[i].current_velocity[1])
                vel_error[i].append(np.linalg.norm(estimate[2:4]-np.array([t[i].current_velocity[0],t[i].current_velocity[1]]).reshape(2,1)))
                pos_error[i].append(np.linalg.norm(estimate[0:2]-np.array(truth).reshape(2,1)))
                innovation[i].append(normalized_innovation[0])
                unormalized_uncertainty = np.sum(tracker_object[i].p_k_k.diagonal())
                #if unormalized_uncertainty>MAX_UNCERTAINTY:
                #   normalized_uncertainty = 1
                #else:
                #   normalized_uncertainty = (1.0/MAX_UNCERTAINTY)*unormalized_uncertainty
                uncertainty[i].append((1.0 / MAX_UNCERTAINTY) * unormalized_uncertainty)
                #if len(uncertainty[i])<window_size+window_lag:
                 #   temp_reward.append(0)
                #else:
                 #   current_avg = np.mean(uncertainty[i][-window_size:])
                  #  prev_avg = np.mean(uncertainty[i][-(window_size+window_lag):-window_lag])
                   # if current_avg<prev_avg or uncertainty[i][-1]<.1:
                    #if current_avg < prev_avg:
                    #    temp_reward.append(1)
                    #else:
                     #   temp_reward.append(0)

            this_uncertainty = []
            [this_uncertainty.append(uncertainty[x][-1]) for x in range(0, num_targets)]
            avg_uncertainty.append(np.mean(this_uncertainty))
            max_uncertainty.append(np.max(this_uncertainty))
            if len(avg_uncertainty) < window_size + window_lag:
                reward.append(0)
            else:
                current_avg = np.mean(avg_uncertainty[-window_size:])
                prev_avg = np.mean(avg_uncertainty[-(window_size + window_lag):-window_lag])
                if current_avg < prev_avg or avg_uncertainty[-1] < .1:
                    # if current_avg < prev_avg:
                    reward.append(1)
                else:
                    reward.append(0)

            #voting
            #if np.mean(temp_reward)>.5:
             #   reward.append(np.mean(temp_reward))
            #else:
             #   reward.append(np.mean(temp_reward))

            #if sum(reward)>1100 and num_targets>2: sys.exit(1)

            #Do something on target_actions
            #Create feature-vector from generated target actions

            normalized_state,index_matrix1,index_matrix2,slope = s.update_location_decentralized(target_actions,sigma,params) #Update the sensor location based on all individual actions
            #index_matrix: an n_s \times T matrix that shows the derivative of state in the output layer to the action space in the internal-layer

            backpropagated_to_internal_1 = index_matrix1.dot(np.array(input_state_temp))#8 by 6
            backpropagated_to_internal_2 = index_matrix2.dot(np.array(input_state_temp))# 8 by 6

            episode_state_out_layer.append(normalized_state)
            episode_state.append([backpropagated_to_internal_1,backpropagated_to_internal_2]) #each entry would be a T \times 6 matrix with T being the number of targets
            #reward.append(-1*uncertainty[-1])
            #update return
            discount_vector = gamma*np.array(discount_vector)
            discounted_return+= (1.0*reward[-1])*discount_vector
            new_return = 1.0*reward[-1]
            list_discounted_return = list(discounted_return)
            list_discounted_return.append(new_return)
            discounted_return = np.array(list_discounted_return)

            list_discount_vector = list(discount_vector)
            list_discount_vector.append(1)
            discount_vector = np.array(list_discount_vector)
            iteration.append(n)
            if n>episode_length: break
            n+=1

        #Based on the return from the episode, update parameters of the policy model
        #Normalize returns by the length of episode
        #if episode_counter%10==0 and episode_counter>0: print(weight_saver[-1])

        prev_params = dict(params)
        condition = True
        for i in range(0,num_targets):
            if np.mean(pos_error[i])>10000:
                condition = False
                break
                episode_condition = False
                episode_counter-=1

        if not condition:
            #print("OOPSSSS...")
            continue
        #if episode_counter%100==0 and training:
            #print("Starting the evaluation phase...")
            #training = False
            #episode_condition = False


        condition = True
        if episode_condition and training:
            normalized_discounted_return = discounted_return
            episode_actions = s.sensor_actions
            #init_weight = np.array(weight)
            rate = gen_learning_rate(episode_counter,learning_rate,1E-12,20000)
            internal_rate = gen_learning_rate(episode_counter, 3*1E-5, 1E-15, 20000)
            total_adjustment = np.zeros(np.shape(weight))
            for e in range(0,len(episode_actions)):
                #calculate gradiant
                #state = np.array(episode_state[e]).reshape(len(episode_state[e]),1)
                out_state = np.array(episode_state_out_layer[e]).reshape(len(episode_state_out_layer[e]),1)
                backpropagated_terms = episode_state[e]

                #calculate gradient
                if method==0:
                    deriv_with_out_state = (episode_actions[e].reshape(2, 1) - params[0]['weight2'].dot(out_state)).transpose().dot(params[0]['weight2']) #1 by n_s==> derivative of F with respect to the output state-vector
                    internal_gradiant1 = deriv_with_out_state.dot(backpropagated_terms[0]) #1 by 6
                    internal_gradiant2 = deriv_with_out_state.dot(backpropagated_terms[1]) #1 by 6
                    internal_gradiant = np.concatenate([internal_gradiant1,internal_gradiant2])

                    #gradiant = ((episode_actions[e].reshape(2,1)-params[0]['weight'].dot(state)).dot(state.transpose()))/sigma**2#This is the gradiant
                    gradiant_out_layer = ((episode_actions[e].reshape(2, 1) - params[0]['weight2'].dot(out_state)).dot(
                        out_state.transpose())) / sigma ** 2  # This is the gradiant
                elif method==1:
                    gradiant = ((episode_actions[e].reshape(2, 1) - params[1]['weight'].dot(state)).dot(
                        state.transpose())) / sigma ** 2  # This is the gradiant
                elif method==2:
                    #Gradient for MLP
                    pass

                if np.max(np.abs(gradiant_out_layer))>1E2 or np.max(np.abs(internal_gradiant))>1E2:
                    #print("OOPPSSSS...")
                    continue #clip large gradients

                if method==0:
                    adjustment_term_out_layer = gradiant_out_layer*normalized_discounted_return[e]#an unbiased sample of return
                    adjustment_term_internal_layer = internal_gradiant*normalized_discounted_return[e]
                    params[0]['weight2'] += rate * adjustment_term_out_layer
                    params[0]['weight'] += internal_rate* adjustment_term_internal_layer
                elif method==1:
                    adjustment_term = gradiant * normalized_discounted_return[e]  # an unbiased sample of return
                    params[1]['weight'] += rate * adjustment_term
                elif method==2:
                    #Gradient for MLP
                    pass

            #if not condition:
             #   weight = prev_weight
              #  continue

            episode_counter+=1
            flatted_weights1 = list(params[0]['weight'][0, :]) + list(params[0]['weight'][1, :])
            flatted_weights2 = list(params[0]['weight2'][0, :]) + list(params[0]['weight2'][1, :])
            temp1 = []
            [temp1.append(str(x)) for x in flatted_weights1]
            temp2 = []
            [temp2.append(str(x)) for x in flatted_weights2]

            weight_file.write("\t".join(temp1)+"$$$"+"\t".join(temp2)+"\n")
            #flatted_weights = list(weight[0, :]) + list(weight[1, :])
            #temp = []
            #[temp.append(str(x)) for x in flatted_weights]
            #weight_file.write("\t".join(temp)+"\n")
            weight_saver1.append(params[0]['weight'][0][0])
            weight_saver2.append(params[0]['weight'][1][0])

            weight_saver2_1.append(params[0]['weight2'][0][0])
            weight_saver2_2.append(params[0]['weight2'][1][0])
        else:
            #print("garbage trajectory: no-update")
            pass


        #if not training:
        return_saver.append(sum(reward))

        error_saver.append(np.mean(pos_error))

        #print(len(return_saver),n)
        if episode_counter%100 == 0 and episode_counter>0:
            # if episode_counter%100==0 and episode_counter>0:
            print(episode_counter, np.mean(return_saver), sigma)
            #print(params[method]['weight'])
            #weight = np.reshape(np.array(weights[episode_counter]), [2, 6])
            #print(weight)
            reward_file.write(str(np.mean(sorted(return_saver,reverse=True)[0:int(.95*len(return_saver))]))+"\n")
            error_file.write(str(np.mean(sorted(error_saver)[0:int(.95*len(error_saver))])) + "\n")
            error_file_median.write(str(np.median(sorted(error_saver)[0:int(.95*len(error_saver))])) + "\n")
            var_error_file.write(str(np.var(sorted(error_saver)[0:int(.95*len(error_saver))])) + "\n")
            var_file.write(str(np.var(sorted(return_saver,reverse=True)[0:int(.95*len(return_saver))]))+"\n")
            #weight_file.write(str(np.mean(return_saver)) + "\n")

            avg_reward.append(np.mean(sorted(return_saver)[0:int(.95*len(return_saver))]))
            avg_error.append(np.mean(sorted(error_saver)[0:int(.95*len(error_saver))]))
            var_reward.append(np.var(return_saver))
            reward_file.close()
            var_file.close()
            error_file.close()
            error_file_median.close()
            var_error_file.close()
            weight_file.close()

            reward_file = open(
                result_folder + "reward_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a")
            error_file = open(
                result_folder + "error_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a")
            var_file = open(
                result_folder + "var_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a")
            var_error_file = open(
                result_folder + "var_error_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt",
                "a")
            weight_file = open(
                result_folder + "weight_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a")
            error_file_median = open(
                result_folder + "error_median_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt",
                "a")

            return_saver = []
            error_saver = []
        num_episodes.append(n)
예제 #26
0
    def run(self):
        '''
            This method is responsable of:
            1. Instantiate the sensor objects
            2. Identify if is running 'local' or not
            3. Identify if the data generated will be save on the 'influxDB'
            4. Generate the sensors data
        '''

        try:

            logger.info('Emulator run triggered')

            ### instantiation of the sensors

            ### create the list that will 'host' the sensors objects
            listSensors = []

            ### loop that control the sensors instantiation process
            for i in range(0, self.numSensors):
                listSensors.append(s.sensor())

            ### if is not running local, create the client that connects to the MQTT broker
            if not self.local:

                ### get the MQTT broker address
                broker = config['mosquitto']['broker_address']

                ### get the MQTT broker port
                port = int(config['mosquitto']['broker_port'])

                ### get the MQTT topic name structure
                topic_structure = config['mosquitto']['topic_structure']

                ### Create the MQTT broker client
                mqtt_broker_client = paho.Client("sensor_Emulator")

                ### Connect to the MQTT broker
                mqtt_broker_client.connect(broker, port)

            ### control if the data generated by the sensors will be load to the local 'influxDB'
            if self.store_db:

                ### set InfluxDB database name
                db_name = 'sensor_data'

                ### Create the InfluxDB client
                client = InfluxDBClient('localhost', 8086, 'root', 'root',
                                        db_name)

                ### Drop the 'db_name' database, if exist (the idea is to always start with an empty DB)
                client.drop_database(db_name)

                ### Create the InfluxDB database, using the 'db_name' value as a name
                client.create_database(db_name)

            ### Generate the data and publish / print
            while True:

                ### Set the timestamp
                timestamp = datetime.fromtimestamp(time.time())

                if self.store_db:
                    ### InfluxDB works with UTC, so I'm adjusting the timestamp to looks correctly on Grafana
                    ### At the moment I'm UTC + 1, this is why I'm adjusting -1 hour
                    timestamp = timestamp - timedelta(hours=1, minutes=0)

                ### This loop will, for each sensor:
                ### 1. generate the data
                ### 2. Create the message (that will be publish to the MQTT topic)
                ### 3. Publish the data
                ### 4. if 'store_db is True', load the data to the influxDB
                for i in range(0, self.numSensors):

                    ### run the sensor method that generates the data
                    data = listSensors[i].generateData()

                    ### check if is running in 'local mode'
                    if self.local:

                        ### Set/Create the message
                        message = "sensor_" + str(i + 1) + "|" + str(
                            timestamp.strftime(
                                '%Y-%m-%d|%H:%M:%S')) + "|" + str(data)

                        ### print the message on the terminal
                        print(message)

                    ### on the case that it is not running locally
                    else:

                        ### Set/Create the message
                        message = str(timestamp.strftime(
                            '%Y-%m-%d|%H:%M:%S')) + "|" + str(data)

                        ### create a topic
                        topic = topic_structure.replace(
                            "[SENSOR_ID]", str(i + 1))

                        ### publish the data to the topic
                        mqtt_broker_client.publish(topic, message)

                        logger.info("topic|" + str(topic) + "|message|" +
                                    message)

                    ### on the case that the data will be load to 'influxDB'
                    if self.store_db:

                        ### Set the name of the table
                        table_name = "sensor_data"

                        ### Specify the format of the date-time
                        date_format = '%Y-%m-%dT%H:%M:%S%Z'

                        ### Create the json structure to load to 'influxDB'
                        json_body = [{
                            "measurement":
                            table_name,
                            "time":
                            str(timestamp.strftime(date_format) + "Z"),
                            "tags": {
                                "sensorId": str(i + 1)
                            },
                            "fields": {
                                "value": data,
                                "tic": 1
                            }
                        }]

                        ### Load the data to 'influxDB'
                        client.write_points(json_body)

                        logger.info("data inserted on db: " + db_name +
                                    "table: " + table_name)
                        logger.info("data inserted:" + str(json_body))

                ### It is here that the time is controled (the frequency that the data is generated)
                time.sleep(self.frequency_in_seconds)

        except Exception as e:
            logger.exception(e)
예제 #27
0
#! /usr/bin/python2.7

import time 
import datetime
from sensor import sensor
from MysqlConnect import MySqlConnect

sens = sensor()
#sens.initGPIO()

database = MySqlConnect()
database.getConnect()

data = {}


while(1):
    dt = str(datetime.datetime.today()).split('.')[0]

    sens.readTemperature()
    data['id'] = sens.getID()
    data['name'] = str(sens.getName())
    data['address'] = 9 # dt # str(sens.getAddress())
    data['temperature'] = sens.getTemperature()
    if sens.getTemperature() < 30 :
        data['type'] = 'normal'
    else:
        data['type'] = 'danger'

    data['lat'] = 58.328315
    data['lng'] = 56.245774
예제 #28
0
 def stopSensors(self):
     for sense in self.sensorMap:
         sense = sensor.sensor(sense)
         sense.update(0)
         logger.debug('Sensor ' + sense.port + ' stopping')
     return self.getAllData()
                       init_target_state[i][3], vel_var, vel_var,
                       "CONS_V"))  #constant-velocity model for target motion
        A, B = t[0].constant_velocity(1E-10)  #Get motion model
        x_var = t[0].x_var
        y_var = t[0].y_var

        tracker_object = []
        for i in range(0, num_targets):
            tracker_object.append(
                EKF_tracker(init_for_smc[i], np.array(init_covariance), A, B,
                            x_var, y_var, bearing_var))  #create tracker object
            #smc_object = smc_tracker(A,B,x_var,y_var,bearing_var,1000,np.array(init_for_smc))

        #Initialize sensor object
        if method == 0:
            s = sensor("POLICY_COMM_LINEAR"
                       )  #create sensor object (stochastic policy)
        elif method == 1:
            s = sensor("POLICY_COMM_RBF")
        elif method == 2:
            s = sensor("POLICY_COMM_MLP")
        measure = measurement(bearing_var)  #create measurement object

        m = []
        x_est = []
        y_est = []
        x_vel_est = []
        y_vel_est = []
        x_truth = []
        y_truth = []
        x_vel_truth = []
        y_vel_truth = []
예제 #30
0
def main():
    s = sensor.sensor()
    logger.log_level(s.get_distance())
예제 #31
0
#!/usr/bin/python3

from sensor import sensor
import time
from motors import Actuation
from mapping import Mapping

##################
### PARAMETERS ###
##################
CAMERA_NUM = 2  # 1 for PiCamera, 2 for USB Camera

###############
### GLOBALS ###
###############
cam = sensor(CAMERA_NUM)
ctrl = Actuation()
mapping = Mapping()


def main():
    cam.start()
    while True:
        if cam.stopped:
            break
        cards, num = cam.get_cards()
        if num != 0:
            print(num)
            print("Card: {} {}".format(cards[0].best_rank_match,
                                       cards[0].best_suit_match))
        time.sleep(0.2)
예제 #32
0
 def getAllData(self):
     sensorData = []
     for sense in self.sensorMap:
         sense = sensor.sensor(sense)
         sensorData += [sense.collect()]
     return sensorData
예제 #33
0
from loggingQ import setupLogger


import argparse

logger = setupLogger('myQ', True, 'sensor_test.log')

calibIMU = False
parser = argparse.ArgumentParser()
parser.add_argument('-c', dest='calibIMU', action='store_true', help='Calibrate IMU')
parser.add_argument('-i', dest='imulog', action='store_true', help='save IMU data log: myQ_sensor.csv')
args = parser.parse_args()
calibIMU = args.calibIMU
imuLog = args.imulog

mySensor = sensor(imulog=imuLog, simulation=False)

if calibIMU:
    mySensor.calibrate()

mySensor.start()


screen = curses.initscr()
# turn off input echoing
curses.noecho()
# respond to keys immediately (don't wait for enter)
curses.cbreak()
# map arrow keys to special values
screen.keypad(True)
#timeout in millis
예제 #34
0
initEsc = args.initEsc
gpio = args.gpio

logger = setupLogger("myQ")

print("gpio: " + str(gpio))
print("initEsc: " + str(initEsc))

mymotor = motor("m1", gpio, simulation=False)
# where 18 is  GPIO18 = pin 12
# GPIO23 = pin 16
# GPIO24 = pin 18
# GPIO25 = pin 22


mySensor = sensor()
mySensor.start()

print("***Press ENTER to start")
res = raw_input()
mymotor.start()

# TODO the next line code is necessary to INITIALIZE the ESC to a desired MAX PWM
# I suggest to run this line at least once, for each esc
# in order to obtain the same behaviour in all the ESCs
# use arg -i to  use this line
if initEsc:
    print("***Disconnect ESC power")
    print("***then press ENTER")
    res = raw_input()
    mymotor.setW(100)
예제 #35
0
lg = piLog()
lg.openLog(piServer.serverLog, moduleLocalName, configXML.verbose)
lg.log(clock_milliseconds(), "info", "starting " + moduleLocalName + " - PID : " + str(os.getpid()))
lg.log(clock_milliseconds(), "info", "port " + moduleLocalName + " : ") #$::piServer::portNumber(${::moduleLocalName})"
lg.log(clock_milliseconds(), "info", "confXML : ") #$confXML")

# On affiche les infos dans le fichier de debug
for element in [attr for attr in dir(configXML()) if not callable(attr) and not attr.startswith("__")]:
    lg.log(clock_milliseconds(), "info", element + " : " + getattr(configXML,element)) 

# Démarrage du serveur
lg.log(clock_milliseconds(), "info", "starting serveur")

piServ = piServer( moduleLocalName, configXML.verbose, "0.0.0.0", piServer.serverAcqSensorUSB)

ssor = sensor.sensor(moduleLocalName, configXML.verbose, piServer.serverLog)

readIsDone = 0
while True:

    # On écoute si un client veut se connecter
    piServ.listen()

    # Toute les 5 secondes on vient lire les différents capteurs
    time.sleep(0.01)
    
    epoch_time = int(time.time())

    if epoch_time % 5 == 0:
        if readIsDone == 0 :
            ssor.readSensor(0)
예제 #36
0
# -*- coding: utf-8 -*-
from sensor import sensor
import curses

mySensor = sensor()
mySensor.start()

screen = curses.initscr()
# turn off input echoing
curses.noecho()
# respond to keys immediately (don't wait for enter)
curses.cbreak()
# map arrow keys to special values
screen.keypad(True)

try:

    cycling = True
    while cycling:

        s = '|roll: ' + str(mySensor.roll)
        s += '|pitch: ' + str(mySensor.pitch)
        s += '|yaw: ' + str(mySensor.yaw)
        screen.clear()
        screen.addstr(1, 1, 'Press any button to stop')
        screen.addstr(2, 2, s)
        #timeout in millis
        screen.timeout(500)
        #getch returns -1 if timeout
        res = screen.getch()
예제 #37
0
from sensor import sensor

### NEED TO BE CHANGED REGULARLY ###
start_date = "2021-02-01"
end_date = "2021-02-07"

### LESS LIKELY TO CHANGE ###
pm25_y_label = "PM 2.5 (medijana)"
chart_title_format = "{sensor_name} za nedelju od {start_date} do {end_date}"

all_sensors = [
    sensor(name="Devet Jugovica", id="esp8266-8000692"),
    sensor(name="Starine Novaka", id="esp8266-8023432")
]

delta_in_days = 1
days = ["PON", "UTO", "SRE", "CET", "PET", "SUB", "NED"]

output_folder = "out"

output_image_format = "png"
output_txt_format = "txt"

PM10_INDEX = 7
PM25_INDEX = 8

url_pattern = "https://api-rrd.madavi.de/data_csv/csv-files/{date_part}/data-{sensor_id}-{date_part}.csv"
filename_pattern = "data-{date_part}.csv"

csv_folder = "data"
예제 #38
0
 def __init__(self):
     self.ToF = sensor()
예제 #39
0
    n._set_loco(nn, l.speed, l.reverse, l.opts)


l = logger(console=True)
l._add_trace('controller')
l._add_trace('serial')

try:
    dev = sys.argv[1]
except IndexError:
    dev = None
d = dispatcher()
n = nce_controller(dev)
usb_listener.make_listeners()
for i in range(12):
    sensor(str(i + 1))
curloco = 0
loco_infos = {}

while True:
    cmd = input('command: ').strip()
    if cmd == '':
        break
    try:
        cmd0 = cmd[0]
        rest = cmd[1:].strip()
        if cmd0 == 'p':
            n.ping()
        elif cmd0 == 'l':
            curloco = int(rest)
            get_loco(curloco)
            MAX_UNCERTAINTY, MAX_UNCERTAINTY, MAX_UNCERTAINTY, MAX_UNCERTAINTY
        ])  #initial covariance of state estimation

        t = target(init_target_state[0:2], init_target_state[2],
                   init_target_state[3], .1, .1,
                   "CONS_V")  #constant-velocity model for target motion
        A, B = t.constant_velocity(1E-10)  #Get motion model
        x_var = t.x_var
        y_var = t.y_var

        tracker_object = EKF_tracker(init_for_smc, init_covariance, A, B,
                                     x_var, y_var,
                                     bearing_var)  #create tracker object
        #smc_object = smc_tracker(A,B,x_var,y_var,bearing_var,1000,np.array(init_for_smc))

        s = sensor("POLICY_COMM")  #create sensor object (stochastic policy)
        #s = sensor("CONS_A")
        measure = measurement(bearing_var)  #create measurement object

        m = []
        x_est = []
        y_est = []
        x_vel_est = []
        y_vel_est = []
        x_truth = []
        y_truth = []
        x_vel_truth = []
        y_vel_truth = []
        uncertainty = []

        vel_error = []
예제 #41
0
  def __init__(self):
    self.logger = logging.getLogger('sensing.sensing')

    self.tmpSensor = sensor()
    self.tmpJsonFileDataMgr = jsonFileDataMgr()
def run(args):
    # initialize parameters of interest
    # Method:
    # 0: linear policy
    # 1: RBF policy
    # 2: MLP policy
    
    method = args[0]
    RBF_components = args[1]
    MLP_neurons = args[2]
    process_index = args[3]
    folder_name = args[4]
    np.random.seed(1 + 100)
    vel_var = args[5]
    
    np.random.seed(process_index)
    print("Starting Thread:" + str(process_index))

    #Initialize all the parameters
    params ={0:{},1:{},2:{}}
    if method==0:
        params[0]["weight"] = np.random.normal(0, .3, [2, num_states])
        #params[0]["weight"] = np.array([[ 1.45702249, -1.17664153, -0.11593174,  1.02967173, -0.25321044,
         #0.09052774],
       #[ 0.67730786,  0.3213561 ,  0.99580938, -2.39007038, -1.16340594,
        #-1.77515938]])
    elif method==1:
        featurizer = sklearn.pipeline.FeatureUnion([("rbf1", RBFSampler(gamma=rbf_var, n_components=RBF_components, random_state=1))])
        featurizer.fit(np.array(list_of_states))  # Use this featurizer for normalization
        params[1]["weight"] = np.random.normal(0, 1, [2, RBF_components])
    elif method==2:
        params[2]["weigh1"] = np.random.normal(0, 1, [MLP_neurons, num_states])
        params[2]["bias1"] = np.random.normal(0,1,[MLP_neurons,1])
        params[2]["weigh2"] = np.random.normal(0, 1, [2, MLP_neurons])
        params[2]["bias2"] = np.random.normal(0, 1, [2, 1])

    return_saver = []
    error_saver = []
    episode_counter = 0
    weight_saver1 = []
    weight_saver2 = []
    #for episode_counter in range(0,N_max):
    #Training parameters
    avg_reward = []
    avg_error = []
    var_reward = []
    training = True


    result_folder = base_path+folder_name+"/"
    reward_file = open(result_folder+"reward_noise:"+str(vel_var)+"_"+str(process_index)+  "_linear_6states.txt","a")
    error_file = open(result_folder + "error_noise:" + str(vel_var) +"_"+str(process_index)+ "_linear_6states.txt", "a")
    error_file_median = open(result_folder + "error_median_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt",
                      "a")
    var_file = open(result_folder + "var_noise:" + str(vel_var) +"_"+str(process_index)+ "_linear_6states.txt", "a")
    var_error_file = open(result_folder + "var_error_noise:" + str(vel_var) +"_"+str(process_index)+ "_linear_6states.txt", "a")
    weight_file = open(result_folder + "weight_noise:" + str(vel_var) +"_"+str(process_index)+ "_linear_6states.txt", "a")

    #flatten initial weight and store the values
    if method==0:
        weight = params[0]['weight']
        flatted_weights = list(weight[0, :]) + list(weight[1, :])
        temp = []
        [temp.append(str(x)) for x in flatted_weights]
        weight_file.write("\t".join(temp)+"\n")
    elif method==1:
        weight = params[1]['weight']
        flatted_weights = list(weight[0, :]) + list(weight[1, :])
        temp = []
        [temp.append(str(x)) for x in flatted_weights]
        weight_file.write("\t".join(temp) + "\n")
    elif method==2:
        pass

    #weight = np.reshape(np.array(weights[0]), [2, 6])
    sigma = sigma_max
    while episode_counter<N_max:
        #sigma = gen_learning_rate(episode_counter,sigma_max,.1,20000)
        if episode_counter%1500==0 and episode_counter>0:
            sigma-= .15
            sigma = max(.1,sigma)
        #sigma = sigma_max
        discounted_return = np.array([])
        discount_vector = np.array([])
        #print(episodes_counter)
        scen = scenario(1,1)
        bearing_var = 1E-2#variance of bearing measurement
        #Target information
        x = 10000*random.random()-5000#initial x-location
        y = 10000 * random.random() - 5000#initial y-location
        xdot = 10*random.random()-5#initial xdot-value
        ydot = 10 * random.random() - 5#initial ydot-value
        init_target_state = [x,y,xdot,ydot]#initialize target state
        init_for_smc = [x+np.random.normal(0,5),y+np.random.normal(0,5),np.random.normal(0,5),np.random.normal(0,5)]#init state for the tracker (tracker doesn't know about the initial state)
        #init_for_smc = [x, y, xdot, ydot]
        init_sensor_state = [10000*random.random()-5000,10000 * random.random() - 5000,3,-2]#initial sensor-state

        temp_loc = np.array(init_target_state[0:2]).reshape(2,1)
        init_location_estimate = temp_loc+0*np.random.normal(np.zeros([2,1]),10)
        init_location_estimate = [init_location_estimate[0][0],init_location_estimate[1][0]]
        init_velocity_estimate = [6*random.random()-3,6*random.random()-3]
        init_velocity_estimate = [init_target_state[2],init_target_state[3]]
        init_estimate = init_location_estimate+init_velocity_estimate
        init_covariance = np.diag([MAX_UNCERTAINTY,MAX_UNCERTAINTY,MAX_UNCERTAINTY,MAX_UNCERTAINTY])#initial covariance of state estimation

        t = target(init_target_state[0:2], init_target_state[2], init_target_state[3], vel_var, vel_var, "CONS_V")#constant-velocity model for target motion
        A, B = t.constant_velocity(1E-10)#Get motion model
        x_var = t.x_var
        y_var = t.y_var

        tracker_object = EKF_tracker(init_for_smc, init_covariance, A,B,x_var,y_var,bearing_var)#create tracker object
        #smc_object = smc_tracker(A,B,x_var,y_var,bearing_var,1000,np.array(init_for_smc))

        #Initialize sensor object
        if method==0:
            s = sensor("POLICY_COMM_LINEAR")#create sensor object (stochastic policy)
        elif method==1:
            s = sensor("POLICY_COMM_RBF")
        elif method==2:
            s = sensor("POLICY_COMM_MLP")
        measure = measurement(bearing_var)#create measurement object

        m = []
        x_est = []; y_est = []; x_vel_est = []; y_vel_est = []
        x_truth = [];
        y_truth = [];
        x_vel_truth = [];
        y_vel_truth = []
        uncertainty = []
        vel_error = []
        pos_error = []
        iteration = []
        innovation = []
        reward = []
        episode_condition = True
        n=0
        violation = 0
        #store required information
        episode_state = []
        episode_MLP_state = []
        episode_actions = []

        while episode_condition:
            t.update_location()
            m.append(measure.generate_bearing(t.current_location,s.current_location))
            tracker_object.update_states(s.current_location, m[-1])
            normalized_innovation = (tracker_object.innovation_list[-1])/tracker_object.innovation_var[-1]
            #print(normalized_innovation)
            #if (normalized_innovation<1E-4 or n<10) and n<200:
                #end of episode
            current_state = list(tracker_object.x_k_k.reshape(len(tracker_object.x_k_k))) + list(s.current_location)

            #print(current_state)
            #state normalization
            x_slope = 2.0/(scen.x_max-scen.x_min)
            y_slope = 2.0 / (scen.y_max - scen.y_min)

            x_slope_sensor = 2.0 / (40000)
            y_slope_sensor = 2.0 / (40000)

            vel_slope = 2.0/(scen.vel_max-scen.vel_min)
            #normalization
            current_state[0] = -1+x_slope*(current_state[0]-scen.x_min)
            current_state[1] = -1 + y_slope * (current_state[1] - scen.y_min)
            current_state[2] = -1 + vel_slope * (current_state[2] - scen.vel_min)
            current_state[3] = -1 + vel_slope * (current_state[3] - scen.vel_min)
            current_state[4] = -1 + x_slope * (current_state[4] -scen.x_min)
            current_state[5] = -1 + y_slope * (current_state[5] - scen.y_min)


            #Refactor states based on the usage
            if method==0 or method==2:
                input_state = current_state
            elif method==1:
                #Generate states for the RBF input
                input_state =  featurizer.transform(np.array(current_state).reshape(1,len(current_state)))
                input_state = list(input_state[0])


            extra_information = s.update_location_new(params,input_state,sigma)
            estimate = tracker_object.x_k_k
            episode_state.append(input_state)
            if method==2: episode_MLP_state.append(extra_information) #Output of the first layer for Gradient calculation
            truth = t.current_location
            x_est.append(estimate[0])
            y_est.append(estimate[1])
            x_vel_est.append(estimate[2])
            y_vel_est.append(estimate[3])
            x_truth.append(truth[0])
            y_truth.append(truth[1])
            x_vel_truth.append(t.current_velocity[0])
            y_vel_truth.append(t.current_velocity[1])
            vel_error.append(np.linalg.norm(estimate[2:4]-np.array([t.current_velocity[0],t.current_velocity[1]]).reshape(2,1)))
            pos_error.append(np.linalg.norm(estimate[0:2]-np.array(truth).reshape(2,1)))
            innovation.append(normalized_innovation[0])
            unormalized_uncertainty = np.sum(tracker_object.p_k_k.diagonal())
            #if unormalized_uncertainty>MAX_UNCERTAINTY:
             #   normalized_uncertainty = 1
            #else:
             #   normalized_uncertainty = (1.0/MAX_UNCERTAINTY)*unormalized_uncertainty
            uncertainty.append((1.0 / MAX_UNCERTAINTY) * unormalized_uncertainty)
            if len(uncertainty)<window_size+window_lag:
                reward.append(0)
            else:
                current_avg = np.mean(uncertainty[-window_size:])
                prev_avg = np.mean(uncertainty[-(window_size+window_lag):-window_lag])
                if current_avg<prev_avg or uncertainty[-1]<.1:
                #if current_avg < prev_avg:
                    reward.append(1)
                else:
                    reward.append(0)

            #reward.append(-1*uncertainty[-1])
            #update return
            discount_vector = gamma*np.array(discount_vector)
            discounted_return+= (1.0*reward[-1])*discount_vector
            new_return = 1.0*reward[-1]
            list_discounted_return = list(discounted_return)
            list_discounted_return.append(new_return)
            discounted_return = np.array(list_discounted_return)

            list_discount_vector = list(discount_vector)
            list_discount_vector.append(1)
            discount_vector = np.array(list_discount_vector)
            iteration.append(n)
            if n>episode_length: break
            n+=1

        #Based on the return from the episode, update parameters of the policy model
        #Normalize returns by the length of episode
        #if episode_counter%10==0 and episode_counter>0: print(weight_saver[-1])

        prev_params = dict(params)
        condition = True
        if np.mean(pos_error)>10000:
            continue
            episode_condition = False
            episode_counter-=1
        #if episode_counter%100==0 and training:
            #print("Starting the evaluation phase...")
            #training = False
            #episode_condition = False
        condition = True
        if episode_condition and training:
            normalized_discounted_return = discounted_return
            episode_actions = s.sensor_actions
            #init_weight = np.array(weight)
            rate = gen_learning_rate(episode_counter,learning_rate,1E-8,10000)
            total_adjustment = np.zeros(np.shape(weight))
            for e in range(0,len(episode_actions)):
                #calculate gradiant
                state = np.array(episode_state[e]).reshape(len(episode_state[e]),1)

                #calculate gradient
                if method==0:
                    gradiant = ((episode_actions[e].reshape(2,1)-params[0]['weight'].dot(state)).dot(state.transpose()))/sigma**2#This is the gradiant
                elif method==1:
                    gradiant = ((episode_actions[e].reshape(2, 1) - params[1]['weight'].dot(state)).dot(
                        state.transpose())) / sigma ** 2  # This is the gradiant
                elif method==2:
                    #Gradient for MLP
                    pass

                if np.max(np.abs(gradiant))>1E2: continue #clip large gradients

                if method==0:
                    adjustment_term = gradiant*normalized_discounted_return[e]#an unbiased sample of return
                    params[0]['weight'] += rate * adjustment_term
                elif method==1:
                    adjustment_term = gradiant * normalized_discounted_return[e]  # an unbiased sample of return
                    params[1]['weight'] += rate * adjustment_term
                elif method==2:
                    #Gradient for MLP
                    pass

            #if not condition:
             #   weight = prev_weight
              #  continue

            episode_counter+=1
            #flatted_weights = list(weight[0, :]) + list(weight[1, :])
            #temp = []
            #[temp.append(str(x)) for x in flatted_weights]
            #weight_file.write("\t".join(temp)+"\n")
            #weight_saver1.append(weight[0][0])
            #weight_saver2.append(weight[0][1])
        else:
            #print("garbage trajectory: no-update")
            pass


        #if not training:
        return_saver.append(sum(reward))

        error_saver.append(np.mean(pos_error))

        #print(len(return_saver),n)
        if episode_counter%100 == 0 and episode_counter>0:
            # if episode_counter%100==0 and episode_counter>0:
            print(episode_counter, np.mean(return_saver), sigma)
            #print(params[method]['weight'])
            #weight = np.reshape(np.array(weights[episode_counter]), [2, 6])
            #print(weight)
            reward_file.write(str(np.mean(sorted(return_saver)[0:int(.95*len(return_saver))]))+"\n")
            error_file.write(str(np.mean(sorted(error_saver)[0:int(.95*len(error_saver))])) + "\n")
            error_file_median.write(str(np.median(sorted(error_saver)[0:int(.95*len(error_saver))])) + "\n")
            var_error_file.write(str(np.var(sorted(error_saver)[0:int(.95*len(error_saver))])) + "\n")
            var_file.write(str(np.var(sorted(return_saver)[0:int(.95*len(return_saver))]))+"\n")
            #weight_file.write(str(np.mean(return_saver)) + "\n")

            avg_reward.append(np.mean(sorted(return_saver)[0:int(.95*len(return_saver))]))
            avg_error.append(np.mean(sorted(error_saver)[0:int(.95*len(error_saver))]))
            var_reward.append(np.var(return_saver))
            reward_file.close()
            var_file.close()
            error_file.close()
            error_file_median.close()
            var_error_file.close()
            weight_file.close()

            reward_file = open(
                result_folder + "reward_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a")
            error_file = open(
                result_folder + "error_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a")
            var_file = open(
                result_folder + "var_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a")
            var_error_file = open(
                result_folder + "var_error_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt",
                "a")
            weight_file = open(
                result_folder + "weight_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt", "a")
            error_file_median = open(
                result_folder + "error_median_noise:" + str(vel_var) + "_" + str(process_index) + "_linear_6states.txt",
                "a")

            return_saver = []
            error_saver = []
        num_episodes.append(n)
예제 #43
0
 def getData(self, port):
     if self.sensorMap.has_key(port):
         sense = sensor.sensor(self.sensorMap.get(port))
         return sense.collect()
     return None
예제 #44
0
import sys
sys.path.append("../common")
sys.path.append("../sensor")
import sensor
import message

import time

pm = message.print_messager()
foo = sensor.sensor("Test sensor", .01, pm)
foo.init()
foo.start()
time.sleep(1)
foo.stop()
예제 #45
0
 def startSensors(self):
     for sense in self.sensorMap:
         sense = sensor.sensor(sense)
         sense.run()