Reset branch again

python/lsst/ts/externalscripts/auxtel/latiss_base_align.py

@@ -22,6 +22,7 @@
 __all__ = ["LatissBaseAlign"]
 
 import abc
+import asyncio
 import dataclasses
 import types
 import typing
@@ -30,6 +31,7 @@
 import yaml
 from lsst.ts import salobj
 from lsst.ts.idl.enums.ATPtg import WrapStrategy
+from lsst.ts.idl.enums.Script import ScriptState
 from lsst.ts.observatory.control.auxtel.atcs import ATCS
 from lsst.ts.observatory.control.auxtel.latiss import LATISS
 from lsst.ts.observatory.control.constants import atcs_constants
@@ -181,6 +183,9 @@ def __init__(self, index=1, remotes=True, descr=""):
 
         self.camera_playlist = None
 
+        # Flag to monitor if iterations have started for cleanup task.
+        self.iterations_started = False
+
     # define the method that sets the hexapod offset to create intra/extra
     # focal images
     @property
@@ -741,25 +746,29 @@ async def arun(self, checkpoint: bool = False) -> None:
         self.offset_total_focus = 0.0
         self.offset_total_coma_x = 0.0
         self.offset_total_coma_y = 0.0
+
+        # Getting initial hexapod position
+        self.initial_hexapod_position = await self.latiss.get_hexapod_position()
+
+        # Getting total number of executed iterations
+        self.executed_iterations = 0
+
+        # Flagging that iterations have started
+        self.iterations_started = True
         for i in range(self.max_iter):
             self.log.debug(f"CWFS iteration {i + 1} starting...")
-
             if checkpoint:
                 await self.checkpoint(
                     f"[{i + 1}/{self.max_iter}]: CWFS loop starting..."
                 )
-
             # Setting visit_id's to none so run_cwfs will take a new dataset.
             self.intra_visit_id = None
             self.extra_visit_id = None
             await self.take_intra_extra()
-
             results = await self.run_align()
-
             coma_x = results.offset_hex[0]
             coma_y = results.offset_hex[1]
             focus_offset = results.offset_hex[2]
-
             total_coma_offset = np.sqrt(coma_x**2.0 + coma_y**2.0)
             if (
                 abs(focus_offset) < self.threshold
@@ -779,7 +788,6 @@ async def arun(self, checkpoint: bool = False) -> None:
                 self.offset_total_coma_x += coma_x
                 self.offset_total_coma_y += coma_y
                 await self.atcs.offset_aos_lut(z=focus_offset, x=coma_x, y=coma_y)
-
                 current_target = await self.atcs.rem.atptg.evt_currentTarget.aget(
                     timeout=self.timeout_short
                 )
@@ -801,7 +809,6 @@ async def arun(self, checkpoint: bool = False) -> None:
                     program=self.program,
                 )
                 await self.atcs.add_point_data()
-
                 self.log.info("latiss_cwfs_align script completed successfully!\n")
                 return
             elif abs(focus_offset) > self.large_defocus:
@@ -829,13 +836,37 @@ async def arun(self, checkpoint: bool = False) -> None:
                 self.offset_total_coma_y += coma_y
                 await self.atcs.offset_aos_lut(z=focus_offset, x=coma_x, y=coma_y)
 
+            # Not sure if here is the best place to get number of executed
+            # iterations
+            self.executed_iterations += 1
+
+        # If we reach this point, it means we did not converge.
         self.log.warning(
-            f"Reached maximum iteration ({self.max_iter}) without convergence.\n"
+            f"Reached maximum iteration ({self.max_iter}) without convergence."
+            f"Returning hexapod to initial position. \n"
+        )
+
+        self.rem.ataos.evt_detailedState.flush()
+
+        # Return hexapod to original position
+        await self.atcs.rem.ataos.cmd_offset.set_start(
+            z=self.initial_hexapod_position.z,
+            x=self.initial_hexapod_position.x,
+            y=self.initial_hexapod_position.y,
+            timeout=self.long_timeout,
         )
 
+    # Place holder (to remove after review)
+    #    await self.hexapod_offset.reset.set_start(
+    #        focus=self.initial_hexapod_position.z,
+    #        x=self.initial_hexapod_position.x,
+    #        y=self.initial_hexapod_position.y,
+    #        timeout=self.long_timeout,
+    #    )
+
     async def assert_feasibility(self) -> None:
-        """Verify that the telescope and camera are in a feasible state to
-        execute the script.
+        """Verify that the telescope and camera are in
+        a feasible state to execute the script.
         """
 
         await self.atcs.assert_all_enabled()
@@ -880,3 +911,38 @@ async def run(self) -> None:
         await self.assert_feasibility()
 
         await self.arun(True)
+
+    async def cleanup(self):
+        if self.state.state != ScriptState.ENDING:
+            # abnormal termination
+            if self.iterations_started and self.executed_iterations != self.max_iter:
+                self.log.warning(
+                    f"Terminating with state={self.state.state}: stop iterations. "
+                    f"Iterations started: {self.iterations_started}. "
+                )
+                try:
+                    self.log.warning(
+                        f"Looping failed at iter {self.executed_iterations} from a total of "
+                        f"{self.max_iter}. Returning hexapod to its initial position.\n"
+                    )
+
+                    self.rem.ataos.evt_detailedState.flush()
+
+                    # Return hexapod to original position
+                    await self.atcs.rem.ataos.cmd_offset.set_start(
+                        z=self.initial_hexapod_position.z,
+                        x=self.initial_hexapod_position.x,
+                        y=self.initial_hexapod_position.y,
+                        timeout=self.long_timeout,
+                    )
+
+                except asyncio.TimeoutError:
+                    self.log.exception(
+                        "Resetting hexapod to its initial position timed "
+                        "out during cleanup procedure."
+                    )
+                except Exception:
+                    self.log.exception(
+                        "Unexpected exception in resetting "
+                        "hexapod to its initial position."
+                    )