def gps_kf(self, dimension=1, init_data=0): """ Invokes Kalman filter for 1D and 2D and plots graphs. """ abs_error = 10.0 try: sensor = Sensor() kf = KalmanFilter(error=abs_error) # generate GPS observations. obs, time = sensor.measure(init_data, abs_error=abs_error, unit='position') # estimate position using Kalman filter est = kf.estimate(obs) # print out results and plot all. self.mean, self.median, est_error = self.print_out( obs, est, init_data) self.plot(obs=obs, est=est, est_error=est_error, time=time, init_data=init_data, dim=dimension, sensor='gps') except Exception as e: print('Exception: '.upper(), e) traceback.print_tb(e.__traceback__)
def test_sensor(): # Each take call to get_new_value should get a new value. s = Sensor(normal_value=10, aggregation_interval=1, perunit_drop_rate=0.0, perunit_confidence_band=0.05) assert s._n == 0 assert s.normal_value == 10 assert s.perunit_dropping == 0.0 assert 1 == s.interval assert s.get_new_value(120, 10) is None assert s.get_new_value(121, 10) # must do 3 calls to get_new_value to get a value. s = Sensor(normal_value=10, aggregation_interval=3, perunit_drop_rate=0.0, perunit_confidence_band=0.05) assert 3 == s.interval, f"The interval is not 3 it was {s.interval}" # For 0, 1, 2 is None, the next will be the real value. for r in range(3): v = s.get_new_value(130 + r, 10) assert v is None, f"The {r}th item was not None" v = s.get_new_value(133, 10) assert v is not None for r in range(3): v = s.get_new_value(130 + r, 10) assert v is None, f"The {r}th item was not None" v = s.get_new_value(137, 10) assert v is not None
def setUp(self, mocked_pigpio, mocked_open_connection, mocked_write_raw_data): mocked_pigpio.connected = True self.__sensor = Sensor(bus=1, address=14, start_command=0x4c, buffer_length=35)
__author__ = 'frivas' import sys from PyQt4 import QtCore, QtGui from gui.GUI import MainWindow from gui.threadGUI import ThreadGUI from sensors.sensor import Sensor from sensors.threadSensor import ThreadSensor from MyAlgorithm import MyAlgorithm if __name__ == "__main__": sensor = Sensor() algorithm=MyAlgorithm(sensor) app = QtGui.QApplication(sys.argv) myGUI = MainWindow() myGUI.setSensor(sensor) myGUI.setAlgorithm(algorithm) myGUI.show() t1 = ThreadSensor(sensor,algorithm) t1.daemon=True t1.start() t2 = ThreadGUI(myGUI) t2.daemon=True t2.start()
frame = MainWindow() grid = Grid(frame) removeMapFromArgs() cfg = config.load(sys.argv[1]) #starting comm jdrc = comm.init(cfg, 'TeleTaxi') motors = jdrc.getMotorsClient("TeleTaxi.Motors") pose = jdrc.getPose3dClient("TeleTaxi.Pose3D") vel = Velocity(0, 0, motors.getMaxV(), motors.getMaxW()) frame.setVelocity(vel) sensor = Sensor(grid, pose, True) sensor.setGetPathSignal(frame.getPathSig) frame.setGrid(grid) frame.setSensor(sensor) algorithm = MyAlgorithm(grid, sensor, vel) frame.setAlgorithm(algorithm) frame.show() t1 = ThreadMotors(motors, vel) t1.daemon = True t1.start() t2 = ThreadGUI(frame) t2.daemon = True t2.start()
motors = jdrc.getMotorPubObject() pose3d = jdrc.getPoseListnerObject() laser = jdrc.getLaserListnerObject() # pathListener = ListenerPath("/amazon_warehouse_robot/move_base/NavfnROS/plan") # This is to be updated navigation_client = NavigateToPoseActionClient() print("Subscribed To Move Base Client, Starting Application") app = QApplication(sys.argv) myGUI = MainWindow() grid = Grid(myGUI) vel = Velocity(0.0, 0.0, motors.getMaxV(), motors.getMaxW()) sensor = Sensor(grid, pose3d, True) sensor.setGetPathSignal(myGUI.getPathSig) myGUI.setVelocity(vel) myGUI.setGrid(grid) myGUI.setSensor(sensor) algorithm = MyAlgorithm(grid, sensor, vel, navigation_client) myGUI.setAlgorithm(algorithm) myGUI.show() removeMapFromArgs() t1 = ThreadMotors(motors, vel) t1.daemon = True t1.start() t2 = ThreadGUI(myGUI)
def main(): assert(0 < len(conf.sensors) <= 8) sensors = list() print("Welcome to the Plant-Life sensor system!") print("Initialising sensors...") for i in range(len(conf.sensors)): s = conf.sensors[i] if s.get("enabled"): if s.get("fake"): sensors.append(FakeSensor(s.get("id"), s.get("fake_good_threshold"), s.get("fake_timerate"))) else: sensors.append(Sensor(s.get("id"), i, s.get("max_threshold"))) print("Sensors initialised!") print("Connecting to server...") connected = threading.Event() sent = threading.Event() sent.set() mc = mqtt.Client(client_id=conf.device_id, clean_session=False, userdata=[connected, sent, 0]) mc.on_connect = on_connect mc.on_disconnect = on_disconnect mc.on_publish = on_publish mc.connect(conf.hostname, conf.port, 3 * conf.send_interval) mc.loop_start() ready = threading.Event() sender = threading.Thread(target=sensor_reader, args=(connected, ready, sent, mc, sensors), daemon=True) sender.start() while True: inp = input("> ").strip().lower().split() if not inp: print("Please enter a command.") elif inp[0] == "exit": if connected.is_set(): sent.wait() mc.disconnect() exit(0) else: exit(0) elif inp[0] == "update": ready.set() elif inp[0] == "sensors": for i in range(len(sensors)): s = sensors[i] print("{}[{}]Sensor {}: {}".format( "[FAKE]" if s is FakeSensor else "", i, s.id, s.read() )) elif inp[0] == "water": # Huge mess if len(inp) < 2: print("Please enter a sensor number!\t(water <sensor number> <humidity>)") try: id = int(inp[1]) if id < 0 or id >= len(sensors): print("Please enter a valid sensor number, between 0 and {}. Hint: Use the sensors command.".format( len(sensors) - 1 )) elif not sensors[id]["fake"]: print("Sensor", id, "is not fake. You will have to water this plant yourself!") else: fraction = None if len(inp) > 2: fraction = float(inp[2]) if fraction is not None and (fraction < 0 or fraction > 1): print("Please enter a valid fraction for humidity between 0 and 1") else: print("Watered plant to humidity {}.".format(sensors[id].water(fraction))) except ValueError: print("Please enter valid numbers for sensor/fraction!\t(water <sensor number> <humidity>)") elif inp[0] == "help": print("Available commands:") print("help\t-\tShow this help text again.") print("update\t-\tSend a sensor update right now, skipping the usual delay.") print("sensors\t-\tPrint information about existing sensors.") print("water <sensor number> <humidity>\t-\tWater the plant to specified fraction of humidity.", "If humidity is unspecified, default to the good threshold. Only relevant for fake sensors.") print("exit\t-\tStop the sensor system and quit this program.") else: print("Invalid command. Hint: Use the help command.")
import sys from MyAlgorithm import MyAlgorithm from sensors.sensor import Sensor from sensors.grid import Grid from sensors.threadSensor import ThreadSensor from gui.threadGUI import ThreadGUI from gui.GUI import MainWindow from PyQt4 import QtGui import signal signal.signal(signal.SIGINT, signal.SIG_DFL) if __name__ == '__main__': sensor = Sensor() app = QtGui.QApplication(sys.argv) frame = MainWindow() frame.setSensor(sensor) sensor.setGetPathSignal(frame.getPathSig) frame.show() grid = Grid(frame.width(), frame.height(), sensor.WORLDWIDTH, sensor.WORLDHEIGHT) sensor.setGrid(grid) frame.setGrid(grid) algorithm = MyAlgorithm(sensor, grid) t1 = ThreadSensor(sensor, algorithm) t1.daemon = True t1.start()
print('ERROR: python main.py --mapConfig=[map config file] --Ice.Config=[ice file]') sys.exit(-1) app = QApplication(sys.argv) frame = MainWindow() grid = Grid(frame) removeMapFromArgs() ic = EasyIce.initialize(sys.argv) pose = Pose3D (ic, "TeleTaxi.Pose3D") motors = Motors (ic, "TeleTaxi.Motors") vel = Velocity(0, 0, motors.getMaxV(), motors.getMaxW()) frame.setVelocity(vel) sensor = Sensor(grid, pose, True) sensor.setGetPathSignal(frame.getPathSig) frame.setGrid(grid) frame.setSensor(sensor) algorithm = MyAlgorithm(grid, sensor, vel) frame.setAlgorithm(algorithm) frame.show() t1 = ThreadMotors(motors, vel) t1.daemon = True t1.start() t2 = ThreadGUI(frame) t2.daemon = True t2.start()
import sys from MyAlgorithm import MyAlgorithm from sensors.sensor import Sensor from sensors.grid import Grid from sensors.threadSensor import ThreadSensor from gui.threadGUI import ThreadGUI from gui.GUI import MainWindow from PyQt4 import QtGui import signal signal.signal(signal.SIGINT, signal.SIG_DFL) if __name__ == '__main__': sensor = Sensor(); app = QtGui.QApplication(sys.argv) frame = MainWindow() frame.setSensor(sensor) sensor.setGetPathSignal(frame.getPathSig) frame.show() grid = Grid(frame.width(), frame.height(), sensor.WORLDWIDTH, sensor.WORLDHEIGHT) sensor.setGrid(grid) frame.setGrid(grid) algorithm=MyAlgorithm(sensor, grid) t1 = ThreadSensor(sensor,algorithm) t1.daemon=True t1.start()
def imu_kf(self, dimension=1, init_data=0): """ Invokes Kalman filter for 1D and 2D and plots graphs. """ dt_gps = 1.0 dt_speed = 1.0 dt_accel = 1.0 gps_error = 50.0 try: sensor = Sensor() kf = KalmanFilter(error=gps_error) # generate GPS observations. gps_obs, time = sensor.measure(init_data, abs_error=gps_error, unit='position', dt=dt_gps) # estimate position using GPS data est_gps = kf.estimate(observations=gps_obs) # generate speedometer and accelerometer observations. speed_obs, time = sensor.measure(init_data, abs_error=0.1, unit='velocity', dt=dt_speed) accel_obs, time = sensor.measure(init_data, abs_error=0.01, unit='acceleration', dt=dt_accel) # correct postion observations gps_rows = len(est_gps[:, 0]) speed_rows = len(speed_obs[:, 0]) accel_rows = len(accel_obs[:, 0]) corrected_position = np.ones((accel_obs.shape)) k = 0 for i in range(gps_rows): for j in range(int(accel_rows / gps_rows)): corrected_position[k] = est_gps[i] k += 1 k = 0 for i in range(speed_rows): for j in range(int(accel_rows / speed_rows)): corrected_position[k] += speed_obs[i] * dt_speed k += 1 for i in range(accel_rows): corrected_position[i] += accel_obs[i] * dt_accel**2 / 2 # estimate corrected position data est = kf.estimate(observations=corrected_position) # print out results. self.mean, self.median, est_error = self.print_out( corrected_position, est, init_data, dt_accel) # plot results self.plot(obs=corrected_position, est=est, est_error=est_error, time=time, init_data=init_data, dim=dimension, sensor='multisensor') except Exception as e: print('Exception: '.upper(), e) traceback.print_tb(e.__traceback__)