Simplify task incrementation

equinor · Jul 18, 2024 · 23aed9e · 23aed9e
1 parent 670c093
commit 23aed9e
Show file tree

Hide file tree

Showing 2 changed files with 33 additions and 32 deletions.
diff --git a/src/isar/state_machine/state_machine.py b/src/isar/state_machine/state_machine.py
@@ -292,7 +292,7 @@ def _resume(self) -> None:
         self.queues.resume_mission.output.put(resume_mission_response)
 
         self.current_task.reset_task()
-        self.update_current_step()
+        self.increment_current_step()
 
         self.robot.resume()
 
@@ -337,12 +337,12 @@ def _mission_started(self) -> None:
         else:
             self.current_task.status = TaskStatus.InProgress
             self.publish_task_status(task=self.current_task)
-            self.update_current_step()
+            self.increment_current_step()
 
     def _step_finished(self) -> None:
         self.publish_step_status(step=self.current_step)
-        self.update_current_task()
-        self.update_current_step()
+        self.increment_current_task()
+        self.increment_current_step()
 
     def _full_mission_finished(self) -> None:
         self.current_task = None
@@ -379,8 +379,8 @@ def _initiate_infeasible(self) -> None:
         if self.stepwise_mission:
             self.current_step.status = StepStatus.Failed
             self.publish_step_status(step=self.current_step)
-            self.update_current_task()
-            self.update_current_step()
+            self.increment_current_task()
+            self.increment_current_step()
 
     def _mission_stopped(self) -> None:
         self.current_mission.status = MissionStatus.Cancelled
@@ -427,10 +427,8 @@ def begin(self):
         """
         self.to_idle()
 
-    def update_current_task(self):
+    def increment_current_task(self):
         if self.current_task.is_finished():
-            self.current_task.update_task_status()
-            self.publish_task_status(task=self.current_task)
             try:
                 self.current_task = self.task_selector.next_task()
                 self.current_task.status = TaskStatus.InProgress
@@ -439,7 +437,7 @@ def update_current_task(self):
                 # Indicates that all tasks are finished
                 self.current_task = None
 
-    def update_current_step(self):
+    def increment_current_step(self):
         if self.current_task != None:
             self.current_step = self.current_task.next_step()
 

diff --git a/src/isar/state_machine/states/monitor.py b/src/isar/state_machine/states/monitor.py
@@ -127,29 +127,32 @@ def _run(self) -> None:
             else:
                 if self._is_step_finished(self.state_machine.current_step):
                     self._report_step_status(self.state_machine.current_step)
-                    self.state_machine.update_current_task()
-                    if self.state_machine.current_task == None:
-                        transition = self.state_machine.full_mission_finished  # type: ignore
-                        break
-                    self.state_machine.update_current_step()
-                    self.state_machine.current_task.update_task_status()
-                else:  # If not all steps are done
-                    self.state_machine.current_task.status = TaskStatus.InProgress
 
-                self.state_machine.publish_task_status(
-                    self.state_machine.current_task
-                )
-                if self.state_machine.current_task.status == TaskStatus.Successful:
-                    try:
-                        self.state_machine.current_task = (
-                            self.state_machine.task_selector.next_task()
+
+
+                    if self.state_machine.current_task.is_finished():
+                        # Report and update finished task
+                        self.state_machine.current_task.update_task_status()  # Uses the updated step status to set the task status
+                        self.state_machine.publish_task_status(
+                            task=self.state_machine.current_task
+                        )
+
+                        self.state_machine.increment_current_task()
+
+                        if self.state_machine.current_task == None:
+                            transition = self.state_machine.full_mission_finished  # type: ignore
+                            break
+
+                        # Report and update next task
+                        self.state_machine.current_task.update_task_status()
+                        self.state_machine.publish_task_status(
+                            task=self.state_machine.current_task
                         )
-                    except TaskSelectorStop:
-                        # Indicates that all tasks are finished
-                        self.state_machine.current_task = None
-                        transition = self.state_machine.full_mission_finished  # type: ignore
-                        break
-                    self.state_machine.update_current_step()
+
+                    self.state_machine.increment_current_step()
+
+                else:  # If not all steps are done
+                    self.state_machine.current_task.status = TaskStatus.InProgress
 
             self.step_status_thread = None
             time.sleep(self.state_machine.sleep_time)
@@ -199,7 +202,7 @@ def _is_step_finished(self, step: Step) -> bool:
         elif step.status == StepStatus.Successful:
             finished = True
         return finished
-    
+
     def _report_step_status(self, step: Step) -> None:
         if step.status == StepStatus.Failed:
             self.logger.warning(