feat: add robot_data in websocket packet, send all robot data (#14)

models/can_packet.py

@@ -1,4 +1,4 @@
-from models.interfaces import DisanceSensor, Position, RobotStatus, Velocity
+from models.interfaces import DistanceSensor, Position, RobotStatus, Velocity
 
 CAN_IDS = {
     'ROBOT_POSITION': 0x3E3,
@@ -44,7 +44,7 @@
     "status_display": 20,
 }
 
-CAN_distance_sensor: DisanceSensor = {
+CAN_distance_sensor: DistanceSensor = {
     "sensor": 1,
     "distance": 10,
     "alarm": 2,

models/interfaces.py

@@ -20,7 +20,8 @@ class RobotStatus(TypedDict):
     robot_selected: int
     status_display: int
 
-class DisanceSensor(TypedDict):
+
+class DistanceSensor(TypedDict):
     sensor: int
     distance: int
     alarm: int

robot/robot.py

@@ -6,8 +6,11 @@
 from can import Message
 from can.interface import Bus
 
+from time import sleep
+
+# from models.socket import position_mocks
 from models.can_packet import CAN_FORMATS, CAN_IDS, MOTION_CMDS
-from models.interfaces import Position
+from models.interfaces import DistanceSensor, Position, RobotStatus
 from models.lidar_mock import SCANDATA_MOCKS
 from robot.config import (
     CAN_BAUD,
@@ -22,6 +25,7 @@
 from robot.motion_command import MotionCommand
 
 
+# pylint: disable=too-many-instance-attributes
 class Robot:
     def __init__(self):
         # Initialize the CAN bus
@@ -33,10 +37,20 @@ def __init__(self):
         self.laser_data = []
         self.laser: Any = None
 
-        # Initialize the starting and current positions of the robot
+        # init robot values with placeholders
         self.StartPosition: Position = { "X": -1000, "Y": -1000, "Angle": 0, "Flags": 0, "Bumpers": 0 }
         self.Position: Position = { "X": 0, "Y": 0, "Angle": 0, "Flags": 0, "Bumpers": 0 }
 
+        self.linear_speed = 0
+
+        self.robot_status: RobotStatus = {
+            "robot_selected": 0,
+            "status_display": 0,
+        }
+
+        # tof alarms data
+        self.distance_sensor: DistanceSensor = {"sensor": 0, "distance": 0, "alarm": 0}
+
     def on_data_received(self, frm: Message) -> None:
         """
         Handle the data received from the CAN bus depending on the CAN ID.
@@ -61,7 +75,6 @@ def on_data_received(self, frm: Message) -> None:
         elif frm.arbitration_id == CAN_IDS["DISTANCE_SENSOR"]:
             self.__handle_distance_sensor(data)
 
-
     def __handle_position(self, data: bytearray) -> None:
         posX, posY, angle, flags, bumpers = struct.unpack(CAN_FORMATS["POSITION"], data)
 
@@ -83,34 +96,49 @@ def __handle_position(self, data: bytearray) -> None:
         if DEBUG_CAN: # debug can viene usata per scopi diversi (switch da virtual a socket / logging), forse serve un altro flag
             print(f"Position: [X: {posX}, Y: {posY}, A: {angle}]")
 
-
     def __handle_speed(self, data: bytearray) -> None:
-        linear_speed = struct.unpack(CAN_FORMATS["VELOCITY"], data)
+        linear_speed, _ = struct.unpack(CAN_FORMATS["VELOCITY"], data)
+        self.linear_speed = linear_speed  # type: ignore
 
         if DEBUG_CAN:
             print('linear_speed', linear_speed)
 
-
     def __handle_robot_status(self, data: bytearray) -> None:
         robot_selected, status_display = struct.unpack(CAN_FORMATS["ROBOT_STATUS"], data)
 
+        self.robot_status["robot_selected"] = robot_selected
+        self.robot_status["status_display"] = status_display
+
         if DEBUG_CAN:
             print('robot_selected', robot_selected)
             print('status_display', status_display)
 
     def __handle_distance_sensor(self, data: bytearray) -> None:
         sensor, distance, alarm = struct.unpack(CAN_FORMATS["DISTANCE_SENSOR"], data)
+        self.distance_sensor = {"sensor": sensor, "distance": distance, "alarm": alarm}
 
         if DEBUG_CAN:
             print('sensor', sensor)
             print('distance', distance)
             print('alarm', alarm)
 
-
-
+    # currently unused
     def get_position(self) -> Position:
         return self.Position
 
+    def get_robot_data(self) -> dict:
+        # for debugging
+        # index = randrange(0, len(position_mocks))
+        # self.Position = position_mocks[index]
+
+        robot_data = {
+            **self.Position,
+            "linear_speed": self.linear_speed,
+            **self.robot_status,
+            **self.distance_sensor,
+        }
+        return robot_data
+
     def set_position(self, position: Position) -> None:
         """
         Set the position of the robot using a Position object.
@@ -157,14 +185,20 @@ def get_lidar_data(self) -> List[int]:
         """
         if DEBUG_VIRTUAL or DEBUG_VCAN:
             index = randrange(0, len(SCANDATA_MOCKS))
-            laser_data = SCANDATA_MOCKS[index]
+            self.laser_data = SCANDATA_MOCKS[index]
         else:
-            laser_data = self.laser.getScan()
-
-        if laser_data:
+            try:
+                self.laser_data = self.laser.getScan()
+            # pylint: disable=bare-except
+            except:
+                # to.do: we could put this in a separate thread
+                print("ERROR reading LiDAR data, waiting 10 seconds")
+                sleep(10)
+
+        if self.laser_data:
             if DEBUG_LIDAR:
-                print(laser_data)
-            return laser_data
+                print(self.laser_data)
+            return self.laser_data
 
         return []
 

webserver/websocket.py

@@ -21,7 +21,7 @@ async def broadcast():
     while True:
         # print(f"clients = {len(CLIENTS)}")
         websockets.broadcast(CLIENTS, get_lidar_data())
-        websockets.broadcast(CLIENTS, get_robot_position())
+        websockets.broadcast(CLIENTS, get_robot_data())
         await asyncio.sleep(1) # delay
 
 
@@ -37,9 +37,10 @@ def get_lidar_data() -> str:
     }
     return json.dumps(laser_data_msg)
 
-def get_robot_position() -> str:
+
+def get_robot_data() -> str:
     robot_position_msg = {
-        "type": "position",
-        "data": robot.get_position(),
+        "type": "robot_data",
+        "data": robot.get_robot_data(),
     }
     return json.dumps(robot_position_msg)