isar 1.22.3__py3-none-any.whl → 1.23.0__py3-none-any.whl

isar/config/predefined_missions/default_turtlebot.json

@@ -1,110 +1,106 @@
 {
-    "id": "2",
-    "tasks": [
-        {
-            "steps": [
-                {
-                    "type": "drive_to_pose",
-                    "pose": {
-                        "position": {
-                            "x": -3.6,
-                            "y": 4,
-                            "z": 0,
-                            "frame": "asset"
-                        },
-                        "orientation": {
-                            "x": 0,
-                            "y": 0,
-                            "z": -0.7286672256879113,
-                            "w": -0.6848660759820616,
-                            "frame": "asset"
-                        },
-                        "frame": "asset"
-                    }
-                },
-                {
-                    "type": "take_image",
-                    "target": {
-                        "x": -4.7,
-                        "y": 4.9,
-                        "z": 0,
-                        "frame": "robot"
-                    }
-                }
-            ]
+  "id": "2",
+  "tasks": [
+    {
+      "type": "take_image",
+      "robot_pose": {
+        "position": {
+          "x": -3.6,
+          "y": 4,
+          "z": 0,
+          "frame": "asset"
         },
-        {
-            "steps": [
-                {
-                    "type": "drive_to_pose",
-                    "pose": {
-                        "position": {
-                            "x": 4.7,
-                            "y": 3,
-                            "z": 0,
-                            "frame": "asset"
-                        },
-                        "orientation": {
-                            "x": 0,
-                            "y": 0,
-                            "z": 0.5769585,
-                            "w": 0.8167734,
-                            "frame": "asset"
-                        },
-                        "frame": "asset"
-                    }
-                },
-                {
-                    "type": "take_image",
-                    "target": {
-                        "x": 5.6,
-                        "y": 5.2,
-                        "z": 0,
-                        "frame": "robot"
-                    }
-                },
-                {
-                    "type": "take_thermal_image",
-                    "target": {
-                        "x": 3.1,
-                        "y": 5.2,
-                        "z": 0,
-                        "frame": "robot"
-                    }
-                }
-            ]
+        "orientation": {
+          "x": 0,
+          "y": 0,
+          "z": -0.7286672256879113,
+          "w": -0.6848660759820616,
+          "frame": "asset"
         },
-        {
-            "steps": [
-                {
-                    "type": "drive_to_pose",
-                    "pose": {
-                        "position": {
-                            "x": 0.95,
-                            "y": 2.6,
-                            "z": 0,
-                            "frame": "asset"
-                        },
-                        "orientation": {
-                            "x": 0,
-                            "y": 0,
-                            "z": -0.6992469,
-                            "w": 0.7148802,
-                            "frame": "asset"
-                        },
-                        "frame": "asset"
-                    }
-                },
-                {
-                    "type": "take_thermal_image",
-                    "target": {
-                        "x": 1.9,
-                        "y": 1.9,
-                        "z": 0,
-                        "frame": "robot"
-                    }
-                }
-            ]
-        }
-    ]
+        "frame": "asset"
+      },
+      "target": {
+        "x": -4.7,
+        "y": 4.9,
+        "z": 0,
+        "frame": "robot"
+      }
+    },
+
+    {
+      "type": "take_image",
+      "robot_pose": {
+        "position": {
+          "x": 4.7,
+          "y": 3,
+          "z": 0,
+          "frame": "asset"
+        },
+        "orientation": {
+          "x": 0,
+          "y": 0,
+          "z": 0.5769585,
+          "w": 0.8167734,
+          "frame": "asset"
+        },
+        "frame": "asset"
+      },
+      "target": {
+        "x": 5.6,
+        "y": 5.2,
+        "z": 0,
+        "frame": "robot"
+      }
+    },
+    {
+      "type": "take_thermal_image",
+      "robot_pose": {
+        "position": {
+          "x": 4.7,
+          "y": 3,
+          "z": 0,
+          "frame": "asset"
+        },
+        "orientation": {
+          "x": 0,
+          "y": 0,
+          "z": 0.5769585,
+          "w": 0.8167734,
+          "frame": "asset"
+        },
+        "frame": "asset"
+      },
+      "target": {
+        "x": 3.1,
+        "y": 5.2,
+        "z": 0,
+        "frame": "robot"
+      }
+    },
+    {
+      "type": "take_thermal_image",
+      "robot_pose": {
+        "position": {
+          "x": 0.95,
+          "y": 2.6,
+          "z": 0,
+          "frame": "asset"
+        },
+        "orientation": {
+          "x": 0,
+          "y": 0,
+          "z": -0.6992469,
+          "w": 0.7148802,
+          "frame": "asset"
+        },
+        "frame": "asset"
+      },
+      "target": {
+        "x": 1.9,
+        "y": 1.9,
+        "z": 0,
+        "frame": "robot"
+      }
+    }
+  ]
 }

isar/config/settings.env

@@ -1,6 +1,6 @@
 ISAR_ROBOT_PACKAGE = isar_robot
 
-ISAR_RUN_MISSION_STEPWISE = true
+ISAR_RUN_MISSION_BY_TASK = true
 
 ISAR_STORAGE_LOCAL_ENABLED = true
 ISAR_STORAGE_BLOB_ENABLED = false

isar/config/settings.py

@@ -26,7 +26,7 @@ class Settings(BaseSettings):
     ROBOT_PACKAGE: str = Field(default="isar_robot")
 
     # The run mode of the robot (stepwise or full mission)
-    RUN_MISSION_STEPWISE: bool = Field(default=True)
+    RUN_MISSION_BY_TASK: bool = Field(default=True)
 
     # Determines the local path in which results from missions are stored
     LOCAL_STORAGE_PATH: str = Field(default="./results")
@@ -327,7 +327,7 @@ class RobotSettings(BaseSettings):
 
     # ISAR steps the robot is capable of performing
     # This should be set in the robot package settings.env file
-    CAPABILITIES: List[str] = Field(default=["drive_to_pose", "take_image"])
+    CAPABILITIES: List[str] = Field(default=["return_to_home", "take_image"])
 
     # Model of the robot which ISAR is connected to
     # This should be set in the robot package settings.env file
@@ -345,6 +345,6 @@ class RobotSettings(BaseSettings):
 
 robot_settings = RobotSettings()
 
-if not settings.RUN_MISSION_STEPWISE:  # If mission-wise, do not run localize missions
+if not settings.RUN_MISSION_BY_TASK:  # If mission-wise, do not run localize missions
     if "localize" in robot_settings.CAPABILITIES:
         robot_settings.CAPABILITIES.remove("localize")

isar/mission_planner/local_planner.py

@@ -1,5 +1,6 @@
 import logging
 from pathlib import Path
+from typing import List, Optional
 
 from alitra import Frame
 from injector import inject
@@ -13,6 +14,7 @@ from isar.mission_planner.mission_planner_interface import (
 from isar.services.readers.base_reader import BaseReader, BaseReaderError
 from robot_interface.models.mission.mission import Mission
 
+
 logger = logging.getLogger("api")
 
 
@@ -38,6 +40,7 @@ class LocalPlanner(MissionPlannerInterface):
     @staticmethod
     def read_mission_from_file(mission_path: Path) -> Mission:
         mission_dict: dict = BaseReader.read_json(location=mission_path)
+
         mission: Mission = BaseReader.dict_to_dataclass(
             dataclass_dict=mission_dict,
             target_dataclass=Mission,

isar/mission_planner/sequential_task_selector.py

@@ -4,7 +4,7 @@ from isar.mission_planner.task_selector_interface import (
     TaskSelectorInterface,
     TaskSelectorStop,
 )
-from robot_interface.models.mission.task import Task
+from robot_interface.models.mission.task import TASKS, Task
 
 
 class SequentialTaskSelector(TaskSelectorInterface):
@@ -12,11 +12,11 @@ class SequentialTaskSelector(TaskSelectorInterface):
         super().__init__()
         self._iterator: Iterator = None
 
-    def initialize(self, tasks: List[Task]) -> None:
+    def initialize(self, tasks: List[TASKS]) -> None:
         super().initialize(tasks=tasks)
         self._iterator = iter(self.tasks)
 
-    def next_task(self) -> Task:
+    def next_task(self) -> TASKS:
         try:
             return next(self._iterator)
         except StopIteration:

isar/mission_planner/task_selector_interface.py

@@ -1,18 +1,18 @@
 from abc import ABCMeta, abstractmethod
 from typing import List
 
-from robot_interface.models.mission.task import Task
+from robot_interface.models.mission.task import TASKS, Task
 
 
 class TaskSelectorInterface(metaclass=ABCMeta):
     def __init__(self) -> None:
-        self.tasks: List[Task] = None
+        self.tasks: List[TASKS] = None
 
-    def initialize(self, tasks: List[Task]) -> None:
+    def initialize(self, tasks: List[TASKS]) -> None:
         self.tasks = tasks
 
     @abstractmethod
-    def next_task(self) -> Task:
+    def next_task(self) -> TASKS:
         """
         Returns
         -------

isar/script.py

@@ -63,7 +63,7 @@ def print_startup_info():
     print_setting(fillchar="-")
     print_setting("Robot package", settings.ROBOT_PACKAGE)
     print_setting("Robot name", settings.ROBOT_NAME)
-    print_setting("Run mission stepwise", settings.RUN_MISSION_STEPWISE)
+    print_setting("Run mission stepwise", settings.RUN_MISSION_BY_TASK)
     print_setting("Running on port", settings.API_PORT)
     print_setting("Mission planner", settings.MISSION_PLANNER)
     print_setting("Using local storage", settings.STORAGE_LOCAL_ENABLED)

isar/services/service_connections/mqtt/mqtt_client.py

@@ -99,7 +99,7 @@ class MqttClient(MqttClientInterface):
     )
     def connect(self, host: str, port: int) -> None:
         self.logger.info("Attempting to connect to MQTT Broker")
-        self.logger.debug(f"Host: {host}, Port: {port}")
+        self.logger.info(f"Host: {host}, Port: {port}")
         self.client.connect(host=host, port=port)
 
     def publish(self, topic: str, payload: str, qos: int = 0, retain: bool = False):

isar/services/utilities/scheduling_utilities.py

@@ -101,10 +101,9 @@ class SchedulingUtilities:
         is_capable: bool = True
         missing_capabilities: Set[str] = set()
         for task in mission.tasks:
-            for step in task.steps:
-                if not step.type in robot_capabilities:
-                    is_capable = False
-                    missing_capabilities.add(step.type)
+            if not task.type in robot_capabilities:
+                is_capable = False
+                missing_capabilities.add(task.type)
 
         if not is_capable:
             error_message = (

isar/state_machine/state_machine.py

@@ -35,11 +35,9 @@ from robot_interface.models.mission.mission import Mission
 from robot_interface.models.mission.status import (
     MissionStatus,
     RobotStatus,
-    StepStatus,
     TaskStatus,
 )
-from robot_interface.models.mission.step import Step
-from robot_interface.models.mission.task import Task
+from robot_interface.models.mission.task import TASKS, Task
 from robot_interface.robot_interface import RobotInterface
 from robot_interface.telemetry.mqtt_client import MqttClientInterface
 from robot_interface.utilities.json_service import EnhancedJSONEncoder
@@ -56,7 +54,7 @@ class StateMachine(object):
         mqtt_publisher: MqttClientInterface,
         task_selector: TaskSelectorInterface,
         sleep_time: float = settings.FSM_SLEEP_TIME,
-        stepwise_mission: bool = settings.RUN_MISSION_STEPWISE,
+        run_mission_by_task: bool = settings.RUN_MISSION_BY_TASK,
         stop_robot_attempts_limit: int = settings.STOP_ROBOT_ATTEMPTS_LIMIT,
         transitions_log_length: int = settings.STATE_TRANSITIONS_LOG_LENGTH,
     ):
@@ -84,7 +82,7 @@ class StateMachine(object):
         self.robot: RobotInterface = robot
         self.mqtt_publisher: Optional[MqttClientInterface] = mqtt_publisher
         self.task_selector: TaskSelectorInterface = task_selector
-        self.stepwise_mission: bool = stepwise_mission
+        self.run_mission_by_task: bool = run_mission_by_task
 
         # List of states
         self.stop_state: State = Stop(self)
@@ -179,10 +177,10 @@ class StateMachine(object):
                     "before": self._resume,
                 },
                 {
-                    "trigger": "step_finished",
+                    "trigger": "task_finished",
                     "source": self.monitor_state,
                     "dest": self.initiate_state,
-                    "before": self._step_finished,
+                    "before": self._task_finished,
                 },
                 {
                     "trigger": "full_mission_finished",
@@ -234,8 +232,7 @@ class StateMachine(object):
 
         self.stopped: bool = False
         self.current_mission: Optional[Mission] = None
-        self.current_task: Optional[Task] = None
-        self.current_step: Optional[Step] = None
+        self.current_task: Optional[TASKS] = None
         self.initial_pose: Optional[Pose] = None
 
         self.current_state: State = States(self.state)  # type: ignore
@@ -255,14 +252,14 @@ class StateMachine(object):
         self._finalize()
 
     def _initiated(self) -> None:
-        if self.stepwise_mission:
-            self.current_step.status = StepStatus.InProgress
+        if self.run_mission_by_task:
+            self.current_task.status = TaskStatus.InProgress
         self.current_mission.status = MissionStatus.InProgress
-        self.publish_step_status(step=self.current_step)
+        self.publish_task_status(task=self.current_task)
         self.logger.info(
             f"Successfully initiated "
-            f"{type(self.current_step).__name__} "
-            f"step: {str(self.current_step.id)[:8]}"
+            f"{type(self.current_task).__name__} "
+            f"task: {str(self.current_task.id)[:8]}"
         )
 
     def _pause(self) -> None:
@@ -291,9 +288,6 @@ class StateMachine(object):
         )
         self.queues.resume_mission.output.put(resume_mission_response)
 
-        self.current_task.reset_task()
-        self.iterate_current_step()
-
         self.robot.resume()
 
     def _mission_finished(self) -> None:
@@ -327,7 +321,7 @@ class StateMachine(object):
             f"Initialization successful. Starting new mission: "
             f"{self.current_mission.id}"
         )
-        self.log_step_overview(mission=self.current_mission)
+        self.log_mission_overview(mission=self.current_mission)
 
         self.current_mission.status = MissionStatus.InProgress
         self.publish_mission_status()
@@ -337,14 +331,11 @@ class StateMachine(object):
         else:
             self.current_task.status = TaskStatus.InProgress
             self.publish_task_status(task=self.current_task)
-            self.iterate_current_step()
 
-    def _step_finished(self) -> None:
-        self.publish_step_status(step=self.current_step)
-        self.current_task.update_task_status()
+    def _task_finished(self) -> None:
         self.publish_task_status(task=self.current_task)
+        self.current_task.update_task_status()
         self.iterate_current_task()
-        self.iterate_current_step()
 
     def _full_mission_finished(self) -> None:
         self.current_task = None
@@ -353,7 +344,6 @@ class StateMachine(object):
         self.logger.info(f"Pausing mission: {self.current_mission.id}")
         self.current_mission.status = MissionStatus.Paused
         self.current_task.status = TaskStatus.Paused
-        self.current_step.status = StepStatus.NotStarted
 
         paused_mission_response: ControlMissionResponse = (
             self._make_control_mission_response()
@@ -362,7 +352,6 @@ class StateMachine(object):
 
         self.publish_mission_status()
         self.publish_task_status(task=self.current_task)
-        self.publish_step_status(step=self.current_step)
 
         self.robot.pause()
 
@@ -370,29 +359,21 @@ class StateMachine(object):
         self.stopped = True
 
     def _initiate_failed(self) -> None:
-        self.current_step.status = StepStatus.Failed
-        self.current_task.update_task_status()
+        self.current_task.status = TaskStatus.Failed
         self.current_mission.status = MissionStatus.Failed
-        self.publish_step_status(step=self.current_step)
         self.publish_task_status(task=self.current_task)
         self._finalize()
 
     def _initiate_infeasible(self) -> None:
-        if self.stepwise_mission:
-            self.current_step.status = StepStatus.Failed
-            self.publish_step_status(step=self.current_step)
-            self.current_task.update_task_status()
+        if self.run_mission_by_task:
+            self.current_task.status = TaskStatus.Failed
             self.publish_task_status(task=self.current_task)
             self.iterate_current_task()
-            self.iterate_current_step()
 
     def _mission_stopped(self) -> None:
         self.current_mission.status = MissionStatus.Cancelled
 
         for task in self.current_mission.tasks:
-            for step in task.steps:
-                if step.status in [StepStatus.NotStarted, StepStatus.InProgress]:
-                    step.status = StepStatus.Cancelled
             if task.status in [
                 TaskStatus.NotStarted,
                 TaskStatus.InProgress,
@@ -406,14 +387,13 @@ class StateMachine(object):
         self.queues.stop_mission.output.put(stopped_mission_response)
 
         self.publish_task_status(task=self.current_task)
-        self.publish_step_status(step=self.current_step)
         self._finalize()
 
     #################################################################################
 
     def _finalize(self) -> None:
         self.publish_mission_status()
-        self.log_step_overview(mission=self.current_mission)
+        self.log_mission_overview(mission=self.current_mission)
         state_transitions: str = ", ".join(
             [
                 f"\n  {transition}" if (i + 1) % 10 == 0 else f"{transition}"
@@ -441,20 +421,6 @@ class StateMachine(object):
                 # Indicates that all tasks are finished
                 self.current_task = None
 
-    def iterate_current_step(self):
-        if self.current_task != None:
-            self.current_step = self.current_task.next_step()
-
-    def update_remaining_steps(self):
-        if self.current_task:
-            for step in self.current_task.steps:
-                if (
-                    step.status == StepStatus.InProgress
-                    or step.status == StepStatus.NotStarted
-                ):
-                    step.status = self.current_task.status
-                    self.publish_step_status(step=step)
-
     def update_state(self):
         """Updates the current state of the state machine."""
         self.current_state = States(self.state)
@@ -466,7 +432,6 @@ class StateMachine(object):
     def reset_state_machine(self) -> None:
         self.logger.info("Resetting state machine")
         self.stopped = False
-        self.current_step = None
         self.current_task = None
         self.current_mission = None
         self.initial_pose = None
@@ -538,7 +503,7 @@ class StateMachine(object):
             retain=True,
         )
 
-    def publish_task_status(self, task: Task) -> None:
+    def publish_task_status(self, task: TASKS) -> None:
         """Publishes the task status to the MQTT Broker"""
         if not self.mqtt_publisher:
             return
@@ -555,6 +520,7 @@ class StateMachine(object):
                 "mission_id": self.current_mission.id if self.current_mission else None,
                 "task_id": task.id if task else None,
                 "status": task.status if task else None,
+                "task_type": task.type,
                 "error_reason": error_message.error_reason if error_message else None,
                 "error_description": (
                     error_message.error_description if error_message else None
@@ -571,41 +537,6 @@ class StateMachine(object):
             retain=True,
         )
 
-    def publish_step_status(self, step: Step) -> None:
-        """Publishes the step status to the MQTT Broker"""
-        if not self.mqtt_publisher:
-            return
-
-        error_message: Optional[ErrorMessage] = None
-        if step:
-            if step.error_message:
-                error_message = step.error_message
-
-        payload: str = json.dumps(
-            {
-                "isar_id": settings.ISAR_ID,
-                "robot_name": settings.ROBOT_NAME,
-                "mission_id": self.current_mission.id if self.current_mission else None,
-                "task_id": self.current_task.id if self.current_task else None,
-                "step_id": step.id if step else None,
-                "step_type": step.__class__.__name__ if step else None,
-                "status": step.status if step else None,
-                "error_reason": error_message.error_reason if error_message else None,
-                "error_description": (
-                    error_message.error_description if error_message else None
-                ),
-                "timestamp": datetime.now(timezone.utc),
-            },
-            cls=EnhancedJSONEncoder,
-        )
-
-        self.mqtt_publisher.publish(
-            topic=settings.TOPIC_ISAR_STEP,
-            payload=payload,
-            qos=1,
-            retain=True,
-        )
-
     def publish_status(self) -> None:
         if not self.mqtt_publisher:
             return
@@ -638,18 +569,13 @@ class StateMachine(object):
         """Logs all state transitions that are not self-transitions."""
         self.transitions_list.append(next_state)
 
-    def log_step_overview(self, mission: Mission):
-        """Log an overview of the steps in a mission"""
+    def log_mission_overview(self, mission: Mission):
+        """Log an overview of the tasks in a mission"""
         log_statements: List[str] = []
         for task in mission.tasks:
             log_statements.append(
                 f"{type(task).__name__:<20} {str(task.id)[:8]:<32} -- {task.status}"
             )
-            for j, step in enumerate(task.steps):
-                log_statements.append(
-                    f"{j:>3} {type(step).__name__:<20} {str(step.id)[:8]:<32} -- {step.status}"  # noqa: E501
-                )
-
         log_statement: str = "\n".join(log_statements)
 
         self.logger.info(f"Mission overview:\n{log_statement}")
@@ -660,8 +586,6 @@ class StateMachine(object):
             mission_status=self.current_mission.status,
             task_id=self.current_task.id,
             task_status=self.current_task.status,
-            step_id=self.current_step.id,
-            step_status=self.current_step.status,
         )
 
 

isar/state_machine/states/idle.py

@@ -82,4 +82,6 @@ class Idle(State):
                 transition = self.state_machine.robot_turned_offline  # type: ignore
                 break
 
+            self.robot_status_thread = None
+
         transition()

isar/state_machine/states/initialize.py

@@ -57,7 +57,7 @@ class Initialize(State):
                 continue
 
             except (RobotInitializeException, RobotException) as e:
-                self.state_machine.current_step.error_message = ErrorMessage(
+                self.state_machine.current_task.error_message = ErrorMessage(
                     error_reason=e.error_reason, error_description=e.error_description
                 )
                 self.logger.error(