State estimator stub

protos/farm_ng/controller/controller.proto

@@ -72,9 +72,13 @@ enum ReplyStatus {
     FAILED = 1;
 }
 enum ControllerServiceState {
-    STOPPED = 0;
-    RUNNING = 1;
-  }
+    UNKNOWN = 0;
+    STOPPED = 1;
+    RUNNING = 2;
+    IDLE = 3;
+    UNAVAILABLE = 4;
+}
+
 message GetServiceStateRequest {
     string message = 1;
 }

protos/farm_ng/state_estimator/state_estimator.proto

@@ -2,14 +2,15 @@ syntax = "proto3";
 
 package farm_ng.state_estimator.proto;
 
-// import "farm_ng/amiga/temp_imports.proto";
+import "sophus/linalg.proto";
+import "sophus/lie.proto";
 
-service StateEstimator {
+service StateEstimatorService {
   rpc robotDynamics(RobotDynamicsRequest)
       returns (stream RobotDynamicsResult) {}
 
   rpc robotCalibrationResult(RobotCalibrationRequest)
-      returns (stream RobotCalibrationResult) {}
+      returns (RobotCalibrationResult) {}
 
   rpc getServiceState(GetServiceStateRequest) returns (GetServiceStateResult) {}
   rpc startService(StartServiceRequest) returns (StartServiceResult) {}
@@ -24,12 +25,22 @@ message RobotDynamicsResult {
     // Timestamp from farm-ng-core
     // SE3d types from Sophus
     Timestamp stamp = 1;  // this contains which clock
-    SE3d world_pose_robot = 2;
+    sophus.proto.Se3F64 odom_pose_robot = 2;
     // Rate of the robot in the robot's frame
-    SE3dLog robot_rate = 3;
+    RobotRate robot_rate = 3;
+}
+
+message RobotRate {
+    double speed = 1;
+    double angular_rate = 2;
 }
 
 message RobotCalibrationRequest {
+    string device = 1;
+}
+
+message RobotCalibrationResult {
+
     Timestamp stamp = 1;  // this contains which clock
     // poses has
     // robot -> oak0/right
@@ -42,15 +53,6 @@ message RobotCalibrationRequest {
     repeated NamedWheel wheels = 3;
 }
 
-message RobotCalibrationResult {
-    // Timestamp from farm-ng-core
-    // SE3d types from Sophus
-    Timestamp stamp = 1;  // this contains which clock
-    SE3d world_pose_robot = 2;
-    // Rate of the robot in the robot's frame
-    SE3dLog robot_rate = 3;
-}
-
 message NamedWheel {
     // e.g. wheel/0xA
     string name = 1;
@@ -63,39 +65,41 @@ message NamedWheel {
 ////////////////////////////////////////////////
 
 // Geometry/etc. protos
-message SE3d {
-    string foo = 1;
-}
-message SE3dLog {
-    SE3d foo = 1;
-}
+
+
 message NamedPose3d {
     string frame_a = 1;
     string frame_b = 2;
-    SE3d a_pose_b = 3;
+    sophus.proto.Se3F64 a_pose_b = 3;
 }
+
 message Timestamp {
     string clock_name = 1;
     double seconds = 2;
     string clock_type = 3;
 }
 
 
+
 // Generic service protos
 enum ReplyStatus {
     OK = 0;
     FAILED = 1;
 }
-enum ServiceState {
-    STOPPED = 0;
-    RUNNING = 1;
-  }
+enum StateEstimatorServiceState {
+    UNKNOWN = 0;
+    STOPPED = 1;
+    RUNNING = 2;
+    IDLE = 3;
+    UNAVAILABLE = 4;
+}
+
 message GetServiceStateRequest {
     string message = 1;
 }
 message GetServiceStateResult {
     string state_name = 1;
-    ServiceState state = 2;
+    StateEstimatorServiceState state = 2;
     ReplyStatus status = 3;
 }
 message StartServiceRequest {

py/farm_ng/controller/controller_client.py

@@ -22,6 +22,24 @@ class ControllerClientConfig:
     address: str = "localhost"  # the address name of the server
 
 
+class ControllerServiceState:
+    """Controller service state."""
+
+    def __init__(self, proto: controller_pb2.ControllerServiceState = None) -> None:
+        self._proto = proto or controller_pb2.ControllerServiceState.UNAVAILABLE
+
+    @property
+    def value(self) -> int:
+        return self._proto
+
+    @property
+    def name(self) -> str:
+        return controller_pb2.ControllerServiceState.DESCRIPTOR.values[self.value].name
+
+    def __repr__(self) -> str:
+        return f"{self.__class__.__name__}: ({self.value}, {self.name})"
+
+
 class ControllerClient:
     def __init__(self, config: ControllerClientConfig) -> None:
         self.config = config
@@ -32,10 +50,10 @@ def __init__(self, config: ControllerClientConfig) -> None:
         self.channel = grpc.aio.insecure_channel(self.server_address)
         self.stub = controller_pb2_grpc.ControllerServiceStub(self.channel)
 
-        self._state = controller_pb2.ControllerServiceState.STOPPED
+        self._state = ControllerServiceState()
 
     @property
-    def state(self) -> controller_pb2.ControllerServiceState:
+    def state(self) -> ControllerServiceState:
         return self._state
 
     @property
@@ -52,27 +70,27 @@ async def _poll_service_state(self) -> None:
                 self.logger.info("Got Cancelled Error")
                 break
 
-    async def get_state(self) -> controller_pb2.ControllerServiceState:
-        state: controller_pb2.ControllerServiceState
+    async def get_state(self) -> ControllerServiceState:
+        state: ControllerServiceState
         try:
             response: controller_pb2.GetServiceStateResult = await self.stub.getServiceState(
                 controller_pb2.GetServiceStateRequest()
             )
-            state = response.state
+            state = ControllerServiceState(response.state)
         except grpc.RpcError:
-            state = controller_pb2.ControllerServiceState.STOPPED
+            state = ControllerServiceState()
         self.logger.debug("ControllerServiceStub: port -> %i state is: %s", self.config.port, state)
         return state
 
     async def start_service(self) -> None:
-        state: controller_pb2.ControllerServiceState = await self.get_state()
-        if state == controller_pb2.ControllerServiceState.STOPPED:
+        state: ControllerServiceState = await self.get_state()
+        if state.value == controller_pb2.ControllerServiceState.UNAVAILABLE:
             return
         await self.stub.startService(controller_pb2.StartServiceRequest())
 
     async def stop_service(self) -> None:
         state: controller_pb2.ControllerServiceState = await self.get_state()
-        if state == controller_pb2.ControllerServiceState.STOPPED:
+        if state.value == controller_pb2.ControllerServiceState.UNAVAILABLE:
             return
         await self.stub.stopService(controller_pb2.StopServiceRequest())
 

py/farm_ng/state_estimator/state_estimator_client.py

@@ -0,0 +1,95 @@
+import asyncio
+import logging
+from dataclasses import dataclass
+
+import grpc
+from farm_ng.state_estimator import state_estimator_pb2
+from farm_ng.state_estimator import state_estimator_pb2_grpc
+
+logging.basicConfig(level=logging.DEBUG)
+
+
+@dataclass
+class StateEstimatorClientConfig:
+    """StateEstimator client configuration.
+
+    Attributes:
+        port (int): the port to connect to the server.
+        address (str): the address to connect to the server.
+    """
+
+    port: int  # the port of the server address
+    address: str = "localhost"  # the address name of the server
+
+
+class StateEstimatorServiceState:
+    """State estimator service state."""
+
+    def __init__(self, proto: state_estimator_pb2.StateEstimatorServiceState = None) -> None:
+        self._proto = proto or state_estimator_pb2.StateEstimatorServiceState.UNAVAILABLE
+
+    @property
+    def value(self) -> int:
+        return self._proto
+
+    @property
+    def name(self) -> str:
+        return state_estimator_pb2.StateEstimatorServiceState.DESCRIPTOR.values[self.value].name
+
+    def __repr__(self) -> str:
+        return f"{self.__class__.__name__}: ({self.value}, {self.name})"
+
+
+class StateEstimatorClient:
+    def __init__(self, config: StateEstimatorClientConfig) -> None:
+        self.config = config
+
+        self.logger = logging.getLogger(self.__class__.__name__)
+
+        # create a async connection with the server
+        self.channel = grpc.aio.insecure_channel(self.server_address)
+        self.stub = state_estimator_pb2_grpc.StateEstimatorServiceStub(self.channel)
+
+        self._state = StateEstimatorServiceState()
+
+    @property
+    def state(self) -> StateEstimatorServiceState:
+        return self._state
+
+    @property
+    def server_address(self) -> str:
+        """Returns the composed address and port."""
+        return f"{self.config.address}:{self.config.port}"
+
+    async def _poll_service_state(self) -> None:
+        while True:
+            try:
+                self._state = await self.get_state()
+                await asyncio.sleep(0.5)
+            except asyncio.CancelledError:
+                self.logger.info("Got Cancelled Error")
+                break
+
+    async def get_state(self) -> StateEstimatorServiceState:
+        state: StateEstimatorServiceState
+        try:
+            response: state_estimator_pb2.GetServiceStateResult = await self.stub.getServiceState(
+                state_estimator_pb2.GetServiceStateRequest()
+            )
+            state = StateEstimatorServiceState(response.state)
+        except grpc.RpcError:
+            state = StateEstimatorServiceState()
+        self.logger.debug("StateEstimatorServiceStub: port -> %i state is: %s", self.config.port, state)
+        return state
+
+    async def start_service(self) -> None:
+        state: StateEstimatorServiceState = await self.get_state()
+        if state.value == state_estimator_pb2.StateEstimatorServiceState.UNAVAILABLE:
+            return
+        await self.stub.startService(state_estimator_pb2.StartServiceRequest())
+
+    async def stop_service(self) -> None:
+        state: StateEstimatorServiceState = await self.get_state()
+        if state.value == state_estimator_pb2.StateEstimatorServiceState.UNAVAILABLE:
+            return
+        await self.stub.stopService(state_estimator_pb2.StopServiceRequest())

py/tests/test_controller.py

@@ -2,6 +2,7 @@
 from farm_ng.controller import controller_pb2
 from farm_ng.controller.controller_client import ControllerClient
 from farm_ng.controller.controller_client import ControllerClientConfig
+from farm_ng.controller.controller_client import ControllerServiceState
 
 
 class TestControllerPb2:
@@ -28,5 +29,5 @@ def test_smoke(self, config: ControllerClientConfig) -> None:
     @pytest.mark.asyncio
     async def test_state(self, config: ControllerClientConfig) -> None:
         client = ControllerClient(config)
-        state: controller_pb2.ControllerServiceState = await client.get_state()
-        assert state == controller_pb2.ControllerServiceState.STOPPED
+        state: ControllerServiceState = await client.get_state()
+        assert state.value == controller_pb2.ControllerServiceState.UNAVAILABLE

py/tests/test_state_estimator.py

@@ -0,0 +1,33 @@
+import pytest
+from farm_ng.state_estimator import state_estimator_pb2
+from farm_ng.state_estimator.state_estimator_client import StateEstimatorClient
+from farm_ng.state_estimator.state_estimator_client import StateEstimatorClientConfig
+from farm_ng.state_estimator.state_estimator_client import StateEstimatorServiceState
+
+
+class TestStateEstimatorPb2:
+    def test_smoke(self) -> None:
+        request = state_estimator_pb2.RobotDynamicsRequest()
+        print(request)
+
+
+@pytest.fixture(name="config")
+def fixture_config() -> StateEstimatorClientConfig:
+    return StateEstimatorClientConfig(port=50051)
+
+
+class TestStateEstimatorClient:
+    def test_smoke_config(self, config: StateEstimatorClientConfig) -> None:
+        assert config.port == 50051
+        assert config.address == "localhost"
+
+    def test_smoke(self, config: StateEstimatorClientConfig) -> None:
+        client = StateEstimatorClient(config)
+        assert client is not None
+        assert client.server_address == "localhost:50051"
+
+    @pytest.mark.asyncio
+    async def test_state(self, config: StateEstimatorClientConfig) -> None:
+        client = StateEstimatorClient(config)
+        state: StateEstimatorServiceState = await client.get_state()
+        assert state.value == state_estimator_pb2.StateEstimatorServiceState.UNAVAILABLE