Module for world object retrieval

spot_wrapper/spot_world_objects.py

@@ -0,0 +1,97 @@
+import logging
+import typing
+
+from bosdyn.api.world_object_pb2 import ListWorldObjectResponse
+from bosdyn.api.world_object_pb2 import WorldObjectType
+from bosdyn.client.async_tasks import AsyncPeriodicQuery
+from bosdyn.client.common import FutureWrapper
+from bosdyn.client.world_object import WorldObjectClient
+
+
+class AsyncWorldObjects(AsyncPeriodicQuery):
+    """
+    Class to get world objects. list_world_objects_async query sent to the robot at every tick. Callback
+    registered to defined callback function.
+    """
+
+    def __init__(
+        self,
+        client: WorldObjectClient,
+        logger: logging.Logger,
+        rate: float,
+        callback: typing.Optional[typing.Callable] = None,
+    ) -> None:
+        """
+        Args:
+            client: Client to the world object service on the robot
+            logger: Logger
+            rate: Rate (Hz) to trigger the query
+            callback: Callback function to call when the results of the query are available
+        """
+        super().__init__(
+            "world-objects", client, logger, period_sec=1.0 / max(rate, 1.0)
+        )
+        self._callback = None
+        if rate > 0.0:
+            self._callback = callback
+
+    def _start_query(self) -> typing.Optional[FutureWrapper]:
+        if self._callback:
+            callback_future = self._client.list_world_objects_async()
+            callback_future.add_done_callback(self._callback)
+            return callback_future
+
+
+class SpotWorldObjects:
+    """
+    Module which allows access to world objects observed by the robot
+    """
+
+    def __init__(
+        self,
+        logger: logging.Logger,
+        world_object_client: WorldObjectClient,
+        rate: float = 10,
+        callback: typing.Optional[typing.Callable] = None,
+    ) -> None:
+        """
+
+        Args:
+            logger: Logger to use
+            world_object_client: Instantiated world object client to use to retrieve world objects
+            rate: Rate at which to list objects
+            callback: Callback to call with the retrieved objects
+        """
+        self._logger = logger
+        self._world_objects_client = world_object_client
+        self._world_objects_task = AsyncWorldObjects(
+            self._world_objects_client,
+            self._logger,
+            rate,
+            callback,
+        )
+
+    @property
+    def async_task(self) -> AsyncWorldObjects:
+        """
+        The async task used to retrieve world objects periodically
+        """
+        return self._world_objects_task
+
+    def list_world_objects(
+        self, object_types: typing.List[WorldObjectType], time_start_point: float
+    ) -> ListWorldObjectResponse:
+        """
+        Get a list of world objects with the specified types which were seen after the given time point
+
+        Args:
+            object_types: List of object types which should be retrieved
+            time_start_point: Objects observed after this time will be filtered out of the response
+
+        Returns:
+            List world object response containing the filtered list of world objects
+
+        """
+        return self._world_objects_client.list_world_objects(
+            object_types, time_start_point
+        )

spot_wrapper/wrapper.py

@@ -18,6 +18,7 @@
 from bosdyn.api import robot_state_pb2
 from bosdyn.api import synchronized_command_pb2
 from bosdyn.api import trajectory_pb2
+from bosdyn.api import world_object_pb2
 from bosdyn.api.graph_nav import graph_nav_pb2
 from bosdyn.api.graph_nav import map_pb2
 from bosdyn.api.graph_nav import nav_pb2
@@ -77,6 +78,9 @@
 from bosdyn.api import basic_command_pb2
 from google.protobuf.timestamp_pb2 import Timestamp
 
+from .spot_world_objects import SpotWorldObjects
+
+
 front_image_sources = [
     "frontleft_fisheye_image",
     "frontright_fisheye_image",
@@ -514,31 +518,6 @@ def _start_query(self):
             pass
 
 
-class AsyncWorldObjects(AsyncPeriodicQuery):
-    """Class to get world objects.  list_world_objects_async query sent to the robot at every tick.  Callback registered to defined callback function.
-
-    Attributes:
-        client: The Client to a service on the robot
-        logger: Logger object
-        rate: Rate (Hz) to trigger the query
-        callback: Callback function to call when the results of the query are available
-    """
-
-    def __init__(self, client, logger, rate, callback):
-        super(AsyncWorldObjects, self).__init__(
-            "world-objects", client, logger, period_sec=1.0 / max(rate, 1.0)
-        )
-        self._callback = None
-        if rate > 0.0:
-            self._callback = callback
-
-    def _start_query(self):
-        if self._callback:
-            callback_future = self._client.list_world_objects_async()
-            callback_future.add_done_callback(self._callback)
-            return callback_future
-
-
 def try_claim(func=None, *, power_on=False):
     """
     Decorator which tries to acquire the lease before executing the wrapped function
@@ -927,12 +906,6 @@ def __init__(
         self._estop_monitor = AsyncEStopMonitor(
             self._estop_client, self._logger, 20.0, self
         )
-        self._world_objects_task = AsyncWorldObjects(
-            self._world_objects_client,
-            self._logger,
-            10.0,
-            self._callbacks.get("world_objects", None),
-        )
 
         self._estop_endpoint = None
         self._estop_keepalive = None
@@ -944,7 +917,6 @@ def __init__(
             self._front_image_task,
             self._idle_task,
             self._estop_monitor,
-            self._world_objects_task,
         ]
 
         if self._point_cloud_client:
@@ -957,6 +929,15 @@ def __init__(
             )
             robot_tasks.append(self._point_cloud_task)
 
+        self._spot_world_objects = SpotWorldObjects(
+            self._logger,
+            self._world_objects_client,
+            self._rates.get("world_objects", 10),
+            self._callbacks.get("world_objects", None),
+        )
+        self._world_objects_task = self._spot_world_objects.async_task
+        robot_tasks.append(self._world_objects_task)
+
         self._async_tasks = AsyncTasks(robot_tasks)
 
         self.camera_task_name_to_task_mapping = {
@@ -1051,9 +1032,14 @@ def lease(self):
         return self._lease_task.proto
 
     @property
-    def world_objects(self):
+    def spot_world_objects(self) -> SpotWorldObjects:
+        """Return SpotWorldObjects instance"""
+        return self._spot_world_objects
+
+    @property
+    def world_objects(self) -> world_object_pb2.ListWorldObjectResponse:
         """Return most recent proto from _world_objects_task"""
-        return self._world_objects_task.proto
+        return self.spot_world_objects.async_task.proto
 
     @property
     def front_images(self):
@@ -1399,11 +1385,6 @@ def get_mobility_params(self):
         """Get mobility params"""
         return self._mobility_params
 
-    def list_world_objects(self, object_types, time_start_point):
-        return self._world_objects_client.list_world_objects(
-            object_types, time_start_point
-        )
-
     @try_claim
     def velocity_cmd(self, v_x, v_y, v_rot, cmd_duration=0.125):
         """Send a velocity motion command to the robot.