Enhance project configuration and testing setup

- Add VSCode settings for pytest configuration
- Create setup.py for package management
- Refactor RobotController to accept configuration during initialization

.gitignore

@@ -1 +1,4 @@
 *.cpython-39.pyc
+.VSCodeCounter/
+*.pyc
+robot-control/src/robot_control.egg-info/

.vscode/settings.json

@@ -0,0 +1,7 @@
+{
+    "python.testing.pytestArgs": [
+        "robot-control"
+    ],
+    "python.testing.unittestEnabled": false,
+    "python.testing.pytestEnabled": true
+}

robot-control/requirements.txt

@@ -4,4 +4,7 @@ opencv-python
 pylibdmtx
 pydantic
 numpy
-PyYAML==6.0.1
+PyYAML==6.0.1
+pytest==7.4.2
+pytest-asyncio==0.21.1
+httpx==0.25.0

robot-control/setup.py

@@ -0,0 +1,8 @@
+from setuptools import setup, find_packages
+
+setup(
+    name="robot-control",
+    version="0.1",
+    package_dir={"": "src"},
+    packages=find_packages(where="src"),
+)

robot-control/src/main.py

@@ -1,12 +1,14 @@
 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:
     def __init__(self):
-        self.controller = RobotController()
+        self.config = ConfigParser()
+        self.controller = RobotController(self.config)
         self.vision = DataMatrixReader()
         self.api = api_app
 

robot-control/src/robot/config.py

@@ -18,7 +18,7 @@ class DeviceConfig:
 @dataclass
 class FeederConfig:
     position: Tuple[float, float, float]
-    capacity_threshold: float
+    approach_position: Tuple[float, float, float]
 
 @dataclass
 class DropoffGradeConfig:
@@ -31,7 +31,7 @@ class SystemConfig:
     acceleration: float
 
 class ConfigParser:
-    def __init__(self, config_path: str = "config/config.yaml"):
+    def __init__(self, config_path: str = "robot-control/config/config.yaml"):
         self.config_path = Path(config_path)
         self.config = self._load_config()
 
@@ -74,7 +74,7 @@ class ConfigParser:
         return grades
 
     def get_system_settings(self) -> SystemConfig:
-        settings = self.config['robot_settings']
+        settings = self.config['system_settings']
         return SystemConfig(
             speed=settings['speed'],
             acceleration=settings['acceleration']

robot-control/src/robot/controller.py

@@ -25,8 +25,8 @@ class DropoffGrade:
         self.position = (x, y, z)
 
 class RobotController:
-    def __init__(self):
-        self.config = ConfigParser()
+    def __init__(self, config: ConfigParser):
+        self.config = config
         self.devices = self.config.get_devices()
         self.feeder = self.config.get_feeder()
         self.dropoff_grades = self.config.get_dropoff_grades()
@@ -77,7 +77,8 @@ class RobotController:
             logger.error("Gripper not occupied")
             return
         slot.occupied = True
-        slot.cell_id = cell
+        slot.cell_id = cell.id
+        self.gripper_occupied = False
         logger.info(f"Cell {cell.id} inserted to slot at position {slot.position}")
         # Move to slot and insert cell
         pass
@@ -85,12 +86,14 @@ class RobotController:
     async def collect_cell_from_slot(self, slot: SlotConfig):
         if self.gripper_occupied:
             logger.error("Gripper already occupied")
-            return
+            return None
         # Collect cell from measurement slot
+        self.gripper_occupied = True
         slot.occupied = False
+        cell_id = slot.cell_id
         slot.cell_id = None
         logger.info(f"Cell {slot.cell_id} collected from slot at position {slot.position}")
-        return slot.cell_id
+        return cell_id
 
     async def sort_cell(self, cell: Cell):
         if cell.status == CellStatus.FAILED:
@@ -100,6 +103,7 @@ class RobotController:
         for name, grade in self.dropoff_grades.items():
             if cell.capacity >= grade.capacity_threshold:
                 await self.dropoff_cell(cell, grade)
+                self.gripper_occupied = False
                 logger.info(f"Cell {cell.id} sorted to grade {name}")
                 return
         logger.error(f"No suitable grade found for cell {cell.id} with capacity {cell.capacity}")

robot-control/src/vision/datamatrix.py

@@ -1,7 +1,9 @@
 import cv2
 from pylibdmtx.pylibdmtx import decode
+import logging
 
 # TODO[SG]: add real datamatrix code. This is just a placeholder.
+logger = logging.getLogger(__name__)
 
 class DataMatrixReader:
     def __init__(self, camera_id: int = 0):
@@ -18,7 +20,8 @@ class DataMatrixReader:
         
         decoded = decode(frame)
         if not decoded:
-            raise Exception("No data matrix found")
+            logger.error("No datamatrix found")
+            return None
             
         return decoded[0].data.decode('utf-8')
 

robot-control/tests/test_api.py

@@ -0,0 +1,35 @@
+import pytest
+from fastapi.testclient import TestClient
+from api.routes import app
+
+@pytest.fixture
+def client():
+    return TestClient(app)
+
+class TestAPIEndpoints:
+    def test_get_status(self, client):
+        response = client.get("/status")
+        assert response.status_code == 200
+        assert "status" in response.json()
+
+    def test_get_robot_position(self, client):
+        response = client.get("/robot/position")
+        assert response.status_code == 200
+        data = response.json()
+        assert "x" in data
+        assert "y" in data
+        assert "z" in data
+
+    def test_get_slots_status(self, client):
+        response = client.get("/slots")
+        assert response.status_code == 200
+        slots = response.json()
+        assert isinstance(slots, list)
+        if len(slots) > 0:
+            assert "id" in slots[0]
+            assert "occupied" in slots[0]
+
+    def test_emergency_stop(self, client):
+        response = client.post("/robot/stop")
+        assert response.status_code == 200
+        assert response.json()["status"] == "stopped"

robot-control/tests/test_robot.py

@@ -0,0 +1,64 @@
+import pytest
+from robot.movement import RobotMovement
+from robot.controller import RobotController, Cell, CellStatus
+from robot.config import ConfigParser
+
+@pytest.fixture
+def robot_movement():
+    return RobotMovement()
+
+@pytest.fixture
+def robot_controller():
+    return RobotController(ConfigParser())
+
+class TestRobotMovement:
+    def test_init_position(self, robot_movement: RobotMovement):
+        assert robot_movement.get_position() == (0.0, 0.0, 0.0)
+        assert not robot_movement.is_moving
+
+    def test_set_speed(self, robot_movement: RobotMovement):
+        robot_movement.set_speed(200.0)
+        assert robot_movement.speed == 200.0
+
+        with pytest.raises(ValueError):
+            robot_movement.set_speed(-100.0)
+
+    def test_move_to_position(self, robot_movement: RobotMovement):
+        result = robot_movement.move_to_position(100.0, 200.0, 300.0)
+        assert result is True
+        assert robot_movement.get_position() == (100.0, 200.0, 300.0)
+
+    def test_move_relative(self, robot_movement: RobotMovement):
+        robot_movement.move_to_position(100.0, 100.0, 100.0)
+        result = robot_movement.move_relative(50.0, -50.0, 25.0)
+        assert result is True
+        assert robot_movement.get_position() == (150.0, 50.0, 125.0)
+
+class TestRobotController:
+    def test_init_controller(self, robot_controller: RobotController):
+        assert len(robot_controller.devices) > 0
+        assert robot_controller.feeder is not None
+        assert not robot_controller.gripper_occupied
+
+    @pytest.mark.asyncio
+    async def test_get_next_free_slot(self, robot_controller: RobotController):
+        slot = robot_controller.get_next_free_slot()
+        assert slot is not None
+        assert not slot.occupied
+
+    @pytest.mark.asyncio
+    async def test_insert_and_collect_cell(self, robot_controller: RobotController):
+        # Test cell insertion
+        cell = Cell(id="test_cell", status=CellStatus.WAITING)
+        slot = robot_controller.get_next_free_slot()
+        robot_controller.gripper_occupied = True
+        
+        await robot_controller.insert_cell_to_slot(cell, slot)
+        assert slot.occupied
+        assert slot.cell_id == cell.id
+
+        # Test cell collection
+        collected_cell = await robot_controller.collect_cell_from_slot(slot)
+        assert not slot.occupied
+        assert slot.cell_id is None
+        assert collected_cell == cell.id

robot-control/tests/test_vision.py

@@ -0,0 +1,20 @@
+import pytest
+from vision.datamatrix import DataMatrixReader
+import numpy as np
+
+@pytest.fixture
+def datamatrix_reader():
+    return DataMatrixReader()
+
+class TestDataMatrixReader:
+    def test_init_reader(self, datamatrix_reader: DataMatrixReader):
+        assert datamatrix_reader is not None
+
+    def test_read_failure(self, datamatrix_reader: DataMatrixReader):
+        datamatrix_reader.initialize()
+        result = datamatrix_reader.read_datamatrix()
+        assert result is None
+
+    def test_read_success(self, datamatrix_reader: DataMatrixReader):
+        #TDOD[SG]: Add real test data
+        pass