def main():
r = SerialReader(port='/dev/cu.usbmodemFFFFFFFEFFFF1')
r.initialize_connection()
num_data_points = 100
cur_i = 0
time.sleep(2)
while cur_i < num_data_points:
print('3 seconds')
time.sleep(1)
print('2 seconds')
time.sleep(1)
print('1 second')
time.sleep(1)
print('go!')
record_data(reader=r, counter=cur_i, label=args.label)
print('Recorded. Sleeping for 5 seconds..')
time.sleep(2)
cur_i += 1
def __init__(self):
self.timezone = pytz.timezone('Europe/Warsaw')
self.capturer = Capturer()
self.serial_reader = SerialReader()
self.command_executor = CommandExecutor()
self.route_sender = None
self.started = False
self.driving_started = False
def main():
parser = argparse.ArgumentParser()
parser.add_argument("port", help="Serial port to listen for events.")
args = parser.parse_args()
inp = Input()
time.sleep(5)
q = Queue()
s = SerialReader(args.port, q)
s.start()
while True:
try:
item = q.get(block=False)
keycode = BUTTONS.get(item[0], False)
if keycode:
inp.inject_event(keycode, item[1])
except Empty:
pass
except KeyboardInterrupt:
break
s.stop()
def __init__(self, port=SENSOR_TILE_PORT):
self.port = port
self.last_flip_dt = None
self.mouse_positions = [None for x in range(10)]
self.current_mouse_pos_idx = 0
self.last_measurements = dict()
self.measurements_counter = 0
self.listening_for_gesture = False
self.list_acc_x, self.list_acc_y, self.list_acc_z = [], [], []
self.model = joblib.load(MODEL_FILEPATH)
try:
self.r = SerialReader(self.port)
self.r.initialize_connection()
except Exception as e:
print(
'Error initializing serial connection to sensor tile. Error: %s'
% str(e))
print('Stacktrace:')
print(traceback.format_exc())
def update(self):
for p in list(self.readers.keys()):
if not self.readers[p].reader_alive:
print(f"Port {p} Disconnected!")
self.readers.pop(p)
for port in list_ports.comports():
if port.device not in self.readers:
try:
reader = SerialReader(port.device)
except Exception:
print(f"Failed to Connect to {port.device}!")
else:
self.readers[port.device] = reader
print(f"Port {port.device} Connected!")
def main():
parser = argparse.ArgumentParser()
parser.add_argument("port", help="Serial port to listen for events.")
args = parser.parse_args()
inp = Input()
time.sleep(5)
q = Queue()
s = SerialReader(args.port, q)
s.start()
while True:
try:
item = q.get(block=False)
keycode = BUTTONS.get(item[0], False)
if keycode:
inp.inject_event(keycode, item[1])
except Empty:
pass
except KeyboardInterrupt:
break
s.stop()
print(line)
def value_handler(x, values):
for i, val in enumerate(values):
add_data_point(i, x, val)
plot.lazy_redraw(0.2)
def add_data_point(curve_id, x, y):
try:
plot.update_values(curve_id, [x], [y])
except KeyError:
plot.add_curve(curve_id, str(curve_id), [x], [y])
initial_timestamp = time.time()
app = QtGui.QApplication(sys.argv)
plot = RealtimePlotWidget()
reader = SerialReader(SER_PORT, SER_BAUDRATE)
thd = threading.Thread(target=reader.run, args=(value_handler, raw_handler))
thd.daemon = True
thd.start()
plot.redraw()
plot.show()
exit(app.exec_())
def main():
r = SerialReader(port='/dev/cu.usbmodemFFFFFFFEFFFF1')
r.initialize_connection()
actions = []
prev_measurements = dict()
# features 6 from sensor 6 data inference
columns = [
'id',
'r',
'theta',
'phi', # angles
'acc_x',
'acc_y',
'acc_z',
'abs_a', # accelerometer
'g_x',
'g_y',
'g_z',
'abs_g', # gyroscope
'delta_r',
'delta_theta',
'delta_phi', # angles change
'delta_acc_x',
'delta_acc_y',
'delta_acc_z',
'delta_abs_acc', # accelerometer change
'delta_g_x',
'delta_g_y',
'delta_g_z',
'delta_abs_g', # gyroscope change
'label'
]
with open('data.csv', 'w') as csv_file:
writer = csv.DictWriter(csv_file, fieldnames=columns)
writer.writeheader()
counter = 0
recording = False
list_acc_x, list_acc_y, list_acc_z = [], [], []
while True:
counter += 1
measurements = next(r.read_data())
#move = None
angles = measurements['angles']
acc = measurements['accelerations']
gyro = measurements['gyroscope']
delta_r, delta_theta, delta_phi = 0, 0, 0
delta_acc_x, delta_acc_y, delta_acc_z, delta_abs_acc = 0, 0, 0, 0
delta_g_x, delta_g_y, delta_g_z, delta_abs_g = 0, 0, 0, 0
if len(prev_measurements) > 0:
# angles
delta_r = angles['r'] - prev_measurements['angles']['r']
delta_theta = angles['theta'] - prev_measurements['angles']['theta']
delta_phi = angles['phi'] - prev_measurements['angles']['phi']
# accelerometer
delta_acc_x = acc['a_x'] - prev_measurements['accelerations']['a_x']
delta_acc_y = acc['a_y'] - prev_measurements['accelerations']['a_y']
delta_acc_z = acc['a_z'] - prev_measurements['accelerations']['a_y']
delta_abs_acc = acc['abs_a'] - prev_measurements['accelerations'][
'abs_a']
# gyroscope
delta_g_x = gyro['g_x'] - prev_measurements['gyroscope']['g_x']
delta_g_y = gyro['g_y'] - prev_measurements['gyroscope']['g_y']
delta_g_z = gyro['g_z'] - prev_measurements['gyroscope']['g_z']
delta_abs_g = gyro['abs_g'] - prev_measurements['gyroscope'][
'abs_g']
measurements['delta_r'], measurements['delta_theta'], measurements['delta_phi'] = \
str(round(delta_r, 2)), str(round(delta_theta, 2)), str(round(delta_phi, 2))
measurements['delta_acc_x'], measurements['delta_acc_y'], measurements['delta_acc_z'], measurements['delta_abs_acc']= \
str(round(delta_acc_x, 2)), str(round(delta_acc_y, 2)), str(round(delta_acc_z, 2)), str(round(delta_abs_acc, 2))
measurements['delta_g_x'], measurements['delta_g_y'], measurements['delta_g_z'], measurements['delta_abs_g'] = \
str(round(delta_g_x, 2)), str(round(delta_g_y, 2)), str(round(delta_g_z, 2)), str(round(delta_abs_g, 2))
else:
measurements['delta_r'], measurements['delta_theta'], measurements[
'delta_phi'] = 0, 0, 0
measurements['delta_acc_x'], measurements[
'delta_acc_y'], measurements['delta_acc_z'], measurements[
'delta_abs_acc'] = 0, 0, 0, 0
measurements['delta_g_x'], measurements['delta_g_y'], measurements[
'delta_g_z'], measurements['delta_abs_g'] = 0, 0, 0, 0
with open("data.csv", 'a') as csv_file:
writer = csv.DictWriter(csv_file, fieldnames=columns)
info = {
'id': counter,
# sensored data
'r': angles['r'],
'theta': angles['theta'],
'phi': angles['phi'],
'acc_x': acc["a_x"],
'acc_y': acc["a_y"],
'acc_z': acc["a_z"],
'abs_a': acc["abs_a"],
'g_x': gyro["g_x"],
'g_y': gyro["g_y"],
'g_z': gyro["g_z"],
'abs_g': gyro["abs_g"],
# inferenced changed data
'delta_r': delta_r,
'delta_theta': delta_theta,
'delta_phi': delta_phi,
'delta_acc_x': delta_acc_x,
'delta_acc_y': delta_acc_y,
'delta_acc_z': delta_acc_z,
'delta_abs_acc': delta_abs_acc,
'delta_g_x': delta_g_x,
'delta_g_y': delta_g_y,
'delta_g_z': delta_g_z,
'delta_abs_g': delta_abs_g,
'label': 0
}
# add to csv file
print(info['id'])
writer.writerow(info)
# save to the prev
prev_measurements = measurements
# slow-down terminal results printing
time.sleep(0.01)
class MouseMove(object):
def __init__(self, port=SENSOR_TILE_PORT):
self.port = port
self.last_flip_dt = None
self.mouse_positions = [None for x in range(10)]
self.current_mouse_pos_idx = 0
self.last_measurements = dict()
self.measurements_counter = 0
self.listening_for_gesture = False
self.list_acc_x, self.list_acc_y, self.list_acc_z = [], [], []
self.model = joblib.load(MODEL_FILEPATH)
try:
self.r = SerialReader(self.port)
self.r.initialize_connection()
except Exception as e:
print(
'Error initializing serial connection to sensor tile. Error: %s'
% str(e))
print('Stacktrace:')
print(traceback.format_exc())
def calculate_before_flip_mouse_position(self):
return self.mouse_positions[self.current_mouse_pos_idx -
BACK_IN_TIME_TICKS]
def handle_action(self, action):
if action == 'flip':
pre_click_mouse_pos = self.calculate_before_flip_mouse_position()
pyautogui.moveTo(pre_click_mouse_pos.x, pre_click_mouse_pos.y)
if self.last_flip_dt is None:
pyautogui.click()
self.last_flip_dt = datetime.now()
else:
if datetime.now() - self.last_flip_dt > timedelta(seconds=1):
pyautogui.click()
self.last_flip_dt = datetime.now()
def handle_mouse_move(self, angles, acc):
"""
Output relative mouse move position based on angles and accelerations.
"""
if angles['theta'] > THETA_TRESHOLD and acc['a_x'] < -ACC_TRESHOLD:
print('move left')
pyautogui.moveRel(-10, 0)
elif angles['theta'] > THETA_TRESHOLD and acc['a_x'] > ACC_TRESHOLD:
print('move right')
pyautogui.moveRel(10, 0)
elif angles['theta'] > THETA_TRESHOLD and acc['a_y'] < -ACC_TRESHOLD:
print('move up')
pyautogui.moveRel(0, -10)
elif angles['theta'] > THETA_TRESHOLD and acc['a_y'] > 200:
print('move down')
pyautogui.moveRel(0, 10)
def update_last_measurements(self, acc, angles, gyro, measurements):
if len(self.last_measurements) > 0:
delta_r, delta_theta, delta_phi = 0, 0, 0
delta_acc_x, delta_acc_y, delta_acc_z, delta_abs_acc = 0, 0, 0, 0
delta_g_x, delta_g_y, delta_g_z, delta_abs_g = 0, 0, 0, 0
# angles
delta_r = angles['r'] - self.last_measurements['angles']['r']
delta_theta = angles['theta'] - self.last_measurements['angles'][
'theta']
delta_phi = angles['phi'] - self.last_measurements['angles']['phi']
# accelerometer
delta_acc_x = acc['a_x'] - self.last_measurements['accelerations'][
'a_x']
delta_acc_y = acc['a_y'] - self.last_measurements['accelerations'][
'a_y']
delta_acc_z = acc['a_z'] - self.last_measurements['accelerations'][
'a_y']
delta_abs_acc = acc['abs_a'] - self.last_measurements[
'accelerations']['abs_a']
# gyroscope
delta_g_x = gyro['g_x'] - self.last_measurements['gyroscope']['g_x']
delta_g_y = gyro['g_y'] - self.last_measurements['gyroscope']['g_y']
delta_g_z = gyro['g_z'] - self.last_measurements['gyroscope']['g_z']
delta_abs_g = gyro['abs_g'] - self.last_measurements['gyroscope'][
'abs_g']
measurements['delta_r'], measurements['delta_theta'], measurements['delta_phi'] = \
str(round(delta_r, 2)), str(round(delta_theta, 2)), str(round(delta_phi, 2))
measurements['delta_acc_x'], measurements['delta_acc_y'], measurements['delta_acc_z'], measurements['delta_abs_acc']= \
str(round(delta_acc_x, 2)), str(round(delta_acc_y, 2)), str(round(delta_acc_z, 2)), str(round(delta_abs_acc, 2))
measurements['delta_g_x'], measurements['delta_g_y'], measurements['delta_g_z'], measurements['delta_abs_g'] = \
str(round(delta_g_x, 2)), str(round(delta_g_y, 2)), str(round(delta_g_z, 2)), str(round(delta_abs_g, 2))
else:
measurements['delta_r'], measurements['delta_theta'], measurements[
'delta_phi'] = 0, 0, 0
measurements['delta_acc_x'], measurements[
'delta_acc_y'], measurements['delta_acc_z'], measurements[
'delta_abs_acc'] = 0, 0, 0, 0
measurements['delta_g_x'], measurements['delta_g_y'], measurements[
'delta_g_z'], measurements['delta_abs_g'] = 0, 0, 0, 0
self.last_measurements = measurements
def do_the_swipe(self, swipe_direction):
assert type(swipe_direction) == str, 'swipe_direction is not a string.'
pyautogui.keyDown('ctrl')
pyautogui.press(swipe_direction)
pyautogui.keyUp('ctrl')
def reset_internal_state(self):
self.listening_for_gesture = False
self.list_acc_x, self.list_acc_y, self.list_acc_z = [], [], []
def run(self):
while True:
measurements = next(self.r.read_data())
# pyautogui.position() returns <class 'pyautogui.Point'>, e.g. Point(x=1211, y=276)
self.mouse_positions[
self.current_mouse_pos_idx] = pyautogui.position()
self.current_mouse_pos_idx += 1
if self.current_mouse_pos_idx >= len(self.mouse_positions):
self.current_mouse_pos_idx = 0
if measurements.get('action', False):
# handling an action
action = measurements['action']
self.handle_action(action)
if measurements.get('angles', False) and measurements.get(
'accelerations', False) and measurements.get(
'gyroscope', False):
angles = measurements['angles']
acc = measurements['accelerations']
gyro = measurements['gyroscope']
# we are not listening_for_gesture, r > 2000, i.e. previous measurement marked the
# start of the gesture. backfill the list_acc_* with previous measurements
# and also add the current measurement
if angles[
'r'] > GESTURE_STARTED_THRESH and not self.listening_for_gesture:
self.list_acc_x.append(
self.last_measurements['accelerations']['a_x'])
self.list_acc_y.append(
self.last_measurements['accelerations']['a_y'])
# this used to be appending to y also
self.list_acc_z.append(
self.last_measurements['accelerations']['a_z'])
self.list_acc_x.append(acc['a_x'])
self.list_acc_y.append(acc['a_y'])
self.list_acc_z.append(acc['a_z'])
self.listening_for_gesture = True
# we are listening_for_gesture (listening for gesture measurements) and we have not
# accumulated the required 4 measurements yet, save them and keep listening_for_gesture
if self.listening_for_gesture and len(self.list_acc_x) < 4:
self.list_acc_x.append(acc['a_x'])
self.list_acc_y.append(acc['a_x'])
self.list_acc_z.append(acc['a_z'])
# we are listening_for_gesture (listening for gesture measurements) and we have accumulated
# the required 4 measurements. Feed these through a model to get the action.
did_the_action = False
if self.listening_for_gesture and len(self.list_acc_x) == 4:
clean_signal = SignalPreprocessing.extract_clean_signal(
self.list_acc_x, self.list_acc_y, self.list_acc_z)
action = self.model.predict([clean_signal])
print('Received Action: %s' % (action))
if action == 1:
self.do_the_swipe('right')
elif action == 2:
self.do_the_swipe('left')
else:
print('Unknown action.')
# action completed, reset
self.reset_internal_state()
did_the_action = True
if not self.listening_for_gesture and not did_the_action:
# move the mouse if not listening for gesture and this measurement was not a final
# one of the four for the gesture.
self.handle_mouse_move(angles, acc)
self.update_last_measurements(acc, angles, gyro, measurements)
def post_data(_data: list, post_url=RELEASE_URL):
"""
将数据以POST请求发送给云函数
"""
_r = requests.post(url=post_url, json=_data)
log.info("send %d at %f", _r.status_code, time.time())
if __name__ == '__main__':
from multiprocessing.connection import Pipe
from multiprocessing import Process
SERIAL_PIPE = Pipe()
serial_reader = SerialReader(_serial_pipe_sender=SERIAL_PIPE[0],
_serial_port='/dev/ttyUSB0', # tty.usbserial-0001
_baud_rate=921600)
p = Process(target=serial_reader.assembly_serial_data,
args=())
p.start()
data = []
MAX_LEN = 1024
while True:
line: str = SERIAL_PIPE[1].recv()
line = line.strip()
if 'CSI_DATA' not in line:
log.info(line)
continue
if not line.startswith('CSI_DATA'):
line = line[1:]
if not line.startswith('CSI_DATA'):
class CarServer(object):
def directory_for_session(self):
directory = "session-%s" % self.timestamp()
images = "%s/images" % directory
os.makedirs(images)
return directory
def __init__(self):
self.timezone = pytz.timezone('Europe/Warsaw')
self.capturer = Capturer()
self.serial_reader = SerialReader()
self.command_executor = CommandExecutor()
self.route_sender = None
self.started = False
self.driving_started = False
@cherrypy.expose
def idle(self):
self.command_executor.change_status(Status.IDLE)
@cherrypy.expose
def remote(self):
self.command_executor.change_status(Status.REMOTE)
@cherrypy.expose
def learning(self):
self.command_executor.change_status(Status.LEARNING)
def autonomous(self):
self.command_executor.change_status(Status.AUTONOMOUS)
@cherrypy.expose
def speed(self, value):
self.command_executor.set_speed(int(value))
@cherrypy.expose
def turn(self, angle):
self.command_executor.make_turn(int(angle))
@cherrypy.expose
def replay(self, directory):
self.driving_started = True
directions = ReplayDirectionsProvider(directory)
self.route_sender = RouteSender(self.command_executor, directions)
self.autonomous()
@cherrypy.expose
@cherrypy.tools.json_out()
def sessions(self):
sessions_provider = SessionsProvider(os.getcwd())
return sessions_provider.get_sessions()
@cherrypy.expose
def drive(self):
self.driving_started = True
directions = CameraDirectionsProvider()
initialized_queue = Queue()
self.route_sender = RouteSender(self.command_executor, directions,
initialized_queue)
print("Waiting for initialization")
if initialized_queue.get():
print("Initialization completed")
self.autonomous()
@cherrypy.expose
def start(self):
if self.started:
cherrypy.response.status = 400
return "WARNING: Session already started"
if self.driving_started:
cherrypy.response.status = 400
return "WARNING: Driving in progress"
directory = self.directory_for_session()
self.started = True
self.capturer.start(directory)
self.serial_reader.start_saving(directory)
self.remote()
return "INFO: Session %s has been started" % directory
@cherrypy.expose
def stop(self):
if not self.started and not self.driving_started:
cherrypy.response.status = 400
return "WARNING: Neither session nor driving started"
self.idle()
if self.started:
self.cleanup()
return "INFO: Session ended successfully"
else:
self.stop_driving()
return "INFO: Driving ended successfully"
def stop_driving(self):
self.driving_started = False
self.route_sender.terminate()
self.route_sender = None
def terminate(self):
self.cleanup()
self.capturer.terminate()
self.serial_reader.terminate()
def cleanup(self):
if not self.started:
return
self.capturer.stop()
self.serial_reader.stop_saving()
self.started = False
def timestamp(self):
utc_dt = datetime.datetime.now(pytz.utc)
loc_dt = utc_dt.astimezone(self.timezone)
return loc_dt.strftime("%Y%m%d%H%M%S")
"""
start program for client side application
"""
import _thread
from serial_reader import SerialReader
from tcp_client import TCPClient
SERIAL_PORT = '/dev/tty.usbserial-0001'
BAUD_RATE = 921600
if __name__ == '__main__':
serial_reader = SerialReader(_serial_port=SERIAL_PORT,
_baud_rate=BAUD_RATE)
_thread.start_new_thread(serial_reader.assembly_serial_data, ())
tcp_client = TCPClient('127.0.0.1', 5000)
tcp_client.send_serial_data()
def _monitor():
with SerialReader() as r:
unix_reader[0] = r
monitor(self, r)
self.lcd_canvas.draw()
if __name__ == '__main__':
global DEBUG, BPP
args = docopt(__doc__)
BPP = False
if args['-b']:
BPP = True
if args['--record']:
with open(RECORD_FILE, 'wb') as f:
f.write('')
with SerialReader() as r:
read_record(r)
else:
DEBUG = False
if args['--debug']:
DEBUG = True
tk = tkinter.Tk()
tk.minsize(width=FRAME_WIDTH, height=FRAME_HEIGHT)
tk.maxsize(width=FRAME_WIDTH, height=FRAME_HEIGHT)
tk.resizable(width=tkinter.FALSE, height=tkinter.FALSE)
app = AppFrame(parent=tk)
if DEBUG:
print('Running in DEBUG mode, reading from UNIX Socket')
# method used with the camera_btn to start and stop reading from the serial reader
def camera_command():
if not data.camera_recording:
data.start_camera()
data.camera_text.set('STOP')
else:
data.stop_camera()
data.camera_text.set('START')
root = Tk()
root.attributes('-fullscreen', True)
data = SerialReader()
frame = Frame(root, width=1920, height=1080)
frame.pack()
# place the background image
background_image = PhotoImage(file='background_image.png')
background_image_label = Label(frame, image=background_image)
background_image_label.place(x=0, y=0, relwidth=1, relheight=1)
# place all of the text
alt_label = Label(frame, fg='#000000', bg='#ffffff', textvariable=data.alt, font=('Major Mono Display', 30), justify=CENTER)
alt_label.place(relx=.465, rely=.195, anchor='center')
speed_label = Label(frame, fg='#000000', bg='#ffffff', textvariable=data.speed, font=('Major Mono Display', 30))
speed_label.place(relx=.818, rely=.538, anchor='center')
import wx
import os
import sys
import wx.lib.agw.advancedsplash as AS
from graph_frame import GraphFrame
from serial_reader import SerialReader
SPLASH_FN = "splashscreen.png"
SPLASH_TIME = 3000
if __name__ == "__main__":
app = wx.App(0)
bitmap = wx.Bitmap(SPLASH_FN, wx.BITMAP_TYPE_PNG)
shadow = wx.WHITE
splash = AS.AdvancedSplash(None,
bitmap=bitmap,
timeout=SPLASH_TIME,
agwStyle=AS.AS_TIMEOUT | AS.AS_CENTER_ON_PARENT
| AS.AS_SHADOW_BITMAP,
shadowcolour=shadow)
data_source = SerialReader()
app.frame = GraphFrame(data_source)
app.frame.Show()
app.MainLoop()
help="Device ('left' or 'right'")
cli.verbose(parser)
args = vars(parser.parse_args())
# Setup logging
setup_logging(level=args[LOG_LEVEL])
userdata = {
TOPIC: "lidar/{0}/mm".format(args[DEVICE]),
COMMAND: "lidar/{0}/command".format(args[DEVICE]),
ENABLED: True,
MOVING_AVERAGE: MovingAverage(size=3),
OOR_VALUES: OutOfRangeValues(size=args[OOR_SIZE]),
OOR_TIME: args[OOR_TIME],
OOR_UPPER: args[OOR_UPPER]
}
with SerialReader(func=fetch_data,
userdata=userdata,
port=SerialReader.lookup_port(args[DEVICE_ID])
if args.get(DEVICE_ID) else args[SERIAL_PORT],
baudrate=args[BAUD_RATE],
debug=True):
with MqttConnection(hostname=args[MQTT_HOST],
userdata=userdata,
on_connect=on_connect,
on_message=frc_utils.on_message):
waitForKeyboardInterrupt()
logger.info("Exiting...")
parser = argparse.ArgumentParser()
cli.grpc_port(parser)
cli.device_id(parser)
cli.serial_port(parser)
cli.baud_rate(parser)
cli.oor_size(parser)
cli.oor_time(parser)
cli.oor_upper(parser)
cli.verbose(parser)
args = vars(parser.parse_args())
# Setup logging
setup_logging(level=args[LOG_LEVEL])
# Start up a server to expose the metrics.
start_http_server(8000)
with DistanceServer(grpc_port=args[GRPC_PORT],
oor_size=args[OOR_SIZE],
oor_time=args[OOR_TIME],
oor_upper=args[OOR_UPPER]) as server:
with SerialReader(
func=server.fetch_data,
userdata=None,
port=SerialReader.lookup_port(args[DEVICE_ID] if args.get(
DEVICE_ID) else args[SERIAL_PORT]),
baudrate=args[BAUD_RATE]):
waitForKeyboardInterrupt()
logger.info("Exiting...")