Gl/swarinfo 638 visualisation tool (#3)

Author: g-lesssard

README.md

@@ -0,0 +1,22 @@
+# Python Test Bench for the SwarmUS Platform
+
+
+## Using the graphical tool
+
+This code repository boasts a graphical tool to visualise the interlocalisation feature. 
+To use the tool, you will need one stationary HiveBoard with its BeeBoards and at least one mobile
+HiveBoard/BeeBoard assembly.
+
+The stationary BeeBoard must be powered by the barrel connector (**not** by USB). Plug the Micro-USB cable to your 
+computer for the serial connection.
+
+Next, start the mobile HiveBoard/BeeBoard assemblies.
+
+To start the tool, start the `main.py` file under `src/visualisation_tool`. By default, the visualisation tool will
+use the com port `/dev/ttyACM0`. To edit this value, edit the `COM_PORT` variable in `DataUpdater.py`.
+
+The visualisation tool will open a window with an orange dot at the center, representing the stationary HiveBoard. The 
+mobile HiveBoards will show as coloured circles and their position will be updated at regular intervals. Specific
+neighbors can be hidden by unchecking the 'visible' check box in the table underneath the graph.
+
+![](img/visualisation_tool.png)

img/visualisation_tool.png

@@ -44,6 +44,12 @@ def set_neighbor_list_callback(self, callback):
     def set_neighbor_position_callback(self, callback):
         self.neighbor_position_callback = callback
 
+    def set_neighbor_list_callback(self, callback):
+        self.neighbors_list_callback = callback
+
+    def set_neighbor_position_callback(self, callback):
+        self.neighbor_position_callback = callback
+
     def kill_receiver(self):
         self._run = False
         self._proto_stream.kill_stream()
@@ -156,6 +162,42 @@ def send_get_neighbor_position_request(self, destination: int, neighbor_id: id):
 
         self._proto_stream.write_message_to_stream(msg)
 
+    def send_get_neighbors_request(self, destination: int):
+        neighbors_request = GetNeighborsListRequest()
+
+        hivemind_api_request = HiveMindHostApiRequest()
+        hivemind_api_request.neighbors_list.CopyFrom(neighbors_request)
+
+        request = Request()
+        request.hivemind_host.CopyFrom(hivemind_api_request)
+
+        msg = Message()
+        msg.source_id = self.uuid
+        msg.destination_id = destination
+        msg.request.CopyFrom(request)
+
+        self._proto_stream.write_message_to_stream(msg)
+
+    def send_get_neighbor_position_request(self, destination: int, neighbor_id: id):
+        neighbor_position_request = GetNeighborRequest()
+        neighbor_position_request.neighbor_id = neighbor_id
+
+        hivemind_host_api_request = HiveMindHostApiRequest()
+        hivemind_host_api_request.neighbor.CopyFrom(neighbor_position_request)
+
+        request = Request()
+        request.hivemind_host.CopyFrom(hivemind_host_api_request)
+
+        msg = Message()
+        msg.source_id = self.uuid
+        msg.destination_id = destination
+        msg.request.CopyFrom(request)
+
+        self._proto_stream.write_message_to_stream(msg)
+
+
+
+
 
 
 

src/hiveboard/HiveBoard.py

@@ -2,7 +2,8 @@
 import time
 
 from Graph2D import Graph2D
-from PyQt5.QtCore import QObject, QThread, pyqtSignal
+from NeighborCoordinateTable import NeighborCoordinateTable
+from PyQt5.QtCore import QObject, QThread, pyqtSignal, pyqtSlot
 import numpy as np
 import sys
 
@@ -17,19 +18,23 @@
 
 
 class DataUpdater(QObject):
-    new_point = pyqtSignal(int, float, float)
+    new_cartesian_point = pyqtSignal(int, float, float)
+    new_polar_point = pyqtSignal(int, float, float)
     received_greeting = pyqtSignal(int)
 
-    def __init__(self, graph: Graph2D):
+    def __init__(self, graph: Graph2D, table: NeighborCoordinateTable):
         super().__init__()
 
         self.graph = graph
-        self.new_point.connect(self.graph.update_point)
+        self.neighbor_table = table
+        self.new_cartesian_point.connect(self.graph.update_point)
+        self.new_polar_point.connect(self.neighbor_table.update_neighbor)
         self.hiveboard = None
         self.hiveboard_connnected = False
         self.target_agent_id = 0
 
         self.neighbor_list = []
+        self.neighbor_table.hide_neighbors.connect(self.graph.hide_neighbors)
 
         self.start_connection_thread()
         self.start_greeting_thread()
@@ -75,11 +80,11 @@ def handle_neighbor_list(self, neighbor_list):
         print(f"New neighbor list is {self.neighbor_list}")
 
     def handle_neigbor_update(self, neighbor):
-        y = -neighbor.position.distance * np.cos(neighbor.position.azimuth / 180 * np.pi)
-        x = neighbor.position.distance * np.sin(neighbor.position.azimuth / 180 * np.pi)
         neighbor_id = neighbor.neighbor_id
-        self.new_point.emit(neighbor_id, x, y)
-        print(f"Agent {neighbor_id} now at ({x},{y})")
+        self.new_polar_point.emit(neighbor_id, neighbor.position.distance, neighbor.position.azimuth)
+        y = neighbor.position.distance * np.cos(neighbor.position.azimuth / 180 * np.pi)
+        x = neighbor.position.distance * np.sin(neighbor.position.azimuth / 180 * np.pi)
+        self.new_cartesian_point.emit(neighbor_id, x, y)
 
     def request_neighbors_update(self):
         while True:
@@ -91,4 +96,4 @@ def request_neighbors_update(self):
                 for neighbor_id in self.neighbor_list:
                     self.hiveboard.send_get_neighbor_position_request(destination=self.target_agent_id,
                                                                       neighbor_id=neighbor_id)
-            time.sleep(0.1)
+            time.sleep(0.2)

src/visualistaion_tool/DataUpdater.py src/visualisation_tool/DataUpdater.pysrc/visualistaion_tool/DataUpdater.py renamed to src/visualisation_tool/DataUpdater.py

@@ -0,0 +1,71 @@
+from PyQt5 import QtWidgets
+from PyQt5.QtCore import pyqtSlot
+from pyqtgraph import PlotWidget, plot, GridItem, LegendItem
+import pyqtgraph as pg
+
+COLOR_OFFSET = 15
+
+
+class Graph2D(QtWidgets.QWidget):
+
+    def __init__(self, parent):
+        super(Graph2D, self).__init__(parent)
+        self.layout = QtWidgets.QGridLayout()
+        self.setLayout(self.layout)
+        self.scatters = {}
+        self.hidden_neighbors = []
+
+        self.create_2d_plot()
+        self.create_grid()
+        self.create_scatter_element()
+
+    def create_2d_plot(self):
+        self.graphWidget = pg.plot()
+        #self.graphWidget.setBackground("w")
+        self.graphWidget.hideAxis('bottom')
+        self.graphWidget.hideAxis('left')
+        self.graphWidget.setXRange(-7, 7)
+        self.graphWidget.setYRange(-7, 7)
+        self.graphWidget.setMouseEnabled(x=False, y=False)
+        self.graphWidget.addLegend()
+        self.layout.addWidget(self.graphWidget)
+
+    def create_grid(self):
+        self.grid = GridItem(pen='white', textPen='white')
+        self.graphWidget.addItem(self.grid)
+
+    def create_scatter_element(self):
+        self.base = pg.ScatterPlotItem()
+        base_symbol = {'pos': [0, 0],
+                       'pen': {'color': 'w', 'width': 1},
+                       'brush': pg.intColor(10, 100),  # Orange
+                       'symbol': 'd',
+                       'size': 30}
+        self.base.addPoints([base_symbol])
+        self.graphWidget.addItem(self.base)
+
+    @pyqtSlot(int, float, float)
+    def update_point(self, neighbor_id: int, x: float, y: float):
+        if neighbor_id not in self.scatters.keys() and neighbor_id not in self.hidden_neighbors:
+            self.scatters.update({neighbor_id: {"scatter": pg.ScatterPlotItem(name=f"agent {neighbor_id}",
+                                                                              brush=pg.intColor(neighbor_id * COLOR_OFFSET, 100)),
+                                                "spot": {}}})
+            self.graphWidget.addItem(self.scatters[neighbor_id]["scatter"])
+        if neighbor_id not in self.hidden_neighbors:
+            self.scatters[neighbor_id]["spot"] = {'pos': [x, y],
+                                                  'pen': {'color': 'w', 'width': 1},
+                                                  # 100 is the number int values for the color spectrum,
+                                                  # COLOR_OFFSET is the offset to apply to have 6 different values for points
+                                                  'brush': pg.intColor(neighbor_id * COLOR_OFFSET, 100),
+                                                  'size': 20}
+            self.scatters[neighbor_id]["scatter"].setData(spots=[self.scatters[neighbor_id]["spot"]],
+                                                          name=f"Agent {neighbor_id}")
+
+    @pyqtSlot(list)
+    def hide_neighbors(self, hidden_neighbors: list):
+        self.hidden_neighbors = hidden_neighbors
+        for neighbor_id in self.hidden_neighbors:
+            if neighbor_id in self.scatters.keys():
+                element = self.scatters.pop(neighbor_id)
+                self.graphWidget.removeItem(element["scatter"])
+

src/visualistaion_tool/DummyHiveBoard.py src/visualisation_tool/DummyHiveBoard.pysrc/visualistaion_tool/DummyHiveBoard.py renamed to src/visualisation_tool/DummyHiveBoard.py

@@ -3,47 +3,54 @@
 
 from PyQt5.QtWidgets import *
 from Graph2D import Graph2D
-from PyQt5.QtCore import QThread, pyqtSlot
+from PyQt5.QtCore import QThread, pyqtSlot, Qt, pyqtSignal
 from pyqtgraph import PlotWidget, plot
 import pyqtgraph as pg
 import numpy as np
+from NeighborCoordinateTable import NeighborCoordinateTable
 from DataUpdater import DataUpdater
 
 
 class MainWindow(QMainWindow):
 
+    refresh_neighbor = pyqtSignal()
+
     def __init__(self):
         super(MainWindow, self).__init__()
         self.target_agent_id = None
 
         # Qt shenaninigans
         self.setWindowTitle("SwarmUS Interlocalisation Visualisation Tool")
-        self.container = QWidget()
-        self.main_layout = QVBoxLayout()
-        self.container.setLayout(self.main_layout)
+        self.container = QSplitter(Qt.Vertical)
         self.setCentralWidget(self.container)
 
         # Create UI elements here
         self.create_2d_graph_area()
         self.create_menu_bar()
+        self.create_neighbor_table()
 
         # Add backend elements here
         self.start_data_acquisition()
         self.data_updater.received_greeting.connect(self.update_target_agent_from_greeting)
 
     def create_menu_bar(self):
         menu_bar = self.menuBar()
-        config_menu = QMenu("&Configuration", self)
-        target_agent_menu = QMenu("Target Agent", self)
+
+        target_agent_menu = QMenu("&Target Agent", self)
         self.checkable_agents = []
         for i in range(1, 7):
             agent = QAction(f"Agent {i}", self)
             agent.setCheckable(True)
             target_agent_menu.addAction(agent)
             self.checkable_agents.append(agent)
             agent.triggered.connect(self.update_target_agent_from_config)
-        config_menu.addMenu(target_agent_menu)
-        menu_bar.addMenu(config_menu)
+
+        refresh_neighbors = QAction("&Refresh neighbor list", self)
+        refresh_neighbors.triggered.connect(self.refresh_neighbor_list)
+        self.refresh_neighbor.emit()
+
+        menu_bar.addMenu(target_agent_menu)
+        menu_bar.addAction(refresh_neighbors)
 
     @pyqtSlot(bool)
     def update_target_agent_from_config(self, checked: bool):
@@ -57,7 +64,7 @@ def update_target_agent_from_config(self, checked: bool):
                     new_agent_id = self.checkable_agents.index(agent) + 1
             self.target_agent_id = new_agent_id
             self.data_updater.target_agent_id = self.target_agent_id
-            self.data_updater.neighbor_list.clear()
+            del self.data_updater.neighbor_list[:]
             print(f"New target agent is {self.target_agent_id} based on configuration set")
 
     @pyqtSlot(int)
@@ -69,12 +76,22 @@ def update_target_agent_from_greeting(self, agent_id):
         self.data_updater.target_agent_id = self.target_agent_id
         print(f"New target agent is {self.target_agent_id} based on greeting received")
 
+    @pyqtSlot()
+    def refresh_neighbor_list(self):
+        self.data_updater.hiveboard.send_get_neighbors_request(self.target_agent_id)
+        self.neighbor_table.clear_table()
+
     def create_2d_graph_area(self):
         self.graphWidget = Graph2D(self)
-        self.main_layout.addWidget(self.graphWidget)
+        self.container.addWidget(self.graphWidget)
+
+    def create_neighbor_table(self):
+        self.neighbor_table = NeighborCoordinateTable(0, 5)
+        self.container.addWidget(self.neighbor_table)
+        self.refresh_neighbor.connect(self.neighbor_table.clear_table)
 
     def start_data_acquisition(self):
-        self.data_updater = DataUpdater(self.graphWidget)
+        self.data_updater = DataUpdater(self.graphWidget, self.neighbor_table)
         self.data_updater_thread = QThread()
         self.data_updater.moveToThread(self.data_updater_thread)
         self.data_updater_thread.started.connect(self.data_updater.request_neighbors_update)