Merge remote-tracking branch 'origin/opt-dev'

raspberry-pi/config.ini

@@ -39,6 +39,18 @@
 [mag_sensor]
 
 [opt_sensor]
+  capture_device = -1
+  debug_image = yes
+
+  # distance between center of fiducials in mm
+  target_distance = 55
+
+  # x offset when target is in the center of camera (at offset=50%) in mm
+  x_offset = 195
+
+  # horizontal FoV of the camera in °
+  # RaspiCam datasheet: https://www.raspberrypi.org/documentation/hardware/camera/
+  fov = 53.50
 
 [gui]
   fullscreen = yes

raspberry-pi/gui/graph.py

@@ -75,15 +75,17 @@ class Graph(tk.Canvas):
 
   def update(self, data):
     # load first point of line
-    coord = [[self.pointToCoord(p)] for p in self.lastPoints]
+    coord = [[self.pointToCoord(p)] for p in self.lastPoints if p]
+    newPoints = False
     # append new points
     for i in range(len(data)):
       for point in data[i]:
         coord[i].append(self.pointToCoord(point))
+        self.lastPoints[i] = point
+        newPoints = True
 
     self.canvas = Image.blend(self.canvas, self.bg, 1/100)
-    if len(data[0]) > 0:
-      self.lastPoints = [line[-1] for line in data]
+    if newPoints:
       #fade out old lines by mixing with background
       draw = ImageDraw.Draw(self.canvas)
       for i in range(len(coord)):

raspberry-pi/gui/mainWindow.py

@@ -11,14 +11,14 @@ import logHandler
 
 
 class MainWindow(tk.Frame):
-  def __init__(self, root, ac_sensor, ac_cal_state, conf):
-    self.root = root
-    self.popup = None
-    self.log_window = None
-    self.ac_cal_state = ac_cal_state
-    self.ac_sensor = ac_sensor
+  def __init__(self, root, conf, ac_sensor, opt_sensor):
+    self.root        = root
+    self.conf        = conf
+    self.ac_sensor   = ac_sensor
+    self.opt_sensor  = opt_sensor
     self.log_handler = logHandler.get_log_handler()
-    self.conf = conf
+    self.popup_window       = None
+    self.log_window  = None
 
     tk.Frame.__init__(self, root)
     # data plot at left side
@@ -42,10 +42,23 @@ class MainWindow(tk.Frame):
     tk.Label(self.controls, textvariable=self.ac_dro_t1, anchor="nw").pack(side="top", fill="both", expand=False)
     tk.Label(self.controls, textvariable=self.ac_dro_t2, anchor="nw").pack(side="top", fill="both", expand=False)
 
+    self.opt_dro_val_sums = np.ndarray((4), dtype=np.float)
+    self.opt_dro_val_count = 0
+    self.opt_dro_x = tk.StringVar()
+    self.opt_dro_y = tk.StringVar()
+    self.opt_dro_offset = tk.StringVar()
+    self.opt_dro_size   = tk.StringVar()
+    self.opt_label = tk.Label(self.controls, text="Optical Sensor", anchor="c", font=("Helvatica", 10, 'bold'))
+    self.opt_label.pack(side="top", fill="both", expand=False)
+    tk.Label(self.controls, textvariable=self.opt_dro_x, anchor="nw").pack(side="top", fill="both", expand=False)
+    tk.Label(self.controls, textvariable=self.opt_dro_y, anchor="nw").pack(side="top", fill="both", expand=False)
+    tk.Label(self.controls, textvariable=self.opt_dro_offset, anchor="nw").pack(side="top", fill="both", expand=False)
+    tk.Label(self.controls, textvariable=self.opt_dro_size, anchor="nw").pack(side="top", fill="both", expand=False)
+
     quit_button = tk.Button(self.controls, text="Quit", command=self.root.destroy, height=2, foreground="red")
     quit_button.pack(side="bottom", fill="both")
 
-    calibrate_button = tk.Button(self.controls, text="Calibrate", command=self.calibrate, height=4)
+    calibrate_button = tk.Button(self.controls, text="Calibrate AC", command=self.calibrate_ac, height=4)
     calibrate_button.pack(side="bottom", fill="both")
 
     clear_button = tk.Button(self.controls, text="Clear graph", command=self.graph.clear, height=4)
@@ -66,9 +79,16 @@ class MainWindow(tk.Frame):
         ac_positions.append(data[0:2])
         self.ac_dro_val_sums += data
         self.ac_dro_val_count += 1
-    
+
+    opt_positions = []
+    while self.opt_sensor.queue.qsize() > 0:
+      name, data = self.opt_sensor.queue.get()
+      if name == "data":
+        opt_positions.append(data[0:2])
+        self.opt_dro_val_sums += data
+        self.opt_dro_val_count += 1
     # graph shows all values as a line
-    self.graph.update([ac_positions])
+    self.graph.update([ac_positions, opt_positions])
 
     # update status color
     if self.ac_sensor.dummyActive:
@@ -78,6 +98,14 @@ class MainWindow(tk.Frame):
     else:
       self.ac_label.config(fg="black", bg="yellow")
 
+
+    if not self.opt_sensor.success:
+      self.opt_label.config(fg="white", bg="red")
+    elif len(opt_positions) > 0:
+      self.opt_label.config(fg="white", bg="green")
+    else:
+      self.opt_label.config(fg="black", bg="yellow")
+
     # readouts will only be updated so often
     if self.controlsUpdateTime + 0.4 < time.time():
       self.controlsUpdateTime = time.time()
@@ -95,29 +123,42 @@ class MainWindow(tk.Frame):
       self.ac_dro_val_sums.fill(0)
       self.ac_dro_val_count = 0
 
-    if self.popup:
-      self.popup.update()
+      if self.opt_dro_val_count > 0:
+        self.opt_dro_val_sums /= self.opt_dro_val_count
+      else:
+        self.opt_dro_val_sums.fill(0)
+
+      self.opt_dro_x.set("X: {:.1f} mm".format(self.opt_dro_val_sums[0]))
+      self.opt_dro_y.set("Y: {:.1f} mm".format(self.opt_dro_val_sums[1]))
+      self.opt_dro_offset.set("offset: {:.1f} %".format(self.opt_dro_val_sums[2]*100))
+      self.opt_dro_size.set("size: {:.1f} %".format(self.opt_dro_val_sums[3]*100))
+
+      self.opt_dro_val_sums.fill(0)
+      self.opt_dro_val_count = 0
+
+    if self.popup_window:
+      self.popup_window.update()
     
     if self.log_window:
       self.log_window.update()
 
     self.root.after(30, self.update)
 
-  def calibrate(self):
+  def calibrate_ac(self):
     self.ac_sensor.start_calibration()
-    if not self.popup or not self.pu_root.winfo_exists():
+    if not self.popup_window or not self.pu_root.winfo_exists():
       # create new window
       self.pu_root = tk.Toplevel(self.root)
       self.pu_root.wm_transient(self.root)
-      self.pu_root.wm_title("Calibration")
+      self.pu_root.wm_title("Acoustic Sensor Calibration")
       # make it centered
       x = (self.pu_root.winfo_screenwidth()  - 500) / 2
       y = (self.pu_root.winfo_screenheight() - 200) / 2
       self.pu_root.geometry(f'500x200+{int(x)}+{int(y)}')
       # deactivate mainWindow
       self.pu_root.grab_set()
-      self.popup = CalibrationPopUp(self.pu_root, self.ac_cal_state, self.conf)
-      self.popup.pack(side="top", fill="both", expand=True)
+      self.popup_window = CalibrationPopUp(self.pu_root, self.ac_sensor.calibration_state, self.conf)
+      self.popup_window.pack(side="top", fill="both", expand=True)
 
   def open_log(self):
     #create new window

raspberry-pi/main.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 
 from sensors.acousticSensor import AcousticSensor
-from sensors.calibration import CalibrationStateMashine
+from sensors.opticalSensor import OpticalSensor
 from gui.mainWindow import MainWindow
 
 import configparser
@@ -14,15 +14,16 @@ conf.read('config.ini')
 
 def main():
   log_handler = logHandler.get_log_handler(int(conf['gui']['log_lines']))
-  ac_calibration_state = CalibrationStateMashine()
-  ac_sensor = AcousticSensor(conf, ac_calibration_state)
+  ac_sensor = AcousticSensor(conf)
+  opt_sensor = OpticalSensor(conf)
 
   try:
     ac_sensor.start()
+    opt_sensor.start()
     root = tk.Tk()
     root.title("Tracking System")
     root.attributes('-fullscreen', conf['gui']['fullscreen'] == "yes")
-    view = MainWindow(root, ac_sensor, ac_calibration_state, conf)
+    view = MainWindow(root, conf, ac_sensor, opt_sensor)
     view.pack(side="top", fill="both", expand=True)
     view.update()
     root.mainloop()
@@ -34,6 +35,7 @@ def main():
     traceback.print_exc()
   finally:
     ac_sensor.stop()
+    opt_sensor.stop()
 
 main()
 

raspberry-pi/sensors/acousticSensor.py

@@ -6,6 +6,7 @@ import random
 import traceback
 import queue
 
+from sensors.calibration import CalibrationStateMashine
 from sensors.connection import globalArduinoSlave
 import logHandler
 
@@ -13,10 +14,10 @@ conn = globalArduinoSlave()
 
 
 class AcousticSensor:
-  def __init__(self, conf, calibration_state):
+  def __init__(self, conf):
     self.conf = conf
     self.queue                  = queue.Queue()
-    self.calibration_state      = calibration_state
+    self.calibration_state      = CalibrationStateMashine()
     self.field_height           = float(conf["field"]["y"])
     self.field_width            = float(conf["field"]["x"])
     self.sensor_y_offset        = float(conf["ac_sensor"]["y_offset"])
@@ -31,7 +32,6 @@ class AcousticSensor:
     self.overhead_right         = float(conf["ac_sensor"]["overhead_right"])
 
     self.log_handler = logHandler.get_log_handler()
-    self.log_handler.log_and_print("start acoustic sensor")
 
     # temporary calibration variables
     self.time_vals = [[],[]]
@@ -42,6 +42,7 @@ class AcousticSensor:
     self.n = 0
 
   def start(self):
+    self.log_handler.log_and_print("start acoustic sensor")
     if not conn.isConnected():
       conn.open(port = self.conf["arduino"]["port"])
     conn.addRecvCallback(self._readCb)

raspberry-pi/sensors/opticalSensor.py

@@ -1,13 +1,19 @@
 #!/bin/python3
 
 import time
+import math
 import threading
 import traceback
+import queue
 
 import numpy as np
 import cv2
 import cv2.aruco as aruco
 
+if __name__ != "__main__":
+  from sensors.calibration import CalibrationStateMashine
+  import logHandler
+
 aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50)
 
 
@@ -16,18 +22,20 @@ def saveMarkers():
   image[:,:] = (255)
 
   image[ 50: 50 + 100, 50: 50 + 100] = aruco_dict.drawMarker(0, 100)
-  image[-50 - 100:-50, 50: 50 + 100] = aruco_dict.drawMarker(1, 100)
-  image[ 50: 50 + 100,-50 - 100:-50] = aruco_dict.drawMarker(2, 100)
-  image[-50 - 100:-50,-50 - 100:-50] = aruco_dict.drawMarker(3, 100)
+  image[ 50: 50 + 100,-50 - 100:-50] = aruco_dict.drawMarker(1, 100)
 
   cv2.imwrite("markers.png", image)
 
 parameters =  aruco.DetectorParameters_create()
 
 
-def find_marker(image):
+def find_marker(image, debug=True):
     # Our operations on the frame come here
   gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
+  if debug:
+    image[:,:,0] = gray
+    image[:,:,1] = gray
+    image[:,:,2] = gray
   corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters)
   
   markers = [None] * 4
@@ -38,15 +46,16 @@ def find_marker(image):
   for corner, id in zip(corners, ids.flatten()):
     # draw the bounding box of the ArUCo detection
     c = corner[0]
-    cv2.line(image, tuple(c[0]), tuple(c[1]), (0, 255, 0), 2)
-    cv2.line(image, tuple(c[1]), tuple(c[2]), (0, 255, 0), 2)
-    cv2.line(image, tuple(c[2]), tuple(c[3]), (0, 255, 0), 2)
-    cv2.line(image, tuple(c[3]), tuple(c[0]), (0, 255, 0), 2)
-    cv2.putText(image, str(id), (int(c[0][0]), int(c[0][1]) - 15), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
-
     cX = sum(c[:,0]) / 4
     cY = sum(c[:,1]) / 4
-    cv2.circle(image, (int(cX), int(cY)), 4, (0, 0, 255), -1)
+
+    if debug:
+      cv2.line(image, tuple(c[0]), tuple(c[1]), (0, 255, 0), 2)
+      cv2.line(image, tuple(c[1]), tuple(c[2]), (0, 255, 0), 2)
+      cv2.line(image, tuple(c[2]), tuple(c[3]), (0, 255, 0), 2)
+      cv2.line(image, tuple(c[3]), tuple(c[0]), (0, 255, 0), 2)
+      cv2.putText(image, str(id), (int(c[0][0]), int(c[0][1]) - 15), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
+      cv2.circle(image, (int(cX), int(cY)), 4, (0, 0, 255), -1)
 
     if id < 4:
       markers[id] = (cX, cY)
@@ -54,63 +63,105 @@ def find_marker(image):
   return markers
 
 
-def measureDistances(image):
-    markers = find_marker(image)
+def measureDistances(image, debug=True):
+    markers = find_marker(image, debug)
 
     if markers[0] and markers[1]:
-      cv2.line(image, (int(markers[0][0]),int(markers[0][1])), (int(markers[1][0]),int(markers[1][1])), (255, 0, 0), 2)
       
-      length = np.sqrt((markers[0][0]-markers[1][0])**2 + (markers[0][1]-markers[1][1])**2)
-      middle = abs(markers[0][0]-markers[1][0])
-
-      return middle, length
+      distance = np.sqrt((markers[0][0]-markers[1][0])**2 + (markers[0][1]-markers[1][1])**2)
+      centerX = (markers[0][0]+markers[1][0])/2
+      centerY = (markers[0][1]+markers[1][1])/2
+
+      if debug:
+        cv2.line(image, (int(markers[0][0]),int(markers[0][1])), (int(markers[1][0]),int(markers[1][1])), (255, 0, 0), 2)
+        cv2.line(image, (0, 20), (int(centerX), 20), (255, 0, 0), 2)
+        cv2.line(image, (int(centerX), int(centerY)), (int(centerX), 0), (0, 255, 0), 2)
+        cv2.circle(image, (int(centerX), int(centerY)), 4, (0, 0, 255), -1)
+        cv2.putText(image, "%.2fpx" % (distance), (int(markers[0][0]), int(markers[0][1]+25)), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 255), 3)
+        cv2.putText(image, "%.2fpx" % (centerX) , (25, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 255), 3)
+        
+      return centerX/image.shape[1], distance/image.shape[1]
 
     else:
       return None
 
 
 class OpticalSensor():
-  def __init__(self):
-    self.cap = cv2.VideoCapture(0)
+  def __init__(self, conf):
+    self.conf = conf
+    self.queue                  = queue.Queue()
+    self.calibration_state      = CalibrationStateMashine()
+    self.field_height           = float(conf["field"]["y"])
+    self.field_width            = float(conf["field"]["x"])
+    self.target_distance        = float(conf["opt_sensor"]["target_distance"])
+    self.x_offset               = float(conf["opt_sensor"]["x_offset"])
+    self.fov                    = float(conf["opt_sensor"]["fov"])
+    self.log_handler            = logHandler.get_log_handler()
     self._t  = None
-    self.values = None
+    self.success = False
+    self.running = True
+    self.showImage = True
 
   def start(self):
+    self.log_handler.log_and_print("start optical sensor")
     if not self._t:
       self._t = threading.Thread(target=self._getFrames, args=())
-      self._t.daemon = True  # thread dies when main thread (only non-daemon thread) exits.
       self._t.start()
 
   def _getFrames(self):
-    while True:
-      success, image = self.cap.read()
-      if success:
-        self.values = measureDistances(image)
-        print("opt:", self.values)
-
-  def calibrate(self, x, y):
+    try:
+      dev = int(self.conf["opt_sensor"]["capture_device"])
+    except ValueError:
+      dev = self.conf["opt_sensor"]["capture_device"]
+    self.cap = cv2.VideoCapture(dev)
+
+    while self.running:
+      self.success, image = self.cap.read()
+      if self.success:
+        values = measureDistances(image, self.showImage)
+        if values:
+          self.calibrate(values)
+          position = self.calculate_position(values)
+          if position != None:
+            self.pass_to_gui(position + values)
+        if self.showImage:
+          cv2.imshow("image", image)
+          if cv2.waitKey(1) & 0xFF == ord('q'):
+            self.showImage = False
+            cv2.destroyAllWindows()
+      else:
+        self.log_handler.log_and_print("optical sensor: image capture error")
+        time.sleep(15)
+
+  def calibrate(self, values):
     pass
 
-  def read(self):
-    return self.values
+  def stop(self):
+    self.running = False
+    if self._t:
+      self._t.join()
+    self.log_handler.log_and_print("stop optical sensor")
+
+  def calculate_position(self, values):
+    centerX_perc, distance_perc = values
 
+    visible_width_at_y = self.target_distance / distance_perc
+    x = self.x_offset + visible_width_at_y * (centerX_perc-0.5)
+    alpha = (self.fov/2) / 180 * math.pi
+    beta = (90 - self.fov/2) / 180 * math.pi
+    y = visible_width_at_y/2 / math.sin(alpha) * math.sin(beta)
+
+    return (x, y)
+
+  def pass_to_gui(self, data):
+    self.queue.put(("data", data))
 
 if __name__ == "__main__":
-  # opt = OpticalSensor()
-  # opt.start()
-  # while True:
-  #   time.sleep(1)
-  cap = cv2.VideoCapture(0) 
+  saveMarkers()
+  cap = cv2.VideoCapture(1) 
   while True:
     success, image = cap.read()
-    markers = find_marker(image)
-
-    if markers[0] and markers[1]:
-      cv2.line(image, (int(markers[0][0]),int(markers[0][1])), (int(markers[1][0]),int(markers[1][1])), (255, 0, 0), 2)
-      length = np.sqrt((markers[0][0]-markers[1][0])**2 + (markers[0][1]-markers[1][1])**2)
-
-      cv2.putText(image, "%.2fpx" % (length), (int(markers[0][0]), int(markers[0][1])), cv2.FONT_HERSHEY_SIMPLEX,
-        1.0, (255, 255, 0), 3)
+    print(measureDistances(image))
     cv2.imshow("image", image)
     if cv2.waitKey(1) & 0xFF == ord('q'):
       break