Adding beacons to book keeping to subscribe from ROS 2 (#823)

* Adding beacons to book keeping to subscribe from ROS 2 Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Lint Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Cleaned up from_tortoise for linting Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Cast to proper bool type from ttm Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Disable pylint for import error as we are not using the same released API Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Disable block Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Disable just for the next issue instead of entire file Signed-off-by: Aaron Chong <aaronchongth@gmail.com> --------- Signed-off-by: Aaron Chong <aaronchongth@gmail.com>
open-rmf · Nov 2, 2023 · 56316c4 · 56316c4
1 parent 7f4ef45
commit 56316c4
Show file tree

Hide file tree

Showing 12 changed files with 225 additions and 6 deletions.
diff --git a/packages/api-server/api_server/app.py b/packages/api-server/api_server/app.py
@@ -33,7 +33,7 @@
 )
 from .models import tortoise_models as ttm
 from .repositories import TaskRepository
-from .rmf_io import HealthWatchdog, RmfBookKeeper, rmf_events
+from .rmf_io import HealthWatchdog, RmfBookKeeper, beacon_events, rmf_events
 from .types import is_coroutine
 
 
@@ -85,7 +85,9 @@ async def on_sio_connect(sid: str, _environ: dict, auth: Optional[dict] = None):
 # will be called in reverse order on app shutdown
 shutdown_cbs: List[Union[Coroutine[Any, Any, Any], Callable[[], None]]] = []
 
-rmf_bookkeeper = RmfBookKeeper(rmf_events, logger=logger.getChild("BookKeeper"))
+rmf_bookkeeper = RmfBookKeeper(
+    rmf_events, beacon_events, logger=logger.getChild("BookKeeper")
+)
 
 app.include_router(routes.main_router)
 app.include_router(

diff --git a/packages/api-server/api_server/gateway.py b/packages/api-server/api_server/gateway.py
@@ -19,6 +19,9 @@
 from rmf_door_msgs.msg import DoorMode as RmfDoorMode
 from rmf_door_msgs.msg import DoorRequest as RmfDoorRequest
 from rmf_door_msgs.msg import DoorState as RmfDoorState
+
+# pylint: disable-next=no-name-in-module
+from rmf_fleet_msgs.msg import BeaconState as RmfBeaconState
 from rmf_ingestor_msgs.msg import IngestorState as RmfIngestorState
 from rmf_lift_msgs.msg import LiftRequest as RmfLiftRequest
 from rmf_lift_msgs.msg import LiftState as RmfLiftState
@@ -27,9 +30,16 @@
 from rosidl_runtime_py.convert import message_to_ordereddict
 
 from .logger import logger as base_logger
-from .models import BuildingMap, DispenserState, DoorState, IngestorState, LiftState
+from .models import (
+    BeaconState,
+    BuildingMap,
+    DispenserState,
+    DoorState,
+    IngestorState,
+    LiftState,
+)
 from .repositories import CachedFilesRepository, cached_files_repo
-from .rmf_io import rmf_events
+from .rmf_io import beacon_events, rmf_events
 from .ros import ros_node
 
 
@@ -157,6 +167,23 @@ def convert_lift_state(lift_state: RmfLiftState):
         )
         self._subscriptions.append(map_sub)
 
+        def convert_beacon_state(beacon_state: RmfBeaconState):
+            return BeaconState(
+                id=beacon_state.id,
+                online=beacon_state.online,
+                category=beacon_state.category,
+                activated=beacon_state.activated,
+                level=beacon_state.level,
+            )
+
+        beacon_sub = ros_node().create_subscription(
+            RmfBeaconState,
+            "beacon_state",
+            lambda msg: beacon_events.beacons.on_next(convert_beacon_state(msg)),
+            10,
+        )
+        self._subscriptions.append(beacon_sub)
+
     @staticmethod
     def now() -> Optional[RosTime]:
         """

diff --git a/packages/api-server/api_server/models/__init__.py b/packages/api-server/api_server/models/__init__.py
@@ -1,4 +1,5 @@
 from .authz import *
+from .beacons import *
 from .building_map import *
 from .dispensers import *
 from .doors import *

diff --git a/packages/api-server/api_server/models/beacons.py b/packages/api-server/api_server/models/beacons.py
@@ -0,0 +1,27 @@
+from typing import cast
+
+from . import tortoise_models as ttm
+from .ros_pydantic import rmf_fleet_msgs
+
+
+class BeaconState(rmf_fleet_msgs.BeaconState):
+    @staticmethod
+    def from_tortoise(tortoise: ttm.BeaconState) -> "BeaconState":
+        return BeaconState(
+            id=tortoise.id,
+            online=cast(bool, tortoise.online),
+            category=tortoise.category,
+            activated=cast(bool, tortoise.activated),
+            level=tortoise.level,
+        )
+
+    async def save(self) -> None:
+        await ttm.BeaconState.update_or_create(
+            {
+                "online": self.online,
+                "category": self.category,
+                "activated": self.activated,
+                "level": self.level,
+            },
+            id=self.id,
+        )
diff --git a/packages/api-server/api_server/models/ros_pydantic/rmf_fleet_msgs/BeaconState.py b/packages/api-server/api_server/models/ros_pydantic/rmf_fleet_msgs/BeaconState.py
@@ -0,0 +1,34 @@
+# This is a generated file, do not edit
+
+from typing import List
+
+import pydantic
+
+
+class BeaconState(pydantic.BaseModel):
+    id: str = ""  # string
+    online: bool = False  # bool
+    category: str = ""  # string
+    activated: bool = False  # bool
+    level: str = ""  # string
+
+    class Config:
+        orm_mode = True
+        schema_extra = {
+            "required": [
+                "id",
+                "online",
+                "category",
+                "activated",
+                "level",
+            ],
+        }
+
+
+# # This message defines data from a robot beacon
+#
+# string id
+# bool online
+# string category
+# bool activated
+# string level
diff --git a/packages/api-server/api_server/models/ros_pydantic/rmf_fleet_msgs/DeliveryAlert.py b/packages/api-server/api_server/models/ros_pydantic/rmf_fleet_msgs/DeliveryAlert.py
@@ -0,0 +1,43 @@
+# This is a generated file, do not edit
+
+from typing import List
+
+import pydantic
+
+from ..rmf_fleet_msgs.DeliveryAlertAction import DeliveryAlertAction
+from ..rmf_fleet_msgs.DeliveryAlertCategory import DeliveryAlertCategory
+from ..rmf_fleet_msgs.DeliveryAlertTier import DeliveryAlertTier
+
+
+class DeliveryAlert(pydantic.BaseModel):
+    id: str = ""  # string
+    category: DeliveryAlertCategory = (
+        DeliveryAlertCategory()
+    )  # rmf_fleet_msgs/DeliveryAlertCategory
+    tier: DeliveryAlertTier = DeliveryAlertTier()  # rmf_fleet_msgs/DeliveryAlertTier
+    task_id: str = ""  # string
+    action: DeliveryAlertAction = (
+        DeliveryAlertAction()
+    )  # rmf_fleet_msgs/DeliveryAlertAction
+    message: str = ""  # string
+
+    class Config:
+        orm_mode = True
+        schema_extra = {
+            "required": [
+                "id",
+                "category",
+                "tier",
+                "task_id",
+                "action",
+                "message",
+            ],
+        }
+
+
+# string id
+# DeliveryAlertCategory category
+# DeliveryAlertTier tier
+# string task_id
+# DeliveryAlertAction action
+# string message
diff --git a/packages/api-server/api_server/models/ros_pydantic/rmf_fleet_msgs/DeliveryAlertAction.py b/packages/api-server/api_server/models/ros_pydantic/rmf_fleet_msgs/DeliveryAlertAction.py
@@ -0,0 +1,24 @@
+# This is a generated file, do not edit
+
+from typing import List
+
+import pydantic
+
+
+class DeliveryAlertAction(pydantic.BaseModel):
+    value: pydantic.conint(ge=0, le=4294967295) = 0  # uint32
+
+    class Config:
+        orm_mode = True
+        schema_extra = {
+            "required": [
+                "value",
+            ],
+        }
+
+
+# uint32 value
+# uint32 WAITING=0
+# uint32 CANCEL=1
+# uint32 OVERRIDE=2
+# uint32 RESUME=3
diff --git a/packages/api-server/api_server/models/ros_pydantic/rmf_fleet_msgs/DeliveryAlertCategory.py b/packages/api-server/api_server/models/ros_pydantic/rmf_fleet_msgs/DeliveryAlertCategory.py
@@ -0,0 +1,22 @@
+# This is a generated file, do not edit
+
+from typing import List
+
+import pydantic
+
+
+class DeliveryAlertCategory(pydantic.BaseModel):
+    value: pydantic.conint(ge=0, le=4294967295) = 0  # uint32
+
+    class Config:
+        orm_mode = True
+        schema_extra = {
+            "required": [
+                "value",
+            ],
+        }
+
+
+# uint32 value
+# uint32 MISSING=0
+# uint32 WRONG=1
diff --git a/packages/api-server/api_server/models/ros_pydantic/rmf_fleet_msgs/DeliveryAlertTier.py b/packages/api-server/api_server/models/ros_pydantic/rmf_fleet_msgs/DeliveryAlertTier.py
@@ -0,0 +1,22 @@
+# This is a generated file, do not edit
+
+from typing import List
+
+import pydantic
+
+
+class DeliveryAlertTier(pydantic.BaseModel):
+    value: pydantic.conint(ge=0, le=4294967295) = 0  # uint32
+
+    class Config:
+        orm_mode = True
+        schema_extra = {
+            "required": [
+                "value",
+            ],
+        }
+
+
+# uint32 value
+# uint32 WARNING=0
+# uint32 ERROR=1
diff --git a/packages/api-server/api_server/models/ros_pydantic/rmf_fleet_msgs/__init__.py b/packages/api-server/api_server/models/ros_pydantic/rmf_fleet_msgs/__init__.py
@@ -1,3 +1,4 @@
+from .BeaconState import BeaconState
 from .ClosedLanes import ClosedLanes
 from .DestinationRequest import DestinationRequest
 from .Dock import Dock

diff --git a/packages/api-server/api_server/rmf_io/book_keeper.py b/packages/api-server/api_server/rmf_io/book_keeper.py
@@ -8,6 +8,7 @@
 from rx.subject.subject import Subject
 
 from api_server.models import (
+    BeaconState,
     BuildingMap,
     DispenserHealth,
     DispenserState,
@@ -21,7 +22,7 @@
 )
 from api_server.models.health import BaseBasicHealth
 
-from .events import RmfEvents
+from .events import BeaconEvents, RmfEvents
 
 
 class RmfBookKeeperEvents:
@@ -33,6 +34,7 @@ class RmfBookKeeper:
     _ChildLoggers = namedtuple(
         "_ChildLoggers",
         [
+            "beacon_state",
             "building_map",
             "door_state",
             "door_health",
@@ -51,16 +53,19 @@ class RmfBookKeeper:
     def __init__(
         self,
         rmf_events: RmfEvents,
+        beacon_events: BeaconEvents,
         *,
         logger: Optional[logging.Logger] = None,
     ):
         self.rmf = rmf_events
+        self.beacon_events = beacon_events
         self.bookkeeper_events = RmfBookKeeperEvents()
         self._loop: asyncio.AbstractEventLoop
         self._main_logger = logger or logging.getLogger(self.__class__.__name__)
         self._pending_tasks = set()
 
         self._loggers = self._ChildLoggers(
+            self._main_logger.getChild("beacon_state"),
             self._main_logger.getChild("building_map"),
             self._main_logger.getChild("door_state"),
             self._main_logger.getChild("door_health"),
@@ -75,6 +80,7 @@ def __init__(
             self._main_logger.getChild("task_summary"),
         )
 
+        self._loggers.beacon_state.parent = self._main_logger
         self._loggers.door_state.parent = self._main_logger
         self._loggers.door_health.parent = self._main_logger
         self._loggers.lift_state.parent = self._main_logger
@@ -91,6 +97,7 @@ def __init__(
 
     async def start(self):
         self._loop = asyncio.get_event_loop()
+        self._record_beacon_state()
         self._record_building_map()
         self._record_door_state()
         self._record_door_health()
@@ -123,6 +130,15 @@ def _report_health(health: BaseBasicHealth, logger: logging.Logger):
         else:
             logger.info(message)
 
+    def _record_beacon_state(self):
+        async def update(beacon_state: BeaconState):
+            await beacon_state.save()
+            self._loggers.beacon_state.info(json.dumps(beacon_state.dict()))
+
+        self._subscriptions.append(
+            self.beacon_events.beacons.subscribe(lambda x: self._create_task(update(x)))
+        )
+
     def _record_building_map(self):
         async def update(building_map: BuildingMap):
             if not building_map:

diff --git a/packages/api-server/generate-models.sh b/packages/api-server/generate-models.sh
@@ -3,7 +3,7 @@ set -e
 shopt -s globstar
 
 RMF_BUILDING_MAP_MSGS_VER=c5e0352e2dfd3d11e4d292a1c2901cad867c1441
-RMF_INTERNAL_MSGS_VER=0c237e1758872917661879975d7dc0acf5fa518c
+RMF_INTERNAL_MSGS_VER=3690a47055ef45466cf970587d8b8e09df1a8825
 RMF_API_MSGS_VER=91295892192d24ec73c9a1c6fa54334963586784
 RMF_ROS2_VER=bf038461b5b0fb7d4594461a724bc9e5e7cb97c6
 CODEGEN_VER=$(pipenv run datamodel-codegen --version)