diff --git a/spot_wrapper/spot_world_objects.py b/spot_wrapper/spot_world_objects.py new file mode 100644 index 00000000..a9ea017b --- /dev/null +++ b/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 + ) diff --git a/spot_wrapper/wrapper.py b/spot_wrapper/wrapper.py index 0e84da3b..3944bb0c 100644 --- a/spot_wrapper/wrapper.py +++ b/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.