Skip to content

Commit 83d70bb

Browse files
tcappellari-bdaitcappellari-bdai
authored
[SW-949] Add Mission Client in spot_wrapper (#105)
* started building client for mission * filled in spot_mission wrapper * mission client wrapper done; testing/debugging * PR comments edits
1 parent a630665 commit 83d70bb

File tree

2 files changed

+212
-1
lines changed

2 files changed

+212
-1
lines changed

‎spot_wrapper/spot_mission_wrapper.py‎

Lines changed: 192 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,192 @@
1+
import logging
2+
from typing import Optional
3+
4+
from bosdyn.api.mission import nodes_pb2
5+
from bosdyn.client import RpcError, robot_command
6+
from bosdyn.client.lease import LeaseClient, LeaseWallet
7+
from bosdyn.client.robot import Robot
8+
from bosdyn.mission.client import (
9+
CompilationError,
10+
MissionClient,
11+
NoMissionError,
12+
NoMissionPlayingError,
13+
ValidationError,
14+
)
15+
16+
from spot_wrapper.wrapper_helpers import RobotState
17+
18+
19+
class SpotMission:
20+
"""
21+
Allow access to mission functionality through the SDK
22+
"""
23+
24+
def __init__(
25+
self,
26+
robot: Robot,
27+
logger: logging.Logger,
28+
robot_state: RobotState,
29+
mission_client: MissionClient,
30+
robot_command_client: robot_command.RobotCommandClient,
31+
lease_client: LeaseClient,
32+
) -> None:
33+
self._robot = robot
34+
self._logger = logger
35+
self._mission_client: MissionClient = mission_client
36+
self._robot_command_client = robot_command_client
37+
self._lease_client = lease_client
38+
self._robot_state = robot_state
39+
self._spot_check_resp = None
40+
self._lease = None
41+
self._lease_wallet: LeaseWallet = self._lease_client.lease_wallet
42+
43+
def load_mission(self, root: nodes_pb2.Node, leases=None, data_chunk_byte_size: Optional[int] = None):
44+
"""Load a mission
45+
Args:
46+
root: Root node in a mission.
47+
leases: All leases necessary to initialize a mission.
48+
data_chunk_byte_size: Optional max size of each streamed message
49+
Raises:
50+
RpcError: Problem communicating with the robot.
51+
bosdyn.mission.client.CompilationError: The mission failed to compile.
52+
bosdyn.mission.client.ValidationError: The mission failed to validate.
53+
"""
54+
if leases is None:
55+
leases = []
56+
if data_chunk_byte_size:
57+
return self._load_mission_as_chunks(root, leases, data_chunk_byte_size)
58+
try:
59+
return self._mission_client.load_mission_async(root, leases)
60+
except RpcError:
61+
return False, "Could not communicate with the robot"
62+
except CompilationError as e:
63+
return False, f"The mission failed to compile: {e}"
64+
except ValidationError as e:
65+
return False, f"The mission could not be validated: {e}"
66+
67+
def _load_mission_as_chunks(self, root: nodes_pb2.Node, leases=None, data_chunk_byte_size: int = 1000 * 1000):
68+
"""Load a mission onto the robot.
69+
Args:
70+
root: Root node in a mission.
71+
leases: All leases necessary to initialize a mission.
72+
data_chunk_byte_size: max size of each streamed message
73+
Raises:
74+
RpcError: Problem communicating with the robot.
75+
bosdyn.mission.client.CompilationError: The mission failed to compile.
76+
bosdyn.mission.client.ValidationError: The mission failed to validate.
77+
"""
78+
if leases is None:
79+
leases = []
80+
try:
81+
return self._mission_client.load_mission_as_chunks2(root, leases, data_chunk_byte_size)
82+
except RpcError:
83+
return False, "Could not communicate with the robot"
84+
except CompilationError as e:
85+
return False, f"The mission failed to compile: {e}"
86+
except ValidationError as e:
87+
return False, f"The mission could not be validated: {e}"
88+
89+
def get_mission_info(self):
90+
"""Get static information about the loaded mission.
91+
92+
Raises:
93+
RpcError: Problem communicating with the robot.
94+
"""
95+
try:
96+
return self._mission_client.get_info()
97+
except RpcError:
98+
return False, "Could not communicate with the robot"
99+
100+
def play_mission(
101+
self,
102+
pause_time_secs: int,
103+
leases=None,
104+
settings=None,
105+
):
106+
"""Play loaded mission or continue a paused mission
107+
Args:
108+
pause_time_secs: Absolute time when the mission should pause execution. Subsequent RPCs
109+
will override this value, so you can use this to say "if you don't hear from me again,
110+
stop running the mission at this time."
111+
leases: Leases the mission service will need to use. Unlike other clients, these MUST
112+
be specified.
113+
Raises:
114+
RpcError: Problem communicating with the robot.
115+
NoMissionError: No mission Loaded.
116+
"""
117+
if leases is None:
118+
leases = []
119+
try:
120+
return self._mission_client.play_mission_async(pause_time_secs, leases, settings)
121+
except RpcError:
122+
return False, "Could not communicate with the robot"
123+
except NoMissionError:
124+
return False, "No mission loaded"
125+
126+
def get_mission_state(
127+
self,
128+
upper_tick_bound: Optional[int] = None,
129+
lower_tick_bound: Optional[int] = None,
130+
past_ticks: Optional[int] = None,
131+
):
132+
"""Get the state of the current playing mission
133+
Raises:
134+
RpcError: Problem communicating with the robot.
135+
NoMissionPlayingError: No mission playing.
136+
"""
137+
try:
138+
return self._mission_client.get_state_async(upper_tick_bound, lower_tick_bound, past_ticks)
139+
except RpcError:
140+
return False, "Could not communicate with the robot"
141+
except NoMissionPlayingError:
142+
return False, "No mission playing"
143+
144+
def pause_mission(self):
145+
"""Pause the current mission
146+
Raises:
147+
RpcError: Problem communicating with the robot.
148+
NoMissionPlayingError: No mission playing.
149+
"""
150+
try:
151+
return self._mission_client.pause_mission_async()
152+
except RpcError:
153+
return False, "Could not communicate with the robot"
154+
except NoMissionPlayingError:
155+
return False, "No mission playing"
156+
157+
def restart_mission(self, pause_time_secs, leases=None, settings=None):
158+
"""Restart mission from the beginning
159+
Args:
160+
pause_time_secs: Absolute time when the mission should pause execution. Subsequent RPCs
161+
to RestartMission will override this value, so you can use this to say "if you don't hear
162+
from me again, stop running the mission at this time."
163+
leases: Leases the mission service will need to use. Unlike other clients, these MUST
164+
be specified.
165+
Raises:
166+
RpcError: Problem communicating with the robot.
167+
NoMissionError: No Mission Loaded.
168+
bosdyn.mission.client.ValidationError: The mission failed to validate.
169+
"""
170+
if leases is None:
171+
leases = []
172+
try:
173+
return self._mission_client.restart_mission_async(pause_time_secs, leases, settings)
174+
except RpcError:
175+
return False, "Could not communicate with the robot"
176+
except NoMissionError:
177+
return False, "No mission loaded"
178+
except ValidationError as e:
179+
return False, f"The mission could not be validated: {e}"
180+
181+
def stop_mission(self):
182+
"""Stop the current mission
183+
Raises:
184+
RpcError: Problem communicating with the robot.
185+
NoMissionPlayingError: No mission playing.
186+
"""
187+
try:
188+
return self._mission_client.stop_mission_async()
189+
except RpcError:
190+
return False, "Could not communicate with the robot"
191+
except NoMissionPlayingError:
192+
return False, "No mission playing"

‎spot_wrapper/wrapper.py‎

Lines changed: 20 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,7 @@
5555
from bosdyn.client.time_sync import TimeSyncEndpoint
5656
from bosdyn.client.world_object import WorldObjectClient
5757
from bosdyn.geometry import EulerZXY
58+
from bosdyn.mission.client import MissionClient
5859
from google.protobuf.timestamp_pb2 import Timestamp
5960

6061
from .spot_arm import SpotArm
@@ -64,6 +65,7 @@
6465
from .spot_eap import SpotEAP
6566
from .spot_graph_nav import SpotGraphNav
6667
from .spot_images import SpotImages
68+
from .spot_mission_wrapper import SpotMission
6769
from .spot_world_objects import SpotWorldObjects
6870
from .wrapper_helpers import ClaimAndPowerDecorator, RobotCommandData, RobotState
6971

@@ -392,7 +394,9 @@ def __init__(
392394
self._command_data = RobotCommandData()
393395

394396
try:
395-
self._sdk = create_standard_sdk(SPOT_CLIENT_NAME, cert_resource_glob=cert_resource_glob)
397+
self._sdk = create_standard_sdk(
398+
SPOT_CLIENT_NAME, service_clients=[MissionClient], cert_resource_glob=cert_resource_glob
399+
)
396400
except Exception as e:
397401
self._logger.error("Error creating SDK object: %s", e)
398402
self._valid = False
@@ -437,6 +441,7 @@ def __init__(
437441
self._estop_client = self._robot.ensure_client(EstopClient.default_service_name)
438442
self._docking_client = self._robot.ensure_client(DockingClient.default_service_name)
439443
self._spot_check_client = self._robot.ensure_client(SpotCheckClient.default_service_name)
444+
self._mission_client = self._robot.ensure_client(MissionClient.default_service_name)
440445
self._license_client = self._robot.ensure_client(LicenseClient.default_service_name)
441446
if self._robot.has_arm():
442447
self._gripper_cam_param_client = self._robot.ensure_client(
@@ -521,6 +526,15 @@ def __init__(
521526
self._lease_client,
522527
)
523528

529+
self._spot_mission = SpotMission(
530+
self._robot,
531+
self._logger,
532+
self._state,
533+
self._mission_client,
534+
self._robot_command_client,
535+
self._lease_client,
536+
)
537+
524538
if self._robot.has_arm():
525539
self._spot_arm = SpotArm(
526540
self._robot,
@@ -726,6 +740,11 @@ def spot_check(self) -> SpotCheck:
726740
"""Return SpotCheck instance"""
727741
return self._spot_check
728742

743+
@property
744+
def spot_mission(self) -> SpotMission:
745+
"""Return SpotMission instance"""
746+
return self._spot_mission
747+
729748
@property
730749
def spot_eap_lidar(self) -> typing.Optional[SpotEAP]:
731750
"""Return SpotEAP instance"""

0 commit comments

Comments
 (0)