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__)
