From 684bdcb9554133cfa0e81d50f475172282928657 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Vetle=20Kopperg=C3=A5rd?= Date: Mon, 30 May 2022 11:08:14 +0200 Subject: [PATCH] Add initialization of robot to state machine --- src/isar/apis/models/__init__.py | 2 +- src/isar/apis/models/models.py | 10 +++ .../apis/schedule/scheduling_controller.py | 51 ++++++++++++--- src/isar/models/communication/message.py | 10 ++- .../utilities/scheduling_utilities.py | 11 +++- src/isar/state_machine/state_machine.py | 63 +++++++++++++++---- src/isar/state_machine/states/__init__.py | 1 + src/isar/state_machine/states/idle.py | 15 +++-- src/isar/state_machine/states/initialize.py | 60 ++++++++++++++++++ src/isar/state_machine/states_enum.py | 1 + .../models/initialize/initialize_params.py | 5 +- .../utilities/test_scheduling_utilities.py | 4 ++ .../isar/state_machine/test_state_machine.py | 20 +++--- tests/mocks/robot_interface.py | 4 ++ 14 files changed, 212 insertions(+), 45 deletions(-) create mode 100644 src/isar/state_machine/states/initialize.py diff --git a/src/isar/apis/models/__init__.py b/src/isar/apis/models/__init__.py index ac3b294f..d071ee82 100644 --- a/src/isar/apis/models/__init__.py +++ b/src/isar/apis/models/__init__.py @@ -1 +1 @@ -from .models import StartMissionResponse +from .models import ApiPose, StartMissionResponse diff --git a/src/isar/apis/models/models.py b/src/isar/apis/models/models.py index a50fe706..48b7f9b9 100644 --- a/src/isar/apis/models/models.py +++ b/src/isar/apis/models/models.py @@ -1,3 +1,4 @@ +from email.policy import default from typing import List, Optional, Union from uuid import UUID @@ -18,3 +19,12 @@ class TaskResponse(BaseModel): class StartMissionResponse(BaseModel): id: Union[UUID, int, str, None] tasks: List[TaskResponse] + + +class ApiPose(BaseModel): + x: float + y: float + z: float + phi: float + theta: float + psi: float diff --git a/src/isar/apis/schedule/scheduling_controller.py b/src/isar/apis/schedule/scheduling_controller.py index ef0537cb..cf76fdba 100644 --- a/src/isar/apis/schedule/scheduling_controller.py +++ b/src/isar/apis/schedule/scheduling_controller.py @@ -1,14 +1,15 @@ import logging from http import HTTPStatus from queue import Empty -from typing import List +from typing import List, Optional +import numpy as np from alitra import Frame, Orientation, Pose, Position -from fastapi import Query, Response +from fastapi import Body, Query, Response from injector import inject from requests import HTTPError -from isar.apis.models import StartMissionResponse +from isar.apis.models import ApiPose, StartMissionResponse from isar.config.settings import robot_settings, settings from isar.mission_planner.mission_planner_interface import ( MissionPlannerError, @@ -44,6 +45,10 @@ def start_mission( title="Mission ID", description="ID-number for predefined mission", ), + initial_pose: Optional[ApiPose] = Body( + default=None, + description="The starting point of the mission. Used for initial localization of robot", + ), ): self.logger.info("Received request to start new mission") try: @@ -56,6 +61,7 @@ def start_mission( return if state in [ + States.Initialize, States.InitiateStep, States.StopStep, States.Monitor, @@ -86,10 +92,32 @@ def start_mission( response.status_code = HTTPStatus.BAD_REQUEST.value return + initial_pose_alitra: Optional[Pose] + if initial_pose: + initial_pose_alitra = Pose( + position=Position( + x=initial_pose.x, + y=initial_pose.y, + z=initial_pose.z, + frame=Frame("asset"), + ), + orientation=Orientation.from_euler_array( + euler=np.array( + [initial_pose.phi, initial_pose.theta, initial_pose.psi] + ), + frame=Frame("asset"), + ), + frame=Frame("asset"), + ) + else: + initial_pose_alitra = None + self.logger.info(f"Starting mission: {mission.id}") try: - self.scheduling_utilities.start_mission(mission=mission) + self.scheduling_utilities.start_mission( + mission=mission, initial_pose=initial_pose_alitra + ) self.logger.info("OK - Mission successfully started") except QueueTimeoutError: response.status_code = HTTPStatus.REQUEST_TIMEOUT.value @@ -109,7 +137,7 @@ def pause_mission(self, response: Response): response.status_code = HTTPStatus.INTERNAL_SERVER_ERROR.value return - if state in [States.Idle, States.StopStep, States.Paused]: + if state in [States.Idle, States.StopStep, States.Paused, States.Initialize]: self.logger.info("Conflict - Pause command received in invalid state") response.status_code = HTTPStatus.CONFLICT.value return @@ -132,7 +160,13 @@ def resume_mission(self, response: Response): response.status_code = HTTPStatus.INTERNAL_SERVER_ERROR.value return - if state in [States.Idle, States.InitiateStep, States.Monitor, States.StopStep]: + if state in [ + States.Idle, + States.InitiateStep, + States.Monitor, + States.StopStep, + States.Initialize, + ]: self.logger.info("Conflict - Resume command received in invalid state") response.status_code = HTTPStatus.CONFLICT.value return @@ -156,7 +190,7 @@ def stop_mission(self, response: Response): response.status_code = HTTPStatus.INTERNAL_SERVER_ERROR.value return - if state in [States.Idle]: + if state in [States.Idle, States.Initialize]: self.logger.info("Conflict - Stop command received in invalid state") response.status_code = HTTPStatus.CONFLICT.value return @@ -202,6 +236,7 @@ def drive_to( return if state in [ + States.Initialize, States.InitiateStep, States.StopStep, States.Monitor, @@ -222,7 +257,7 @@ def drive_to( mission: Mission = Mission(tasks=[Task(steps=[step])]) try: - self.scheduling_utilities.start_mission(mission=mission) + self.scheduling_utilities.start_mission(mission=mission, initial_pose=None) self.logger.info("OK - Drive to successfully started") except QueueTimeoutError: self.logger.error("Timout - Failed to start drive to") diff --git a/src/isar/models/communication/message.py b/src/isar/models/communication/message.py index 855be662..4fac44cd 100644 --- a/src/isar/models/communication/message.py +++ b/src/isar/models/communication/message.py @@ -1,6 +1,12 @@ from dataclasses import dataclass +from typing import Optional + +from alitra import Pose + +from isar.models.mission import Mission @dataclass -class Message: - message: str +class StartMissionMessage: + mission: Mission + initial_pose: Optional[Pose] diff --git a/src/isar/services/utilities/scheduling_utilities.py b/src/isar/services/utilities/scheduling_utilities.py index d6e4ac77..bc239af7 100644 --- a/src/isar/services/utilities/scheduling_utilities.py +++ b/src/isar/services/utilities/scheduling_utilities.py @@ -1,10 +1,12 @@ import logging from copy import deepcopy -from typing import Any +from typing import Any, Optional +from alitra import Pose from injector import inject from isar.config.settings import settings +from isar.models.communication.message import StartMissionMessage from isar.models.communication.queues import QueueIO, Queues, QueueTimeoutError from isar.models.mission.mission import Mission from isar.services.utilities.queue_utilities import QueueUtilities @@ -26,8 +28,11 @@ def __init__(self, queues: Queues, queue_timeout: int = settings.QUEUE_TIMEOUT): def get_state(self) -> States: return self.queues.state.check() - def start_mission(self, mission: Mission) -> None: - self._send_command(deepcopy(mission), self.queues.start_mission) + def start_mission(self, mission: Mission, initial_pose: Optional[Pose]) -> None: + self._send_command( + StartMissionMessage(mission=deepcopy(mission), initial_pose=initial_pose), + self.queues.start_mission, + ) def pause_mission(self) -> None: self._send_command(True, self.queues.pause_mission) diff --git a/src/isar/state_machine/state_machine.py b/src/isar/state_machine/state_machine.py index 821419df..5999684d 100644 --- a/src/isar/state_machine/state_machine.py +++ b/src/isar/state_machine/state_machine.py @@ -5,18 +5,29 @@ from datetime import datetime from typing import Deque, List, Optional +from alitra import Pose from injector import Injector, inject from transitions import Machine from transitions.core import State from isar.config.settings import settings +from isar.models.communication.message import StartMissionMessage from isar.models.communication.queues.queues import Queues from isar.models.mission import Mission, Task from isar.models.mission.status import MissionStatus, TaskStatus from isar.services.service_connections.mqtt.mqtt_client import MqttClientInterface from isar.services.utilities.json_service import EnhancedJSONEncoder -from isar.state_machine.states import Idle, InitiateStep, Monitor, Off, Paused, StopStep +from isar.state_machine.states import ( + Idle, + Initialize, + InitiateStep, + Monitor, + Off, + Paused, + StopStep, +) from isar.state_machine.states_enum import States +from robot_interface.models.initialize.initialize_params import InitializeParams from robot_interface.models.mission import StepStatus from robot_interface.models.mission.step import Step from robot_interface.robot_interface import RobotInterface @@ -61,6 +72,7 @@ def __init__( self.stop_step_state: State = StopStep(self) self.paused_state: State = Paused(self) self.idle_state: State = Idle(self) + self.initialize_state: State = Initialize(self) self.monitor_state: State = Monitor(self) self.initiate_step_state: State = InitiateStep(self) self.off_state: State = Off(self) @@ -68,6 +80,7 @@ def __init__( self.states: List[State] = [ self.off_state, self.idle_state, + self.initialize_state, self.initiate_step_state, self.monitor_state, self.stop_step_state, @@ -112,9 +125,21 @@ def __init__( { "trigger": "mission_started", "source": self.idle_state, - "dest": self.initiate_step_state, + "dest": self.initialize_state, "before": self._mission_started, }, + { + "trigger": "initialization_successful", + "source": self.initialize_state, + "dest": self.initiate_step_state, + "before": self._initialization_successful, + }, + { + "trigger": "initialization_failed", + "source": self.initialize_state, + "dest": self.idle_state, + "before": self._initialization_failed, + }, { "trigger": "resume", "source": self.paused_state, @@ -161,6 +186,7 @@ def __init__( self.current_mission: Optional[Mission] = None self.current_task: Optional[Task] = None self.current_step: Optional[Step] = None + self.initial_pose: Optional[Pose] = None self.current_state: State = States(self.state) # type: ignore @@ -171,6 +197,23 @@ def __init__( ################################################################################# # Transition Callbacks + def _initialization_successful(self) -> None: + self.queues.start_mission.output.put(True) + self.logger.info( + f"Initialization successful. Starting new mission: {self.current_mission.id}" + ) + self.log_step_overview(mission=self.current_mission) + self.current_mission.status = MissionStatus.InProgress + self.publish_mission_status() + self.current_task = self.current_mission.next_task() + self.current_task.status = TaskStatus.InProgress + self.publish_task_status() + self.update_current_step() + + def _initialization_failed(self) -> None: + self.queues.start_mission.output.put(False) + self._finalize() + def _step_initiated(self) -> None: self.current_step.status = StepStatus.InProgress self.publish_step_status() @@ -198,11 +241,7 @@ def _mission_finished(self) -> None: self._finalize() def _mission_started(self) -> None: - self.logger.info(f"Starting new mission: {self.current_mission.id}") - self.current_mission.status = MissionStatus.InProgress - self.current_task.status = TaskStatus.InProgress - self.log_step_overview(mission=self.current_mission) - self.update_current_step() + return def _step_finished(self) -> None: self.publish_step_status() @@ -302,13 +341,15 @@ def reset_state_machine(self) -> None: self.current_task = None self.current_mission = None - def start_mission(self, mission: Mission): + def start_mission(self, mission: Mission, initial_pose: Pose): """Starts a scheduled mission.""" self.current_mission = mission - self.current_task = mission.next_task() - self.queues.start_mission.output.put(True) + self.initial_pose = initial_pose + + def get_initialize_params(self): + return InitializeParams(initial_pose=self.initial_pose) - def should_start_mission(self) -> Optional[Mission]: + def should_start_mission(self) -> Optional[StartMissionMessage]: try: return self.queues.start_mission.input.get(block=False) except queue.Empty: diff --git a/src/isar/state_machine/states/__init__.py b/src/isar/state_machine/states/__init__.py index 14080df5..23459eb4 100644 --- a/src/isar/state_machine/states/__init__.py +++ b/src/isar/state_machine/states/__init__.py @@ -1,4 +1,5 @@ from .idle import Idle +from .initialize import Initialize from .initiate_step import InitiateStep from .monitor import Monitor from .off import Off diff --git a/src/isar/state_machine/states/idle.py b/src/isar/state_machine/states/idle.py index 2aecd803..1520dd88 100644 --- a/src/isar/state_machine/states/idle.py +++ b/src/isar/state_machine/states/idle.py @@ -1,10 +1,10 @@ import logging import time -from typing import TYPE_CHECKING +from typing import TYPE_CHECKING, Optional from transitions import State -from isar.models.mission import Mission +from isar.models.communication.message import StartMissionMessage if TYPE_CHECKING: from isar.state_machine.state_machine import StateMachine @@ -25,9 +25,14 @@ def stop(self): def _run(self): while True: - mission: Mission = self.state_machine.should_start_mission() - if mission: - self.state_machine.start_mission(mission) + start_mission: Optional[ + StartMissionMessage + ] = self.state_machine.should_start_mission() + if start_mission: + self.state_machine.start_mission( + mission=start_mission.mission, + initial_pose=start_mission.initial_pose, + ) transition = self.state_machine.mission_started break time.sleep(self.state_machine.sleep_time) diff --git a/src/isar/state_machine/states/initialize.py b/src/isar/state_machine/states/initialize.py new file mode 100644 index 00000000..d1f47e11 --- /dev/null +++ b/src/isar/state_machine/states/initialize.py @@ -0,0 +1,60 @@ +import logging +import time +from typing import TYPE_CHECKING, Callable, Optional + +from injector import inject +from transitions import State + +from isar.services.utilities.threaded_request import ( + ThreadedRequest, + ThreadedRequestNotFinishedError, +) +from robot_interface.models.exceptions import RobotException + +if TYPE_CHECKING: + from isar.state_machine.state_machine import StateMachine + + +class Initialize(State): + @inject + def __init__(self, state_machine: "StateMachine"): + super().__init__(name="initialize", on_enter=self.start, on_exit=self.stop) + self.state_machine: "StateMachine" = state_machine + + self.logger = logging.getLogger("state_machine") + self.initialize_thread: Optional[ThreadedRequest] = None + + def start(self): + self.state_machine.update_state() + self._run() + + def stop(self): + if self.initialize_thread: + self.initialize_thread.wait_for_thread() + self.initialize_thread = None + + def _run(self): + transition: Callable + while True: + if not self.initialize_thread: + self.initialize_thread = ThreadedRequest( + self.state_machine.robot.initialize + ) + self.initialize_thread.start_thread( + self.state_machine.get_initialize_params() + ) + + try: + self.initialize_thread.get_output() + except ThreadedRequestNotFinishedError: + time.sleep(self.state_machine.sleep_time) + continue + except RobotException as e: + self.logger.error("Initialization of robot failed") + self.logger.error(e) + transition = self.state_machine.initialization_failed + break + + transition = self.state_machine.initialization_successful + break + transition() diff --git a/src/isar/state_machine/states_enum.py b/src/isar/state_machine/states_enum.py index 8f02614f..f01da215 100644 --- a/src/isar/state_machine/states_enum.py +++ b/src/isar/state_machine/states_enum.py @@ -6,6 +6,7 @@ class States(str, Enum): Off = "off" Idle = "idle" InitiateStep = "initiate_step" + Initialize = "initialize" Monitor = "monitor" Paused = "paused" StopStep = "stop_step" diff --git a/src/robot_interface/models/initialize/initialize_params.py b/src/robot_interface/models/initialize/initialize_params.py index 43b13389..523c8076 100644 --- a/src/robot_interface/models/initialize/initialize_params.py +++ b/src/robot_interface/models/initialize/initialize_params.py @@ -1,8 +1,7 @@ -from dataclasses import dataclass -from email.policy import default +from dataclasses import Field, dataclass +from typing import Optional from alitra import Pose -from pyparsing import Optional @dataclass diff --git a/tests/isar/services/utilities/test_scheduling_utilities.py b/tests/isar/services/utilities/test_scheduling_utilities.py index 1c907c86..bb40253c 100644 --- a/tests/isar/services/utilities/test_scheduling_utilities.py +++ b/tests/isar/services/utilities/test_scheduling_utilities.py @@ -10,3 +10,7 @@ def test_timeout_send_command(mocker, scheduling_utilities): with pytest.raises(QueueTimeoutError): scheduling_utilities._send_command(True, q) assert q.input.empty() + + +import queue +from queue import Queue diff --git a/tests/isar/state_machine/test_state_machine.py b/tests/isar/state_machine/test_state_machine.py index 23939f7b..bac99784 100644 --- a/tests/isar/state_machine/test_state_machine.py +++ b/tests/isar/state_machine/test_state_machine.py @@ -17,7 +17,6 @@ from isar.storage.uploader import Uploader from robot_interface.models.mission import DriveToPose, Step, TakeImage from robot_interface.models.mission.status import StepStatus -from tests.mocks.mission_definition import MockMissionDefinition from tests.mocks.pose import MockPose from tests.mocks.robot_interface import MockRobot from tests.mocks.step import MockStep @@ -83,24 +82,18 @@ def test_reset_state_machine(state_machine): empty_mission: Mission = Mission([], None) -def test_start_mission(state_machine): - mission: Mission = MockMissionDefinition.default_mission - state_machine.start_mission(mission=mission) - message = state_machine.queues.start_mission.output.get() - assert message - - def test_state_machine_transitions(injector, state_machine_thread): step: Step = DriveToPose(pose=MockPose.default_pose) mission: Mission = Mission(tasks=[Task(steps=[step])]) scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) - scheduling_utilities.start_mission(mission=mission) + scheduling_utilities.start_mission(mission=mission, initial_pose=None) time.sleep(1) expected_transitions_list = deque( [ States.Idle, + States.Initialize, States.InitiateStep, States.Monitor, States.InitiateStep, @@ -120,12 +113,13 @@ def test_state_machine_failed_dependency(injector, state_machine_thread, mocker) mocker.patch.object(MockRobot, "step_status", return_value=StepStatus.Failed) scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) - scheduling_utilities.start_mission(mission=mission) + scheduling_utilities.start_mission(mission=mission, initial_pose=None) time.sleep(1) expected_transitions_list = deque( [ States.Idle, + States.Initialize, States.InitiateStep, States.Monitor, States.InitiateStep, @@ -146,11 +140,12 @@ def test_state_machine_with_successful_collection( mission: Mission = Mission(tasks=[Task(steps=[step])]) scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) - scheduling_utilities.start_mission(mission=mission) + scheduling_utilities.start_mission(mission=mission, initial_pose=None) time.sleep(1) expected_transitions_list = deque( [ States.Idle, + States.Initialize, States.InitiateStep, States.Monitor, States.InitiateStep, @@ -175,11 +170,12 @@ def test_state_machine_with_unsuccessful_collection( mission: Mission = Mission(tasks=[Task(steps=[step])]) scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) - scheduling_utilities.start_mission(mission=mission) + scheduling_utilities.start_mission(mission=mission, initial_pose=None) time.sleep(1) expected_transitions_list = deque( [ States.Idle, + States.Initialize, States.InitiateStep, States.Monitor, States.InitiateStep, diff --git a/tests/mocks/robot_interface.py b/tests/mocks/robot_interface.py index 7f647868..5b153128 100644 --- a/tests/mocks/robot_interface.py +++ b/tests/mocks/robot_interface.py @@ -3,6 +3,7 @@ from alitra import Frame, Orientation, Pose, Position +from robot_interface.models.initialize import InitializeParams from robot_interface.models.inspection.inspection import ( Image, ImageMetadata, @@ -44,6 +45,9 @@ def get_inspections(self, step: InspectionStep) -> Sequence[Inspection]: image.data = b"Some binary image data" return [image] + def initialize(self, params: InitializeParams) -> None: + return + def mock_image_metadata() -> ImageMetadata: return ImageMetadata(