isar 1.33.9__py3-none-any.whl → 1.34.1__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
Potentially problematic release.
This version of isar might be problematic. Click here for more details.
- isar/apis/models/models.py +2 -5
- isar/config/settings.py +0 -7
- isar/models/events.py +9 -10
- isar/modules.py +5 -11
- isar/robot/robot.py +109 -24
- isar/robot/robot_monitor_mission.py +399 -0
- isar/robot/robot_status.py +0 -10
- isar/robot/robot_stop_mission.py +7 -7
- isar/robot/robot_upload_inspection.py +80 -0
- isar/services/utilities/scheduling_utilities.py +30 -10
- isar/state_machine/state_machine.py +7 -223
- isar/state_machine/states/going_to_lockdown.py +23 -34
- isar/state_machine/states/home.py +4 -9
- isar/state_machine/states/monitor.py +20 -38
- isar/state_machine/states/paused.py +2 -2
- isar/state_machine/states/pausing.py +6 -22
- isar/state_machine/states/pausing_return_home.py +6 -22
- isar/state_machine/states/returning_home.py +40 -37
- isar/state_machine/states/stopping.py +2 -22
- isar/state_machine/states/stopping_go_to_lockdown.py +1 -21
- isar/state_machine/states/stopping_return_home.py +0 -2
- isar/state_machine/transitions/functions/fail_mission.py +1 -9
- isar/state_machine/transitions/functions/finish_mission.py +2 -32
- isar/state_machine/transitions/functions/pause.py +8 -7
- isar/state_machine/transitions/functions/resume.py +3 -12
- isar/state_machine/transitions/functions/return_home.py +1 -16
- isar/state_machine/transitions/functions/robot_status.py +7 -12
- isar/state_machine/transitions/functions/start_mission.py +2 -44
- isar/state_machine/transitions/functions/stop.py +4 -33
- isar/state_machine/transitions/mission.py +2 -17
- isar/state_machine/transitions/return_home.py +3 -24
- isar/state_machine/transitions/robot_status.py +9 -0
- isar/state_machine/utils/common_event_handlers.py +4 -96
- {isar-1.33.9.dist-info → isar-1.34.1.dist-info}/METADATA +1 -63
- {isar-1.33.9.dist-info → isar-1.34.1.dist-info}/RECORD +41 -42
- robot_interface/models/mission/task.py +0 -10
- robot_interface/robot_interface.py +25 -1
- isar/mission_planner/sequential_task_selector.py +0 -23
- isar/mission_planner/task_selector_interface.py +0 -31
- isar/robot/robot_task_status.py +0 -87
- {isar-1.33.9.dist-info → isar-1.34.1.dist-info}/WHEEL +0 -0
- {isar-1.33.9.dist-info → isar-1.34.1.dist-info}/entry_points.txt +0 -0
- {isar-1.33.9.dist-info → isar-1.34.1.dist-info}/licenses/LICENSE +0 -0
- {isar-1.33.9.dist-info → isar-1.34.1.dist-info}/top_level.txt +0 -0
|
@@ -3,20 +3,14 @@ import logging
|
|
|
3
3
|
from collections import deque
|
|
4
4
|
from datetime import datetime, timezone
|
|
5
5
|
from threading import Event
|
|
6
|
-
from typing import Deque, List, Optional
|
|
6
|
+
from typing import Deque, List, Optional
|
|
7
7
|
|
|
8
8
|
from transitions import Machine
|
|
9
9
|
from transitions.core import State
|
|
10
10
|
|
|
11
|
-
from isar.apis.models.models import ControlMissionResponse
|
|
12
11
|
from isar.config.settings import settings
|
|
13
|
-
from isar.mission_planner.task_selector_interface import (
|
|
14
|
-
TaskSelectorInterface,
|
|
15
|
-
TaskSelectorStop,
|
|
16
|
-
)
|
|
17
12
|
from isar.models.events import Events, SharedState
|
|
18
13
|
from isar.models.status import IsarStatus
|
|
19
|
-
from isar.services.service_connections.mqtt.mqtt_client import props_expiry
|
|
20
14
|
from isar.state_machine.states.await_next_mission import AwaitNextMission
|
|
21
15
|
from isar.state_machine.states.blocked_protective_stop import BlockedProtectiveStop
|
|
22
16
|
from isar.state_machine.states.going_to_lockdown import GoingToLockdown
|
|
@@ -39,23 +33,13 @@ from isar.state_machine.states_enum import States
|
|
|
39
33
|
from isar.state_machine.transitions.mission import get_mission_transitions
|
|
40
34
|
from isar.state_machine.transitions.return_home import get_return_home_transitions
|
|
41
35
|
from isar.state_machine.transitions.robot_status import get_robot_status_transitions
|
|
42
|
-
from robot_interface.models.exceptions.robot_exceptions import (
|
|
43
|
-
ErrorMessage,
|
|
44
|
-
RobotException,
|
|
45
|
-
RobotRetrieveInspectionException,
|
|
46
|
-
)
|
|
47
|
-
from robot_interface.models.inspection.inspection import Inspection
|
|
48
36
|
from robot_interface.models.mission.mission import Mission
|
|
49
|
-
from robot_interface.models.mission.status import TaskStatus
|
|
50
|
-
from robot_interface.models.mission.task import TASKS, InspectionTask, Task
|
|
51
37
|
from robot_interface.robot_interface import RobotInterface
|
|
52
38
|
from robot_interface.telemetry.mqtt_client import MqttClientInterface
|
|
53
39
|
from robot_interface.telemetry.payloads import (
|
|
54
40
|
InterventionNeededPayload,
|
|
55
41
|
IsarStatusPayload,
|
|
56
42
|
MissionAbortedPayload,
|
|
57
|
-
MissionPayload,
|
|
58
|
-
TaskPayload,
|
|
59
43
|
)
|
|
60
44
|
from robot_interface.utilities.json_service import EnhancedJSONEncoder
|
|
61
45
|
|
|
@@ -69,7 +53,6 @@ class StateMachine(object):
|
|
|
69
53
|
shared_state: SharedState,
|
|
70
54
|
robot: RobotInterface,
|
|
71
55
|
mqtt_publisher: MqttClientInterface,
|
|
72
|
-
task_selector: TaskSelectorInterface,
|
|
73
56
|
sleep_time: float = settings.FSM_SLEEP_TIME,
|
|
74
57
|
stop_robot_attempts_limit: int = settings.STOP_ROBOT_ATTEMPTS_LIMIT,
|
|
75
58
|
transitions_log_length: int = settings.STATE_TRANSITIONS_LOG_LENGTH,
|
|
@@ -98,7 +81,6 @@ class StateMachine(object):
|
|
|
98
81
|
self.shared_state: SharedState = shared_state
|
|
99
82
|
self.robot: RobotInterface = robot
|
|
100
83
|
self.mqtt_publisher: Optional[MqttClientInterface] = mqtt_publisher
|
|
101
|
-
self.task_selector: TaskSelectorInterface = task_selector
|
|
102
84
|
|
|
103
85
|
self.signal_state_machine_to_stop: Event = Event()
|
|
104
86
|
|
|
@@ -165,23 +147,14 @@ class StateMachine(object):
|
|
|
165
147
|
self.stop_robot_attempts_limit: int = stop_robot_attempts_limit
|
|
166
148
|
self.sleep_time: float = sleep_time
|
|
167
149
|
|
|
168
|
-
self.current_mission: Optional[Mission] = None
|
|
169
|
-
self.current_task: Optional[TASKS] = None
|
|
170
|
-
|
|
171
|
-
self.mission_ongoing: bool = False
|
|
172
|
-
|
|
173
150
|
self.current_state: State = States(self.state) # type: ignore
|
|
174
151
|
|
|
175
|
-
self.awaiting_task_status: bool = False
|
|
176
|
-
|
|
177
152
|
self.transitions_log_length: int = transitions_log_length
|
|
178
153
|
self.transitions_list: Deque[States] = deque([], self.transitions_log_length)
|
|
179
154
|
|
|
180
155
|
#################################################################################
|
|
181
156
|
|
|
182
|
-
def
|
|
183
|
-
self.publish_mission_status()
|
|
184
|
-
self.log_mission_overview(mission=self.current_mission)
|
|
157
|
+
def print_transitions(self) -> None:
|
|
185
158
|
state_transitions: str = ", ".join(
|
|
186
159
|
[
|
|
187
160
|
f"\n {transition}" if (i + 1) % 10 == 0 else f"{transition}"
|
|
@@ -189,7 +162,7 @@ class StateMachine(object):
|
|
|
189
162
|
]
|
|
190
163
|
)
|
|
191
164
|
self.logger.info("State transitions:\n %s", state_transitions)
|
|
192
|
-
self.
|
|
165
|
+
self.transitions = []
|
|
193
166
|
|
|
194
167
|
def begin(self):
|
|
195
168
|
"""Starts the state machine. Transitions into unknown status state."""
|
|
@@ -199,20 +172,6 @@ class StateMachine(object):
|
|
|
199
172
|
self.logger.info("Stopping state machine")
|
|
200
173
|
self.signal_state_machine_to_stop.set()
|
|
201
174
|
|
|
202
|
-
def iterate_current_task(self):
|
|
203
|
-
if self.current_task is None:
|
|
204
|
-
raise ValueError("No current task is set")
|
|
205
|
-
|
|
206
|
-
if self.current_task.is_finished():
|
|
207
|
-
try:
|
|
208
|
-
self.current_task = self.task_selector.next_task()
|
|
209
|
-
self.current_task.status = TaskStatus.InProgress
|
|
210
|
-
self.publish_task_status(task=self.current_task)
|
|
211
|
-
except TaskSelectorStop:
|
|
212
|
-
# Indicates that all tasks are finished
|
|
213
|
-
self.current_task = None
|
|
214
|
-
self.send_task_status()
|
|
215
|
-
|
|
216
175
|
def battery_level_is_above_mission_start_threshold(self):
|
|
217
176
|
if not self.shared_state.robot_battery_level.check():
|
|
218
177
|
self.logger.warning("Battery level is None")
|
|
@@ -226,45 +185,19 @@ class StateMachine(object):
|
|
|
226
185
|
"""Updates the current state of the state machine."""
|
|
227
186
|
self.current_state = States(self.state) # type: ignore
|
|
228
187
|
self.shared_state.state.update(self.current_state)
|
|
229
|
-
self.
|
|
188
|
+
self.transitions_list.append(self.current_state)
|
|
230
189
|
self.logger.info("State: %s", self.current_state)
|
|
231
190
|
self.publish_status()
|
|
232
191
|
|
|
233
|
-
def reset_state_machine(self) -> None:
|
|
234
|
-
self.logger.info("Resetting state machine")
|
|
235
|
-
self.current_task = None
|
|
236
|
-
self.send_task_status()
|
|
237
|
-
self.current_mission = None
|
|
238
|
-
self.mission_ongoing = False
|
|
239
|
-
|
|
240
192
|
def start_mission(self, mission: Mission):
|
|
241
193
|
"""Starts a scheduled mission."""
|
|
242
|
-
self.
|
|
243
|
-
|
|
244
|
-
self.task_selector.initialize(tasks=self.current_mission.tasks)
|
|
245
|
-
|
|
246
|
-
def send_task_status(self):
|
|
247
|
-
self.shared_state.state_machine_current_task.update(self.current_task)
|
|
248
|
-
|
|
249
|
-
def report_task_status(self, task: Task) -> None:
|
|
250
|
-
if task.status == TaskStatus.Failed:
|
|
251
|
-
self.logger.warning(
|
|
252
|
-
f"Task: {str(task.id)[:8]} was reported as failed by the robot"
|
|
253
|
-
)
|
|
254
|
-
elif task.status == TaskStatus.Successful:
|
|
255
|
-
self.logger.info(
|
|
256
|
-
f"{type(task).__name__} task: {str(task.id)[:8]} completed"
|
|
257
|
-
)
|
|
258
|
-
else:
|
|
259
|
-
self.logger.info(
|
|
260
|
-
f"Task: {str(task.id)[:8]} was reported as task.status by the robot"
|
|
261
|
-
)
|
|
194
|
+
self.events.state_machine_events.start_mission.trigger_event(mission)
|
|
262
195
|
|
|
263
196
|
def publish_mission_aborted(self, reason: str, can_be_continued: bool) -> None:
|
|
264
197
|
if not self.mqtt_publisher:
|
|
265
198
|
return
|
|
266
199
|
|
|
267
|
-
if self.
|
|
200
|
+
if self.shared_state.mission_id.check() is None:
|
|
268
201
|
self.logger.warning(
|
|
269
202
|
"Could not publish mission aborted message. No ongoing mission."
|
|
270
203
|
)
|
|
@@ -273,7 +206,7 @@ class StateMachine(object):
|
|
|
273
206
|
payload: MissionAbortedPayload = MissionAbortedPayload(
|
|
274
207
|
isar_id=settings.ISAR_ID,
|
|
275
208
|
robot_name=settings.ROBOT_NAME,
|
|
276
|
-
mission_id=self.
|
|
209
|
+
mission_id=self.shared_state.mission_id.check(),
|
|
277
210
|
reason=reason,
|
|
278
211
|
can_be_continued=can_be_continued,
|
|
279
212
|
timestamp=datetime.now(timezone.utc),
|
|
@@ -286,68 +219,6 @@ class StateMachine(object):
|
|
|
286
219
|
retain=True,
|
|
287
220
|
)
|
|
288
221
|
|
|
289
|
-
def publish_mission_status(self) -> None:
|
|
290
|
-
if not self.mqtt_publisher:
|
|
291
|
-
return
|
|
292
|
-
|
|
293
|
-
error_message: Optional[ErrorMessage] = None
|
|
294
|
-
if self.current_mission:
|
|
295
|
-
if self.current_mission.error_message:
|
|
296
|
-
error_message = self.current_mission.error_message
|
|
297
|
-
|
|
298
|
-
payload: MissionPayload = MissionPayload(
|
|
299
|
-
isar_id=settings.ISAR_ID,
|
|
300
|
-
robot_name=settings.ROBOT_NAME,
|
|
301
|
-
mission_id=self.current_mission.id if self.current_mission else None,
|
|
302
|
-
status=self.current_mission.status if self.current_mission else None,
|
|
303
|
-
error_reason=error_message.error_reason if error_message else None,
|
|
304
|
-
error_description=(
|
|
305
|
-
error_message.error_description if error_message else None
|
|
306
|
-
),
|
|
307
|
-
timestamp=datetime.now(timezone.utc),
|
|
308
|
-
)
|
|
309
|
-
|
|
310
|
-
if self.current_mission:
|
|
311
|
-
self.mqtt_publisher.publish(
|
|
312
|
-
topic=settings.TOPIC_ISAR_MISSION + f"/{self.current_mission.id}",
|
|
313
|
-
payload=json.dumps(payload, cls=EnhancedJSONEncoder),
|
|
314
|
-
qos=1,
|
|
315
|
-
retain=True,
|
|
316
|
-
properties=props_expiry(settings.MQTT_MISSION_AND_TASK_EXPIRY),
|
|
317
|
-
)
|
|
318
|
-
|
|
319
|
-
def publish_task_status(self, task: TASKS) -> None:
|
|
320
|
-
"""Publishes the task status to the MQTT Broker"""
|
|
321
|
-
if not self.mqtt_publisher:
|
|
322
|
-
return
|
|
323
|
-
|
|
324
|
-
error_message: Optional[ErrorMessage] = None
|
|
325
|
-
if task:
|
|
326
|
-
if task.error_message:
|
|
327
|
-
error_message = task.error_message
|
|
328
|
-
|
|
329
|
-
payload: TaskPayload = TaskPayload(
|
|
330
|
-
isar_id=settings.ISAR_ID,
|
|
331
|
-
robot_name=settings.ROBOT_NAME,
|
|
332
|
-
mission_id=self.current_mission.id if self.current_mission else None,
|
|
333
|
-
task_id=task.id if task else None,
|
|
334
|
-
status=task.status if task else None,
|
|
335
|
-
task_type=task.type if task else None,
|
|
336
|
-
error_reason=error_message.error_reason if error_message else None,
|
|
337
|
-
error_description=(
|
|
338
|
-
error_message.error_description if error_message else None
|
|
339
|
-
),
|
|
340
|
-
timestamp=datetime.now(timezone.utc),
|
|
341
|
-
)
|
|
342
|
-
|
|
343
|
-
self.mqtt_publisher.publish(
|
|
344
|
-
topic=settings.TOPIC_ISAR_TASK + f"/{task.id}",
|
|
345
|
-
payload=json.dumps(payload, cls=EnhancedJSONEncoder),
|
|
346
|
-
qos=1,
|
|
347
|
-
retain=True,
|
|
348
|
-
properties=props_expiry(settings.MQTT_MISSION_AND_TASK_EXPIRY),
|
|
349
|
-
)
|
|
350
|
-
|
|
351
222
|
def publish_intervention_needed(self, error_message: str) -> None:
|
|
352
223
|
"""Publishes the intervention needed message to the MQTT Broker"""
|
|
353
224
|
if not self.mqtt_publisher:
|
|
@@ -411,93 +282,6 @@ class StateMachine(object):
|
|
|
411
282
|
else:
|
|
412
283
|
return IsarStatus.Busy
|
|
413
284
|
|
|
414
|
-
def _log_state_transition(self, next_state) -> None:
|
|
415
|
-
"""Logs all state transitions that are not self-transitions."""
|
|
416
|
-
self.transitions_list.append(next_state)
|
|
417
|
-
|
|
418
|
-
def log_mission_overview(self, mission: Mission) -> None:
|
|
419
|
-
"""Log an overview of the tasks in a mission"""
|
|
420
|
-
log_statements: List[str] = []
|
|
421
|
-
for task in mission.tasks:
|
|
422
|
-
log_statements.append(
|
|
423
|
-
f"{type(task).__name__:<20} {str(task.id)[:8]:<32} -- {task.status}"
|
|
424
|
-
)
|
|
425
|
-
log_statement: str = "\n".join(log_statements)
|
|
426
|
-
|
|
427
|
-
self.logger.info("Mission overview:\n%s", log_statement)
|
|
428
|
-
|
|
429
|
-
def _make_control_mission_response(self) -> ControlMissionResponse:
|
|
430
|
-
return ControlMissionResponse(
|
|
431
|
-
mission_id=self.current_mission.id if self.current_mission else None,
|
|
432
|
-
mission_status=(
|
|
433
|
-
self.current_mission.status if self.current_mission else None
|
|
434
|
-
),
|
|
435
|
-
task_id=self.current_task.id if self.current_task else None,
|
|
436
|
-
task_status=self.current_task.status if self.current_task else None,
|
|
437
|
-
)
|
|
438
|
-
|
|
439
|
-
def _queue_empty_response(self) -> None:
|
|
440
|
-
self.events.api_requests.stop_mission.response.trigger_event(
|
|
441
|
-
ControlMissionResponse(
|
|
442
|
-
mission_id="None",
|
|
443
|
-
mission_status="None",
|
|
444
|
-
task_id="None",
|
|
445
|
-
task_status="None",
|
|
446
|
-
)
|
|
447
|
-
)
|
|
448
|
-
|
|
449
|
-
def should_upload_inspections(self) -> bool:
|
|
450
|
-
if settings.UPLOAD_INSPECTIONS_ASYNC:
|
|
451
|
-
return False
|
|
452
|
-
|
|
453
|
-
return (
|
|
454
|
-
self.current_task.is_finished()
|
|
455
|
-
and self.current_task.status == TaskStatus.Successful
|
|
456
|
-
and isinstance(self.current_task, InspectionTask)
|
|
457
|
-
)
|
|
458
|
-
|
|
459
|
-
def queue_inspections_for_upload(
|
|
460
|
-
self, mission: Mission, current_task: InspectionTask, logger: logging.Logger
|
|
461
|
-
) -> None:
|
|
462
|
-
try:
|
|
463
|
-
inspection: Inspection = self.robot.get_inspection(task=current_task)
|
|
464
|
-
if current_task.inspection_id != inspection.id:
|
|
465
|
-
logger.warning(
|
|
466
|
-
f"The inspection_id of task ({current_task.inspection_id}) "
|
|
467
|
-
f"and result ({inspection.id}) is not matching. "
|
|
468
|
-
f"This may lead to confusions when accessing the inspection later"
|
|
469
|
-
)
|
|
470
|
-
|
|
471
|
-
except (RobotRetrieveInspectionException, RobotException) as e:
|
|
472
|
-
error_message: ErrorMessage = ErrorMessage(
|
|
473
|
-
error_reason=e.error_reason, error_description=e.error_description
|
|
474
|
-
)
|
|
475
|
-
self.current_task.error_message = error_message
|
|
476
|
-
logger.error(
|
|
477
|
-
f"Failed to retrieve inspections because: {e.error_description}"
|
|
478
|
-
)
|
|
479
|
-
return
|
|
480
|
-
|
|
481
|
-
except Exception as e:
|
|
482
|
-
logger.error(
|
|
483
|
-
f"Failed to retrieve inspections because of unexpected error: {e}"
|
|
484
|
-
)
|
|
485
|
-
return
|
|
486
|
-
|
|
487
|
-
if not inspection:
|
|
488
|
-
logger.warning(
|
|
489
|
-
f"No inspection result data retrieved for task {str(current_task.id)[:8]}"
|
|
490
|
-
)
|
|
491
|
-
|
|
492
|
-
inspection.metadata.tag_id = current_task.tag_id
|
|
493
|
-
|
|
494
|
-
message: Tuple[Inspection, Mission] = (
|
|
495
|
-
inspection,
|
|
496
|
-
mission,
|
|
497
|
-
)
|
|
498
|
-
self.events.upload_queue.put(message)
|
|
499
|
-
logger.info(f"Inspection result: {str(inspection.id)[:8]} queued for upload")
|
|
500
|
-
|
|
501
285
|
|
|
502
286
|
def main(state_machine: StateMachine):
|
|
503
287
|
"""Starts a state machine instance."""
|
|
@@ -2,13 +2,9 @@ from typing import TYPE_CHECKING, Callable, List, Optional
|
|
|
2
2
|
|
|
3
3
|
from isar.eventhandlers.eventhandler import EventHandlerBase, EventHandlerMapping
|
|
4
4
|
from isar.models.events import Event
|
|
5
|
-
from isar.state_machine.utils.common_event_handlers import
|
|
6
|
-
|
|
7
|
-
|
|
8
|
-
task_status_failed_event_handler,
|
|
9
|
-
)
|
|
10
|
-
from robot_interface.models.exceptions.robot_exceptions import ErrorMessage, ErrorReason
|
|
11
|
-
from robot_interface.models.mission.status import TaskStatus
|
|
5
|
+
from isar.state_machine.utils.common_event_handlers import mission_started_event_handler
|
|
6
|
+
from robot_interface.models.exceptions.robot_exceptions import ErrorMessage
|
|
7
|
+
from robot_interface.models.mission.status import MissionStatus
|
|
12
8
|
|
|
13
9
|
if TYPE_CHECKING:
|
|
14
10
|
from isar.state_machine.state_machine import StateMachine
|
|
@@ -19,15 +15,6 @@ class GoingToLockdown(EventHandlerBase):
|
|
|
19
15
|
def __init__(self, state_machine: "StateMachine"):
|
|
20
16
|
events = state_machine.events
|
|
21
17
|
|
|
22
|
-
def _handle_task_completed(status: TaskStatus):
|
|
23
|
-
if status != TaskStatus.Successful:
|
|
24
|
-
state_machine.current_mission.error_message = ErrorMessage(
|
|
25
|
-
error_reason=ErrorReason.RobotActionException,
|
|
26
|
-
error_description="Lock down mission failed.",
|
|
27
|
-
)
|
|
28
|
-
return state_machine.lockdown_mission_failed # type: ignore
|
|
29
|
-
return state_machine.reached_lockdown # type: ignore
|
|
30
|
-
|
|
31
18
|
def _mission_failed_event_handler(
|
|
32
19
|
event: Event[Optional[ErrorMessage]],
|
|
33
20
|
) -> Optional[Callable]:
|
|
@@ -36,16 +23,27 @@ class GoingToLockdown(EventHandlerBase):
|
|
|
36
23
|
return None
|
|
37
24
|
|
|
38
25
|
state_machine.logger.warning(
|
|
39
|
-
f"Failed to
|
|
40
|
-
f"{str(state_machine.current_mission.id)[:8]} because: "
|
|
26
|
+
f"Failed to go to lockdown because: "
|
|
41
27
|
f"{mission_failed.error_description}"
|
|
42
28
|
)
|
|
43
|
-
state_machine.current_mission.error_message = ErrorMessage(
|
|
44
|
-
error_reason=mission_failed.error_reason,
|
|
45
|
-
error_description=mission_failed.error_description,
|
|
46
|
-
)
|
|
47
29
|
return state_machine.lockdown_mission_failed # type: ignore
|
|
48
30
|
|
|
31
|
+
def _mission_status_event_handler(
|
|
32
|
+
event: Event[MissionStatus],
|
|
33
|
+
) -> Optional[Callable]:
|
|
34
|
+
mission_status: Optional[MissionStatus] = event.consume_event()
|
|
35
|
+
|
|
36
|
+
if mission_status and mission_status not in [
|
|
37
|
+
MissionStatus.InProgress,
|
|
38
|
+
MissionStatus.NotStarted,
|
|
39
|
+
MissionStatus.Paused,
|
|
40
|
+
]:
|
|
41
|
+
if mission_status != MissionStatus.Successful:
|
|
42
|
+
return state_machine.lockdown_mission_failed # type: ignore
|
|
43
|
+
|
|
44
|
+
return state_machine.reached_lockdown # type: ignore
|
|
45
|
+
return None
|
|
46
|
+
|
|
49
47
|
event_handlers: List[EventHandlerMapping] = [
|
|
50
48
|
EventHandlerMapping(
|
|
51
49
|
name="mission_started_event",
|
|
@@ -60,18 +58,9 @@ class GoingToLockdown(EventHandlerBase):
|
|
|
60
58
|
handler=_mission_failed_event_handler,
|
|
61
59
|
),
|
|
62
60
|
EventHandlerMapping(
|
|
63
|
-
name="
|
|
64
|
-
event=events.robot_service_events.
|
|
65
|
-
handler=
|
|
66
|
-
state_machine, _handle_task_completed, event
|
|
67
|
-
),
|
|
68
|
-
),
|
|
69
|
-
EventHandlerMapping(
|
|
70
|
-
name="task_status_event",
|
|
71
|
-
event=events.robot_service_events.task_status_updated,
|
|
72
|
-
handler=lambda event: task_status_event_handler(
|
|
73
|
-
state_machine, _handle_task_completed, event
|
|
74
|
-
),
|
|
61
|
+
name="mission_status_event",
|
|
62
|
+
event=events.robot_service_events.mission_status_updated,
|
|
63
|
+
handler=_mission_status_event_handler,
|
|
75
64
|
),
|
|
76
65
|
]
|
|
77
66
|
super().__init__(
|
|
@@ -31,13 +31,12 @@ class Home(EventHandlerBase):
|
|
|
31
31
|
return state_machine.reached_lockdown # type: ignore
|
|
32
32
|
|
|
33
33
|
def _robot_status_event_handler(
|
|
34
|
-
state_machine: "StateMachine",
|
|
35
34
|
status_changed_event: Event[bool],
|
|
36
|
-
status_event: Event[RobotStatus],
|
|
37
35
|
) -> Optional[Callable]:
|
|
38
|
-
|
|
36
|
+
has_changed = status_changed_event.consume_event()
|
|
37
|
+
if not has_changed:
|
|
39
38
|
return None
|
|
40
|
-
robot_status: Optional[RobotStatus] =
|
|
39
|
+
robot_status: Optional[RobotStatus] = shared_state.robot_status.check()
|
|
41
40
|
if not (
|
|
42
41
|
robot_status == RobotStatus.Available
|
|
43
42
|
or robot_status == RobotStatus.Home
|
|
@@ -66,11 +65,7 @@ class Home(EventHandlerBase):
|
|
|
66
65
|
EventHandlerMapping(
|
|
67
66
|
name="robot_status_event",
|
|
68
67
|
event=events.robot_service_events.robot_status_changed,
|
|
69
|
-
handler=
|
|
70
|
-
state_machine=state_machine,
|
|
71
|
-
status_changed_event=event,
|
|
72
|
-
status_event=shared_state.robot_status,
|
|
73
|
-
),
|
|
68
|
+
handler=_robot_status_event_handler,
|
|
74
69
|
),
|
|
75
70
|
EventHandlerMapping(
|
|
76
71
|
name="send_to_lockdown_event",
|
|
@@ -1,19 +1,14 @@
|
|
|
1
|
-
import logging
|
|
2
|
-
from copy import deepcopy
|
|
3
1
|
from typing import TYPE_CHECKING, Callable, List, Optional
|
|
4
2
|
|
|
5
3
|
from isar.config.settings import settings
|
|
6
4
|
from isar.eventhandlers.eventhandler import EventHandlerBase, EventHandlerMapping
|
|
7
5
|
from isar.models.events import Event
|
|
8
|
-
from isar.services.utilities.threaded_request import ThreadedRequest
|
|
9
6
|
from isar.state_machine.utils.common_event_handlers import (
|
|
10
7
|
mission_failed_event_handler,
|
|
11
8
|
mission_started_event_handler,
|
|
12
9
|
stop_mission_event_handler,
|
|
13
|
-
task_status_event_handler,
|
|
14
|
-
task_status_failed_event_handler,
|
|
15
10
|
)
|
|
16
|
-
from robot_interface.models.mission.status import
|
|
11
|
+
from robot_interface.models.mission.status import MissionStatus
|
|
17
12
|
|
|
18
13
|
if TYPE_CHECKING:
|
|
19
14
|
from isar.state_machine.state_machine import StateMachine
|
|
@@ -22,7 +17,6 @@ if TYPE_CHECKING:
|
|
|
22
17
|
class Monitor(EventHandlerBase):
|
|
23
18
|
|
|
24
19
|
def __init__(self, state_machine: "StateMachine"):
|
|
25
|
-
logger = logging.getLogger("state_machine")
|
|
26
20
|
events = state_machine.events
|
|
27
21
|
shared_state = state_machine.shared_state
|
|
28
22
|
|
|
@@ -32,24 +26,6 @@ class Monitor(EventHandlerBase):
|
|
|
32
26
|
|
|
33
27
|
return state_machine.pause # type: ignore
|
|
34
28
|
|
|
35
|
-
def _handle_task_completed(task_status: TaskStatus):
|
|
36
|
-
if state_machine.should_upload_inspections():
|
|
37
|
-
get_inspection_thread = ThreadedRequest(
|
|
38
|
-
state_machine.queue_inspections_for_upload
|
|
39
|
-
)
|
|
40
|
-
get_inspection_thread.start_thread(
|
|
41
|
-
deepcopy(state_machine.current_mission),
|
|
42
|
-
deepcopy(state_machine.current_task),
|
|
43
|
-
logger,
|
|
44
|
-
name="State Machine Get Inspections",
|
|
45
|
-
)
|
|
46
|
-
|
|
47
|
-
state_machine.iterate_current_task()
|
|
48
|
-
if state_machine.current_task is not None:
|
|
49
|
-
return None
|
|
50
|
-
|
|
51
|
-
return state_machine.mission_finished # type: ignore
|
|
52
|
-
|
|
53
29
|
def _robot_battery_level_updated_handler(
|
|
54
30
|
event: Event[float],
|
|
55
31
|
) -> Optional[Callable]:
|
|
@@ -60,7 +36,6 @@ class Monitor(EventHandlerBase):
|
|
|
60
36
|
state_machine.publish_mission_aborted(
|
|
61
37
|
"Robot battery too low to continue mission", True
|
|
62
38
|
)
|
|
63
|
-
state_machine._finalize()
|
|
64
39
|
state_machine.logger.warning(
|
|
65
40
|
"Cancelling current mission due to low battery"
|
|
66
41
|
)
|
|
@@ -78,6 +53,22 @@ class Monitor(EventHandlerBase):
|
|
|
78
53
|
)
|
|
79
54
|
return state_machine.stop_go_to_lockdown # type: ignore
|
|
80
55
|
|
|
56
|
+
def _mission_status_event_handler(
|
|
57
|
+
event: Event[MissionStatus],
|
|
58
|
+
) -> Optional[Callable]:
|
|
59
|
+
mission_status: Optional[MissionStatus] = event.consume_event()
|
|
60
|
+
if mission_status:
|
|
61
|
+
if mission_status not in [
|
|
62
|
+
MissionStatus.InProgress,
|
|
63
|
+
MissionStatus.NotStarted,
|
|
64
|
+
MissionStatus.Paused,
|
|
65
|
+
]:
|
|
66
|
+
state_machine.logger.info(
|
|
67
|
+
f"Mission completed with status {mission_status}"
|
|
68
|
+
)
|
|
69
|
+
return state_machine.mission_finished # type: ignore
|
|
70
|
+
return None
|
|
71
|
+
|
|
81
72
|
event_handlers: List[EventHandlerMapping] = [
|
|
82
73
|
EventHandlerMapping(
|
|
83
74
|
name="stop_mission_event",
|
|
@@ -104,18 +95,9 @@ class Monitor(EventHandlerBase):
|
|
|
104
95
|
),
|
|
105
96
|
),
|
|
106
97
|
EventHandlerMapping(
|
|
107
|
-
name="
|
|
108
|
-
event=events.robot_service_events.
|
|
109
|
-
handler=
|
|
110
|
-
state_machine, _handle_task_completed, event
|
|
111
|
-
),
|
|
112
|
-
),
|
|
113
|
-
EventHandlerMapping(
|
|
114
|
-
name="task_status_event",
|
|
115
|
-
event=events.robot_service_events.task_status_updated,
|
|
116
|
-
handler=lambda event: task_status_event_handler(
|
|
117
|
-
state_machine, _handle_task_completed, event
|
|
118
|
-
),
|
|
98
|
+
name="mission_status_event",
|
|
99
|
+
event=events.robot_service_events.mission_status_updated,
|
|
100
|
+
handler=_mission_status_event_handler,
|
|
119
101
|
),
|
|
120
102
|
EventHandlerMapping(
|
|
121
103
|
name="robot_battery_update_event",
|
|
@@ -25,7 +25,7 @@ class Paused(EventHandlerBase):
|
|
|
25
25
|
state_machine.publish_mission_aborted(
|
|
26
26
|
"Robot battery too low to continue mission", True
|
|
27
27
|
)
|
|
28
|
-
state_machine.
|
|
28
|
+
state_machine.print_transitions()
|
|
29
29
|
state_machine.logger.warning(
|
|
30
30
|
"Cancelling current mission due to low battery"
|
|
31
31
|
)
|
|
@@ -38,7 +38,7 @@ class Paused(EventHandlerBase):
|
|
|
38
38
|
if not should_lockdown:
|
|
39
39
|
return None
|
|
40
40
|
|
|
41
|
-
state_machine.
|
|
41
|
+
state_machine.print_transitions()
|
|
42
42
|
state_machine.logger.warning(
|
|
43
43
|
"Cancelling current mission due to robot going to lockdown"
|
|
44
44
|
)
|
|
@@ -4,7 +4,6 @@ from isar.apis.models.models import ControlMissionResponse
|
|
|
4
4
|
from isar.eventhandlers.eventhandler import EventHandlerBase, EventHandlerMapping
|
|
5
5
|
from isar.models.events import Event
|
|
6
6
|
from robot_interface.models.exceptions.robot_exceptions import ErrorMessage
|
|
7
|
-
from robot_interface.models.mission.status import MissionStatus, TaskStatus
|
|
8
7
|
|
|
9
8
|
if TYPE_CHECKING:
|
|
10
9
|
from isar.state_machine.state_machine import StateMachine
|
|
@@ -20,40 +19,25 @@ class Pausing(EventHandlerBase):
|
|
|
20
19
|
) -> Optional[Callable]:
|
|
21
20
|
error_message: Optional[ErrorMessage] = event.consume_event()
|
|
22
21
|
|
|
23
|
-
|
|
24
|
-
|
|
25
|
-
)
|
|
22
|
+
if error_message is None:
|
|
23
|
+
return None
|
|
26
24
|
|
|
27
25
|
state_machine.events.api_requests.pause_mission.response.trigger_event(
|
|
28
|
-
|
|
26
|
+
ControlMissionResponse(
|
|
27
|
+
success=False, failure_reason=error_message.error_reason
|
|
28
|
+
)
|
|
29
29
|
)
|
|
30
30
|
|
|
31
|
-
state_machine.publish_mission_status()
|
|
32
|
-
state_machine.send_task_status()
|
|
33
|
-
|
|
34
|
-
if error_message is None:
|
|
35
|
-
return None
|
|
36
|
-
|
|
37
31
|
return state_machine.mission_pausing_failed # type: ignore
|
|
38
32
|
|
|
39
33
|
def _successful_pause_event_handler(event: Event[bool]) -> Optional[Callable]:
|
|
40
34
|
if not event.consume_event():
|
|
41
35
|
return None
|
|
42
36
|
|
|
43
|
-
state_machine.current_mission.status = MissionStatus.Paused
|
|
44
|
-
state_machine.current_task.status = TaskStatus.Paused
|
|
45
|
-
|
|
46
|
-
paused_mission_response: ControlMissionResponse = (
|
|
47
|
-
state_machine._make_control_mission_response()
|
|
48
|
-
)
|
|
49
|
-
|
|
50
37
|
state_machine.events.api_requests.pause_mission.response.trigger_event(
|
|
51
|
-
|
|
38
|
+
ControlMissionResponse(success=True)
|
|
52
39
|
)
|
|
53
40
|
|
|
54
|
-
state_machine.publish_mission_status()
|
|
55
|
-
state_machine.send_task_status()
|
|
56
|
-
|
|
57
41
|
return state_machine.mission_paused # type:ignore
|
|
58
42
|
|
|
59
43
|
event_handlers: List[EventHandlerMapping] = [
|
|
@@ -4,7 +4,6 @@ from isar.apis.models.models import ControlMissionResponse
|
|
|
4
4
|
from isar.eventhandlers.eventhandler import EventHandlerBase, EventHandlerMapping
|
|
5
5
|
from isar.models.events import Event
|
|
6
6
|
from robot_interface.models.exceptions.robot_exceptions import ErrorMessage
|
|
7
|
-
from robot_interface.models.mission.status import MissionStatus, TaskStatus
|
|
8
7
|
|
|
9
8
|
if TYPE_CHECKING:
|
|
10
9
|
from isar.state_machine.state_machine import StateMachine
|
|
@@ -20,40 +19,25 @@ class PausingReturnHome(EventHandlerBase):
|
|
|
20
19
|
) -> Optional[Callable]:
|
|
21
20
|
error_message: Optional[ErrorMessage] = event.consume_event()
|
|
22
21
|
|
|
23
|
-
|
|
24
|
-
|
|
25
|
-
)
|
|
22
|
+
if error_message is None:
|
|
23
|
+
return None
|
|
26
24
|
|
|
27
25
|
state_machine.events.api_requests.pause_mission.response.trigger_event(
|
|
28
|
-
|
|
26
|
+
ControlMissionResponse(
|
|
27
|
+
success=False, failure_reason=error_message.error_reason
|
|
28
|
+
)
|
|
29
29
|
)
|
|
30
30
|
|
|
31
|
-
state_machine.publish_mission_status()
|
|
32
|
-
state_machine.send_task_status()
|
|
33
|
-
|
|
34
|
-
if error_message is None:
|
|
35
|
-
return None
|
|
36
|
-
|
|
37
31
|
return state_machine.return_home_mission_pausing_failed # type: ignore
|
|
38
32
|
|
|
39
33
|
def _successful_pause_event_handler(event: Event[bool]) -> Optional[Callable]:
|
|
40
34
|
if not event.consume_event():
|
|
41
35
|
return None
|
|
42
36
|
|
|
43
|
-
state_machine.current_mission.status = MissionStatus.Paused
|
|
44
|
-
state_machine.current_task.status = TaskStatus.Paused
|
|
45
|
-
|
|
46
|
-
paused_mission_response: ControlMissionResponse = (
|
|
47
|
-
state_machine._make_control_mission_response()
|
|
48
|
-
)
|
|
49
|
-
|
|
50
37
|
state_machine.events.api_requests.pause_mission.response.trigger_event(
|
|
51
|
-
|
|
38
|
+
ControlMissionResponse(success=True)
|
|
52
39
|
)
|
|
53
40
|
|
|
54
|
-
state_machine.publish_mission_status()
|
|
55
|
-
state_machine.send_task_status()
|
|
56
|
-
|
|
57
41
|
return state_machine.return_home_mission_paused # type: ignore
|
|
58
42
|
|
|
59
43
|
event_handlers: List[EventHandlerMapping] = [
|