RSDK-6011 - Add Motion Expose Paths project RPC methods (#500)

Author: nicksanford

src/viam/services/motion/client.py

@@ -15,16 +15,24 @@
 )
 from viam.proto.service.motion import (
     Constraints,
+    GetPlanRequest,
+    GetPlanResponse,
     GetPoseRequest,
     GetPoseResponse,
+    ListPlanStatusesRequest,
+    ListPlanStatusesResponse,
     MotionConfiguration,
     MotionServiceStub,
+    MoveOnGlobeNewRequest,
+    MoveOnGlobeNewResponse,
     MoveOnGlobeRequest,
     MoveOnGlobeResponse,
     MoveOnMapRequest,
     MoveOnMapResponse,
     MoveRequest,
     MoveResponse,
+    StopPlanRequest,
+    StopPlanResponse,
 )
 from viam.resource.rpc_client_base import ReconfigurableResourceRPCClientBase
 from viam.resource.types import RESOURCE_NAMESPACE_RDK, RESOURCE_TYPE_SERVICE, Subtype
@@ -108,10 +116,10 @@ async def move_on_globe(
             component_name (ResourceName): The component to move
             destination (GeoPoint): The destination point
             movement_sensor_name (ResourceName): The ``MovementSensor`` which will be used to check robot location
-            obstacles (Optional[Sequence[GeoObstacle]], optional): Obstacles to be considered for motion planning. Defaults to None.
-            heading (Optional[float], optional): Compass heading to achieve at the destination, in degrees [0-360). Defaults to None.
-            linear_meters_per_sec (Optional[float], optional): Linear velocity to target when moving. Defaults to None.
-            angular_deg_per_sec (Optional[float], optional): Angular velocity to target when turning. Defaults to None.
+            obstacles (Optional[Sequence[GeoObstacle]]): Obstacles to be considered for motion planning. Defaults to None.
+            heading (Optional[float]): Compass heading to achieve at the destination, in degrees [0-360). Defaults to None.
+            linear_meters_per_sec (Optional[float]): Linear velocity to target when moving. Defaults to None.
+            angular_deg_per_sec (Optional[float]): Angular velocity to target when turning. Defaults to None.
 
         Returns:
             bool: Whether the request was successful
@@ -131,6 +139,57 @@ async def move_on_globe(
         response: MoveOnGlobeResponse = await self.client.MoveOnGlobe(request, timeout=timeout)
         return response.success
 
+    async def move_on_globe_new(
+        self,
+        component_name: ResourceName,
+        destination: GeoPoint,
+        movement_sensor_name: ResourceName,
+        obstacles: Optional[Sequence[GeoObstacle]] = None,
+        heading: Optional[float] = None,
+        configuration: Optional[MotionConfiguration] = None,
+        *,
+        extra: Optional[Mapping[str, ValueTypes]] = None,
+        timeout: Optional[float] = None,
+    ) -> str:
+        """
+        **Experimental**: use move_on_globe instead.
+        Move a component to a specific latitude and longitude, using a ``MovementSensor`` to check the location.
+
+        ``move_on_globe_new()`` is non blocking, meaning the motion service will move the component to the destination
+        GPS point after ``move_on_globe_new()`` returns.
+
+        Each successful ``move_on_globe_new()`` call retuns a unique ExectionID which you can use to identify all plans
+        generated durring the ``move_on_globe_new()`` call.
+
+        You can monitor the progress of the ``move_on_globe_new()`` call by querying ``get_plan()`` and ``list_plan_statuses()``.
+
+        Args:
+            component_name (ResourceName): The component to move
+            destination (GeoPoint): The destination point
+            movement_sensor_name (ResourceName): The ``MovementSensor`` which will be used to check robot location
+            obstacles (Optional[Sequence[GeoObstacle]]): Obstacles to be considered for motion planning. Defaults to None.
+            heading (Optional[float]): Compass heading to achieve at the destination, in degrees [0-360]. Defaults to None.
+            linear_meters_per_sec (Optional[float]): Linear velocity to target when moving. Defaults to None.
+            angular_deg_per_sec (Optional[float]): Angular velocity to target when turning. Defaults to None.
+
+        Returns:
+            str: ExecutionID of the move_on_globe_new call, which can be used to track execution progress.
+        """
+        if extra is None:
+            extra = {}
+        request = MoveOnGlobeNewRequest(
+            name=self.name,
+            component_name=component_name,
+            destination=destination,
+            movement_sensor_name=movement_sensor_name,
+            obstacles=obstacles,
+            heading=heading,
+            motion_configuration=configuration,
+            extra=dict_to_struct(extra),
+        )
+        response: MoveOnGlobeNewResponse = await self.client.MoveOnGlobeNew(request, timeout=timeout)
+        return response.execution_id
+
     async def move_on_map(
         self,
         component_name: ResourceName,
@@ -140,7 +199,8 @@ async def move_on_map(
         extra: Optional[Mapping[str, ValueTypes]] = None,
         timeout: Optional[float] = None,
     ) -> bool:
-        """Move a component to a specific pose, using a ``SlamService`` for the SLAM map
+        """
+        Move a component to a specific pose, using a ``SlamService`` for the SLAM map
 
         Args:
             component_name (ResourceName): The component to move
@@ -162,6 +222,117 @@ async def move_on_map(
         response: MoveOnMapResponse = await self.client.MoveOnMap(request, timeout=timeout)
         return response.success
 
+    async def stop_plan(
+        self,
+        component_name: ResourceName,
+        *,
+        extra: Optional[Mapping[str, ValueTypes]] = None,
+        timeout: Optional[float] = None,
+    ):
+        """**Experimental**
+        Stop a component being moved by an in progress ``move_on_globe_new()`` call.
+
+        Args:
+            component_name (ResourceName): The component to stop
+
+        Returns:
+            None
+        """
+        if extra is None:
+            extra = {}
+
+        request = StopPlanRequest(
+            name=self.name,
+            component_name=component_name,
+            extra=dict_to_struct(extra),
+        )
+        _: StopPlanResponse = await self.client.StopPlan(request, timeout=timeout)
+        return
+
+    async def get_plan(
+        self,
+        component_name: ResourceName,
+        last_plan_only: bool = False,
+        execution_id: Optional[str] = None,
+        *,
+        extra: Optional[Mapping[str, ValueTypes]] = None,
+        timeout: Optional[float] = None,
+    ) -> GetPlanResponse:
+        """**Experimental**
+        By default: returns the plan history of the most recent ``move_on_globe_new()`` call to move a component.
+
+        The plan history for executions before the most recent can be requested by providing an ExecutionID in the request.
+
+        Returns a result if both of the following conditions are met:
+
+        - the execution (call to ``move_on_globe_new()``) is still executing **or** changed state within the last 24 hours
+        - the robot has not reinitialized
+
+        Plans never change.
+
+        Replans always create new plans.
+
+        Replans share the ExecutionID of the previously executing plan.
+
+        All repeated fields are in time ascending order.
+
+        Args:
+            component_name (ResourceName): The component to stop
+            last_plan_only (Optional[bool]): If supplied, the response will only return the last plan for the component / execution
+            execution_id (Optional[str]): If supplied, the response will only return plans with the provided execution_id
+
+        Returns:
+            ``GetPlanResponse`` (GetPlanResponse): The current PlanWithStatus & replan history which matches the request
+        """
+        if extra is None:
+            extra = {}
+
+        if last_plan_only is None:
+            last_plan_only = False
+        request = GetPlanRequest(
+            name=self.name,
+            component_name=component_name,
+            last_plan_only=last_plan_only,
+            execution_id=execution_id,
+            extra=dict_to_struct(extra),
+        )
+        response: GetPlanResponse = await self.client.GetPlan(request, timeout=timeout)
+        return response
+
+    async def list_plan_statuses(
+        self,
+        only_active_plans: bool = False,
+        *,
+        extra: Optional[Mapping[str, ValueTypes]] = None,
+        timeout: Optional[float] = None,
+    ) -> ListPlanStatusesResponse:
+        """**Experimental**
+        Returns the statuses of plans created by `move_on_globe_new()` calls that meet at least one of the following
+        conditions since the motion service initialized:
+
+        - the plan's status is in progress
+        - the plan's status changed state within the last 24 hours
+
+        All repeated fields are in chronological order.
+
+        Args:
+            only_active_plans (Optional[bool]):  If supplied, the response will filter out any plans that are not executing
+
+        Returns:
+            ``ListPlanStatusesResponse`` (ListPlanStatusesResponse): List of last known statuses with the
+            associated IDs of all plans within the TTL ordered by timestamp in ascending order
+        """
+        if extra is None:
+            extra = {}
+
+        request = ListPlanStatusesRequest(
+            name=self.name,
+            only_active_plans=only_active_plans,
+            extra=dict_to_struct(extra),
+        )
+        response: ListPlanStatusesResponse = await self.client.ListPlanStatuses(request, timeout=timeout)
+        return response
+
     async def get_pose(
         self,
         component_name: ResourceName,

tests/mocks/services.py

@@ -443,9 +443,13 @@ def __init__(
         self,
         move_responses: Dict[str, bool],
         get_pose_responses: Dict[str, PoseInFrame],
+        get_plan_response: GetPlanResponse,
+        list_plan_statuses_response: ListPlanStatusesResponse
     ):
         self.move_responses = move_responses
         self.get_pose_responses = get_pose_responses
+        self.get_plan_response = get_plan_response
+        self.list_plan_statuses_response = list_plan_statuses_response
         self.constraints: Optional[Constraints] = None
         self.extra: Optional[Mapping[str, Any]] = None
         self.timeout: Optional[float] = None
@@ -495,16 +499,45 @@ async def GetPose(self, stream: Stream[GetPoseRequest, GetPoseResponse]) -> None
         await stream.send_message(response)
 
     async def MoveOnGlobeNew(self, stream: Stream[MoveOnGlobeNewRequest, MoveOnGlobeNewResponse]) -> None:
-        raise NotImplementedError()
+        request = await stream.recv_message()
+        assert request is not None
+        self.component_name = request.component_name
+        self.destination = request.destination
+        self.movement_sensor = request.movement_sensor_name
+        self.obstacles = request.obstacles
+        self.heading = request.heading
+        self.configuration = request.motion_configuration
+        self.extra = struct_to_dict(request.extra)
+        self.timeout = stream.deadline.time_remaining() if stream.deadline else None
+        self.execution_id = "some_execution_id"
+        await stream.send_message(MoveOnGlobeNewResponse(execution_id=self.execution_id))
 
     async def StopPlan(self, stream: Stream[StopPlanRequest, StopPlanResponse]) -> None:
-        raise NotImplementedError()
+        request = await stream.recv_message()
+        assert request is not None
+        self.component_name = request.component_name
+        self.extra = struct_to_dict(request.extra)
+        self.timeout = stream.deadline.time_remaining() if stream.deadline else None
+        await stream.send_message(StopPlanResponse())
 
     async def ListPlanStatuses(self, stream: Stream[ListPlanStatusesRequest, ListPlanStatusesResponse]) -> None:
-        raise NotImplementedError()
+        request = await stream.recv_message()
+        assert request is not None
+        self.only_active_plans = request.only_active_plans
+        self.extra = struct_to_dict(request.extra)
+        self.timeout = stream.deadline.time_remaining() if stream.deadline else None
+        response = self.list_plan_statuses_response
+        await stream.send_message(response)
 
     async def GetPlan(self, stream: Stream[GetPlanRequest, GetPlanResponse]) -> None:
-        raise NotImplementedError()
+        request = await stream.recv_message()
+        assert request is not None
+        self.component_name = request.component_name
+        self.last_plan_only = request.last_plan_only
+        self.execution_id = request.execution_id
+        self.extra = struct_to_dict(request.extra)
+        self.timeout = stream.deadline.time_remaining() if stream.deadline else None
+        await stream.send_message(self.get_plan_response)
 
     async def DoCommand(self, stream: Stream[DoCommandRequest, DoCommandResponse]) -> None:
         request = await stream.recv_message()