diff --git a/examples/server/v1/services.py b/examples/server/v1/services.py index a9e079f02..df0edafdd 100644 --- a/examples/server/v1/services.py +++ b/examples/server/v1/services.py @@ -1,3 +1,4 @@ +from datetime import datetime from typing import Dict, List, Optional from tests.mocks.services import MockMLModel, MockSLAM from viam.services.slam import Pose, SLAM @@ -24,6 +25,7 @@ def __init__(self, name: str): self.position = MockSLAM.POSITION self.internal_chunks = MockSLAM.INTERNAL_STATE_CHUNKS self.point_cloud_chunks = MockSLAM.POINT_CLOUD_PCD_CHUNKS + self.time = MockSLAM.LAST_UPDATE super().__init__(name) async def get_internal_state(self, **kwargs) -> List[bytes]: @@ -34,3 +36,6 @@ async def get_point_cloud_map(self, **kwargs) -> List[bytes]: async def get_position(self, **kwargs) -> Pose: return self.position + + async def get_latest_map_info(self, **kwargs) -> datetime: + return self.time diff --git a/src/viam/services/slam/client.py b/src/viam/services/slam/client.py index 5f2083d55..89ee8efaf 100644 --- a/src/viam/services/slam/client.py +++ b/src/viam/services/slam/client.py @@ -1,4 +1,5 @@ from typing import List, Mapping, Optional +from datetime import datetime from grpclib.client import Channel @@ -6,6 +7,8 @@ from viam.proto.service.slam import ( GetInternalStateRequest, GetInternalStateResponse, + GetLatestMapInfoRequest, + GetLatestMapInfoResponse, GetPointCloudMapRequest, GetPointCloudMapResponse, GetPositionRequest, @@ -46,6 +49,11 @@ async def get_internal_state(self, *, timeout: Optional[float] = None) -> List[G response: List[GetInternalStateResponse] = await self.client.GetInternalState(request, timeout=timeout) return response + async def get_latest_map_info(self, *, timeout: Optional[float] = None) -> datetime: + request = GetLatestMapInfoRequest(name=self.name) + response: GetLatestMapInfoResponse = await self.client.GetLatestMapInfo(request, timeout=timeout) + return response.last_map_update.ToDatetime() + async def do_command(self, command: Mapping[str, ValueTypes], *, timeout: Optional[float] = None) -> Mapping[str, ValueTypes]: request = DoCommandRequest(name=self.name, command=dict_to_struct(command)) response: DoCommandResponse = await self.client.DoCommand(request, timeout=timeout) diff --git a/src/viam/services/slam/service.py b/src/viam/services/slam/service.py index e580e25de..b11b9b1b8 100644 --- a/src/viam/services/slam/service.py +++ b/src/viam/services/slam/service.py @@ -1,6 +1,5 @@ from grpclib.server import Stream -from viam.errors import MethodNotImplementedError from viam.proto.common import DoCommandRequest, DoCommandResponse from viam.proto.service.slam import ( GetInternalStateRequest, @@ -14,7 +13,7 @@ SLAMServiceBase, ) from viam.resource.rpc_service_base import ResourceRPCServiceBase -from viam.utils import dict_to_struct, struct_to_dict +from viam.utils import datetime_to_timestamp, dict_to_struct, struct_to_dict from .slam import SLAM @@ -59,7 +58,13 @@ async def GetPosition(self, stream: Stream[GetPositionRequest, GetPositionRespon await stream.send_message(response) async def GetLatestMapInfo(self, stream: Stream[GetLatestMapInfoRequest, GetLatestMapInfoResponse]) -> None: - raise MethodNotImplementedError("GetLatestMapInfo").grpc_error + request = await stream.recv_message() + assert request is not None + slam = self.get_resource(request.name) + timeout = stream.deadline.time_remaining() if stream.deadline else None + time = await slam.get_latest_map_info(timeout=timeout) + response = GetLatestMapInfoResponse(last_map_update=datetime_to_timestamp(time)) + await stream.send_message(response) async def DoCommand(self, stream: Stream[DoCommandRequest, DoCommandResponse]) -> None: request = await stream.recv_message() diff --git a/src/viam/services/slam/slam.py b/src/viam/services/slam/slam.py index d2871603f..c5d852532 100644 --- a/src/viam/services/slam/slam.py +++ b/src/viam/services/slam/slam.py @@ -1,4 +1,5 @@ import abc +from datetime import datetime from typing import Final, List, Optional from viam.resource.types import RESOURCE_NAMESPACE_RDK, RESOURCE_TYPE_SERVICE, Subtype @@ -39,6 +40,16 @@ async def get_point_cloud_map(self, *, timeout: Optional[float]) -> List[bytes]: """ ... + @abc.abstractmethod + async def get_latest_map_info(self, *, timeout: Optional[float]) -> datetime: + """ + Get the timestamp of the last update to the point cloud SLAM map. + + Returns: + datetime: The timestamp of the last update. + """ + ... + @abc.abstractmethod async def get_position(self, *, timeout: Optional[float]) -> Pose: """ diff --git a/tests/mocks/services.py b/tests/mocks/services.py index 8f273b4b5..5b56eac8f 100644 --- a/tests/mocks/services.py +++ b/tests/mocks/services.py @@ -1,4 +1,5 @@ from typing import Any, Dict, List, Mapping, Optional, Union +from datetime import datetime from grpclib.server import Stream from PIL import Image @@ -413,6 +414,7 @@ class MockSLAM(SLAM): INTERNAL_STATE_CHUNKS = [bytes(5), bytes(2)] POINT_CLOUD_PCD_CHUNKS = [bytes(3), bytes(2)] POSITION = Pose(x=1, y=2, z=3, o_x=2, o_y=3, o_z=4, theta=20) + LAST_UPDATE = datetime(2023, 3, 12, 3, 24, 34, 29) def __init__(self, name: str): self.name = name @@ -431,6 +433,10 @@ async def get_position(self, *, timeout: Optional[float] = None) -> Pose: self.timeout = timeout return self.POSITION + async def get_latest_map_info(self, *, timeout: Optional[float] = None) -> datetime: + self.timeout = timeout + return self.LAST_UPDATE + async def do_command(self, command: Mapping[str, ValueTypes], *, timeout: Optional[float] = None, **kwargs) -> Mapping[str, ValueTypes]: return {"command": command} @@ -512,12 +518,10 @@ async def TabularDataByFilter(self, stream: Stream[TabularDataByFilterRequest, T data=dict_to_struct(tabular_data.data), metadata_index=idx, time_requested=datetime_to_timestamp(tabular_data.time_requested), - time_received=datetime_to_timestamp(tabular_data.time_received) + time_received=datetime_to_timestamp(tabular_data.time_received), ) ) - await stream.send_message(TabularDataByFilterResponse( - data=tabular_response_structs, metadata=tabular_metadata) - ) + await stream.send_message(TabularDataByFilterResponse(data=tabular_response_structs, metadata=tabular_metadata)) self.was_tabular_data_requested = True async def BinaryDataByFilter(self, stream: Stream[BinaryDataByFilterRequest, BinaryDataByFilterResponse]) -> None: @@ -527,8 +531,8 @@ async def BinaryDataByFilter(self, stream: Stream[BinaryDataByFilterRequest, Bin await stream.send_message(BinaryDataByFilterResponse()) return self.filter = request.data_request.filter - await stream.send_message(BinaryDataByFilterResponse( - data=[BinaryData(binary=data.data, metadata=data.metadata) for data in self.binary_response]) + await stream.send_message( + BinaryDataByFilterResponse(data=[BinaryData(binary=data.data, metadata=data.metadata) for data in self.binary_response]) ) self.was_binary_data_requested = True @@ -536,8 +540,8 @@ async def BinaryDataByIDs(self, stream: Stream[BinaryDataByIDsRequest, BinaryDat request = await stream.recv_message() assert request is not None self.binary_ids = request.binary_ids - await stream.send_message(BinaryDataByIDsResponse( - data=[BinaryData(binary=data.data, metadata=data.metadata) for data in self.binary_response]) + await stream.send_message( + BinaryDataByIDsResponse(data=[BinaryData(binary=data.data, metadata=data.metadata) for data in self.binary_response]) ) async def DeleteTabularDataByFilter(self, stream: Stream[DeleteTabularDataByFilterRequest, DeleteTabularDataByFilterResponse]) -> None: diff --git a/tests/test_slam.py b/tests/test_slam.py index 254b61df0..d5bb5be9a 100644 --- a/tests/test_slam.py +++ b/tests/test_slam.py @@ -7,6 +7,8 @@ from viam.proto.service.slam import ( GetInternalStateRequest, GetInternalStateResponse, + GetLatestMapInfoRequest, + GetLatestMapInfoResponse, GetPointCloudMapRequest, GetPointCloudMapResponse, GetPositionRequest, @@ -39,6 +41,11 @@ async def test_get_position(self): pos = await self.slam.get_position() assert pos == MockSLAM.POSITION + @pytest.mark.asyncio + async def test_get_latest_map_info(self): + time = await self.slam.get_latest_map_info() + assert time == MockSLAM.LAST_UPDATE + @pytest.mark.asyncio async def test_do(self): command = {"command": "args"} @@ -80,6 +87,14 @@ async def test_get_position(self): response: GetPositionResponse = await client.GetPosition(request) assert response.pose == MockSLAM.POSITION + @pytest.mark.asyncio + async def test_get_latest_map_info(self): + async with ChannelFor([self.service]) as channel: + client = SLAMServiceStub(channel) + request = GetLatestMapInfoRequest(name=self.name) + response: GetLatestMapInfoResponse = await client.GetLatestMapInfo(request) + assert response.last_map_update.ToDatetime() == MockSLAM.LAST_UPDATE + @pytest.mark.asyncio async def test_do(self): async with ChannelFor([self.service]) as channel: @@ -124,6 +139,13 @@ async def test_get_position(self): response = await client.get_position() assert response == MockSLAM.POSITION + @pytest.mark.asyncio + async def test_get_latest_map_info(self): + async with ChannelFor([self.service]) as channel: + client = SLAMClient(self.name, channel) + response = await client.get_latest_map_info() + assert response == MockSLAM.LAST_UPDATE + @pytest.mark.asyncio async def test_do(self): async with ChannelFor([self.service]) as channel: