enhance MQTT handling and configuration; add slot_id and device_id to config

robot-control/config/config.yaml

@@ -4,20 +4,26 @@ measurement_devices:
     slots:
       - position: [10.0, 20.0, 50.0]
         occupied: False
+        slot_id: 0
       - position: [20.0, 20.0, 50.0]
         occupied: False
+        slot_id: 1
       - position: [30.0, 20.0, 50.0]
         occupied: False
+        slot_id: 2
 
   - id: "device_2"
     position: [100.0, 300.0, 50.0]
     slots:
       - position: [10.0, 20.0, 50.0]
         occupied: False
+        slot_id: 0
       - position: [20.0, 20.0, 50.0]
         occupied: False
+        slot_id: 1
       - position: [30.0, 20.0, 50.0]
         occupied: False
+        slot_id: 2
 
 feeder:
   position: [0.0, 0.0, 50.0]

robot-control/src/api/routes.py

@@ -28,6 +28,10 @@ class MQTTHandler:
         self.client.on_connect = self.on_connect
         self.client.on_message = self.on_message
         
+        if broker == "debug":
+            self.client.connect("test.mosquitto.org", 1883)
+            return
+        
         self.client.connect(broker, port, 60)
         self.client.loop_start()
 
@@ -72,7 +76,7 @@ class MQTTHandler:
                     
             elif topic.startswith("soa/"):
                 logger.info(f"SOA update for device {device_id}: {payload}")
-                # Handle SOA update here
+                # [SG]TODO: Handle SOA update here
                 
         except Exception as e:
             logger.error(f"Error processing message: {e}")

robot-control/src/main.py

@@ -2,7 +2,6 @@ import asyncio
 from fastapi import FastAPI
 from robot.controller import RobotController, Cell
 from robot.config import ConfigParser
-from api.routes import app as api_app
 from vision.datamatrix import DataMatrixReader
 
 class LoaderSystem:
@@ -10,7 +9,6 @@ class LoaderSystem:
         self.config = ConfigParser()
         self.controller = RobotController(self.config)
         self.vision = DataMatrixReader()
-        self.api = api_app
 
     async def run(self):
         while True:

robot-control/src/robot/config.py

@@ -7,6 +7,8 @@ from pathlib import Path
 class SlotConfig:
     position: Tuple[float, float, float]
     occupied: bool
+    slot_id: int
+    device_id: str
     cell_id: str = None
 
 @dataclass
@@ -47,6 +49,8 @@ class ConfigParser:
                 SlotConfig(
                     position=tuple(slot['position']),
                     occupied=slot['occupied'],
+                    slot_id=slot['slot_id'],
+                    device_id=device['id'],
                     cell_id=slot['cell_id'] if slot['occupied'] else None,
                 )
                 for slot in device['slots']

robot-control/src/robot/controller.py

@@ -39,6 +39,12 @@ class RobotController:
         self.movement = RobotMovement()
         self.movement.set_speed(self.system_settings.speed)
         
+        # Initialize with configured values
+        self.total_slots = sum(len(device.slots) for device in self.devices)
+        self.work_queue: List[SlotConfig] = []
+
+        self.gripper_occupied = False
+        
         # Initialize MQTT handler
         mqtt_config = self.config.get_mqtt_config()  # Add this to config parser
         self.mqtt_handler = MQTTHandler(
@@ -53,12 +59,6 @@ class RobotController:
             self.mqtt_handler.register_device(
                 Device(device_id=device.id, num_slots=len(device.slots))
             )
-        
-        # Initialize with configured values
-        self.total_slots = sum(len(device.slots) for device in self.devices)
-        self.work_queue: List[SlotConfig] = []
-
-        self.gripper_occupied = False
 
     async def cleanup(self):
         """Cleanup resources on shutdown"""
@@ -92,10 +92,6 @@ class RobotController:
             self.gripper_occupied = False
             return False
 
-    async def read_cell_id(self):
-        # Use vision system to read data matrix and return cell ID
-        return None
-
     def get_next_free_slot(self):
         for device in self.devices:
             for slot in device.slots:
@@ -133,18 +129,29 @@ class RobotController:
             await self.movement.move_to_position(x, y, z)
             # Release cell
             if not await self.movement.deactivate_gripper():
-                raise RuntimeError("Failed to release cell")            
-            slot.occupied = True
-            slot.cell_id = cell.id
-            self.gripper_occupied = False
-            
-            # Move back to safe height
-            await self.movement.move_to_position(x, y, self.system_settings.safe_height)
-            logger.info(f"Cell {cell.id} inserted to slot at position {slot.position}")
-            return True
+                raise RuntimeError("Failed to release cell")     
         except Exception as e:
             logger.error(f"Failed to insert cell: {str(e)}")
-            return False
+            return False   
+            
+        slot.occupied = True
+        slot.cell_id = cell.id
+        self.gripper_occupied = False
+
+        # Start measurement
+        cell.status = CellStatus.MEASURING
+        # cell.measurement_slot = slot.slot_id # TODO[SG]: Make this consistent
+        self.mqtt_handler.start_measurement(
+            device_id=slot.device_id,
+            slot=slot.slot_id,
+            cell_id=cell.id,
+            callback=lambda device_id, slot, cell_id, capacity: self.work_queue.append((cell_id, slot, device_id, capacity))
+        )
+        
+        # Move back to safe height
+        await self.movement.move_to_position(x, y, self.system_settings.safe_height)
+        logger.info(f"Cell {cell.id} inserted to slot at position {slot.position}")
+        return True
 
     async def collect_cell_from_slot(self, slot: SlotConfig):
         if self.gripper_occupied:

robot-control/tests/test_robot.py

@@ -9,7 +9,10 @@ def robot_movement():
 
 @pytest.fixture
 def robot_controller():
-    return RobotController(ConfigParser())
+    config_parser = ConfigParser()
+    config_parser.config["mqtt"]["broker"] = "debug" # connects to test mqtt broker
+    controller = RobotController(config_parser)
+    return controller
 
 class TestRobotMovement:
     def test_init_position(self, robot_movement: RobotMovement):