Module for autonomous docking

spot_wrapper/spot_docking.py

@@ -0,0 +1,67 @@
+import logging
+import typing
+
+from bosdyn.api.docking import docking_pb2
+from bosdyn.client import robot_command
+from bosdyn.client.docking import DockingClient, blocking_dock_robot, blocking_undock
+from bosdyn.client.robot import Robot
+
+from spot_wrapper.wrapper_helpers import RobotState, RobotCommandData
+
+
+class SpotDocking:
+    """
+    Interactions with spot's autonomous docking station
+    """
+
+    def __init__(
+        self,
+        robot: Robot,
+        logger: logging.Logger,
+        robot_state: RobotState,
+        command_data: RobotCommandData,
+        docking_client: DockingClient,
+        robot_command_client: robot_command.RobotCommandClient,
+    ) -> None:
+        self._robot = robot
+        self._logger = logger
+        self._command_data = command_data
+        self._docking_client: DockingClient = docking_client
+        self._robot_command_client = robot_command_client
+        self._robot_state = robot_state
+
+    def dock(self, dock_id: int) -> typing.Tuple[bool, str]:
+        """Dock the robot to the docking station with fiducial ID [dock_id]."""
+        try:
+            # Make sure we're powered on and standing
+            self._robot.power_on()
+            if not self._robot_state.is_standing:
+                robot_command.blocking_stand(
+                    command_client=self._robot_command_client, timeout_sec=10
+                )
+                self._logger.info("Spot is standing")
+            else:
+                self._logger.info("Spot is already standing")
+            # Dock the robot
+            self._command_data.last_docking_command = dock_id
+            blocking_dock_robot(self._robot, dock_id)
+            self._command_data.last_docking_command = None
+            return True, "Success"
+        except Exception as e:
+            return False, f"Exception while trying to dock: {e}"
+
+    def undock(self, timeout: int = 20) -> typing.Tuple[bool, str]:
+        """Power motors on and undock the robot from the station."""
+        try:
+            # Maker sure we're powered on
+            self._robot.power_on()
+            # Undock the robot
+            blocking_undock(self._robot, timeout)
+            return True, "Success"
+        except Exception as e:
+            return False, f"Exception while trying to undock: {e}"
+
+    def get_docking_state(self, **kwargs) -> docking_pb2.DockState:
+        """Get docking state of robot."""
+        state = self._docking_client.get_docking_state(**kwargs)
+        return state

spot_wrapper/wrapper.py

@@ -31,7 +31,7 @@
 from bosdyn.client import math_helpers
 from bosdyn.client import robot_command
 from bosdyn.client.async_tasks import AsyncPeriodicQuery, AsyncTasks
-from bosdyn.client.docking import DockingClient, blocking_dock_robot, blocking_undock
+from bosdyn.client.docking import DockingClient
 from bosdyn.client.estop import (
     EstopClient,
     EstopEndpoint,
@@ -83,9 +83,12 @@
 from bosdyn.api import basic_command_pb2
 from google.protobuf.timestamp_pb2 import Timestamp
 
+from .spot_docking import SpotDocking
 from .spot_eap import SpotEAP
 from .spot_world_objects import SpotWorldObjects
 
+from .wrapper_helpers import RobotCommandData, RobotState
+
 
 front_image_sources = [
     "frontleft_fisheye_image",
@@ -533,36 +536,6 @@ def wrapper_try_claim(self, *args, **kwargs):
     return wrapper_try_claim
 
 
-@dataclass()
-class RobotState:
-    """
-    Dataclass which stores information about the robot's state. The values in it may be changed by methods
-    """
-
-    is_sitting: bool = True
-    is_standing: bool = False
-    is_moving: bool = False
-    at_goal: bool = False
-    near_goal: bool = False
-
-
-@dataclass()
-class RobotCommandData:
-    """
-    Store data about the commands the wrapper sends to the SDK. Running a command returns an integer value
-    representing that command's ID. These values are used to monitor the progress of the command and modify attributes
-    of RobotState accordingly. The values should be reset to none when the command completes.
-    """
-
-    last_stand_command: typing.Optional[int] = None
-    last_sit_command: typing.Optional[int] = None
-    last_docking_command: typing.Optional[int] = None
-    last_trajectory_command: typing.Optional[int] = None
-    # Was the last trajectory command requested to be precise
-    last_trajectory_command_precise: typing.Optional[bool] = None
-    last_velocity_command_time: typing.Optional[float] = None
-
-
 class SpotWrapper:
     """Generic wrapper class to encompass release 1.1.4 API features as well as maintaining leases automatically"""
 
@@ -908,6 +881,15 @@ def __init__(
             self._estop_monitor,
         ]
 
+        self._spot_docking = SpotDocking(
+            self._robot,
+            self._logger,
+            self._state,
+            self._command_data,
+            self._docking_client,
+            self._robot_command_client,
+        )
+
         if self._point_cloud_client:
             self._spot_eap = SpotEAP(
                 self._logger,
@@ -1036,6 +1018,11 @@ def spot_world_objects(self) -> SpotWorldObjects:
         """Return SpotWorldObjects instance"""
         return self._spot_world_objects
 
+    @property
+    def spot_docking(self) -> SpotDocking:
+        """Return SpotDocking instance"""
+        return self._spot_docking
+
     @property
     def world_objects(self) -> world_object_pb2.ListWorldObjectResponse:
         """Return most recent proto from _world_objects_task"""
@@ -2724,35 +2711,6 @@ def _match_edge(self, current_edges, waypoint1, waypoint2):
                     )
         return None
 
-    @try_claim
-    def dock(self, dock_id):
-        """Dock the robot to the docking station with fiducial ID [dock_id]."""
-        try:
-            # Make sure we're powered on and standing
-            self._robot.power_on()
-            self.stand()
-            # Dock the robot
-            self.last_docking_command = dock_id
-            blocking_dock_robot(self._robot, dock_id)
-            self.last_docking_command = None
-            # Necessary to reset this as docking often causes the last stand command to go into an unknown state
-            self.last_stand_command = None
-            return True, "Success"
-        except Exception as e:
-            return False, f"Exception while trying to dock: {e}"
-
-    @try_claim
-    def undock(self, timeout=20):
-        """Power motors on and undock the robot from the station."""
-        try:
-            # Maker sure we're powered on
-            self._robot.power_on()
-            # Undock the robot
-            blocking_undock(self._robot, timeout)
-            return True, "Success"
-        except Exception as e:
-            return False, f"Exception while trying to undock: {e}"
-
     @try_claim
     def execute_dance(self, data):
         if self._is_licensed_for_choreography:
@@ -2782,11 +2740,6 @@ def list_all_dances(self) -> typing.Tuple[bool, str, typing.List[str]]:
         else:
             return False, "Spot is not licensed for choreography", []
 
-    def get_docking_state(self, **kwargs):
-        """Get docking state of robot."""
-        state = self._docking_client.get_docking_state(**kwargs)
-        return state
-
     def update_image_tasks(self, image_name):
         """Adds an async tasks to retrieve images from the specified image source"""
 

spot_wrapper/wrapper_helpers.py

@@ -0,0 +1,34 @@
+"""Helper classes for the wrapper. This file is necessary to prevent circular imports caused by the modules also
+using these classes"""
+import typing
+from dataclasses import dataclass
+
+
+@dataclass()
+class RobotState:
+    """
+    Dataclass which stores information about the robot's state. The values in it may be changed by methods
+    """
+
+    is_sitting: bool = True
+    is_standing: bool = False
+    is_moving: bool = False
+    at_goal: bool = False
+    near_goal: bool = False
+
+
+@dataclass()
+class RobotCommandData:
+    """
+    Store data about the commands the wrapper sends to the SDK. Running a command returns an integer value
+    representing that command's ID. These values are used to monitor the progress of the command and modify attributes
+    of RobotState accordingly. The values should be reset to none when the command completes.
+    """
+
+    last_stand_command: typing.Optional[int] = None
+    last_sit_command: typing.Optional[int] = None
+    last_docking_command: typing.Optional[int] = None
+    last_trajectory_command: typing.Optional[int] = None
+    # Was the last trajectory command requested to be precise
+    last_trajectory_command_precise: typing.Optional[bool] = None
+    last_velocity_command_time: typing.Optional[float] = None