use aborted outcome to call cancel_align_controller

itu-auv · Dec 22, 2024 · 1fed4e1 · 1fed4e1
1 parent ee70ac7
commit 1fed4e1
Showing 1 changed file with 15 additions and 21 deletions.
diff --git a/auv_smach/scripts/main_state_machine.py b/auv_smach/scripts/main_state_machine.py
@@ -2,8 +2,8 @@
 
 import rospy
 import smach
-import signal
 import auv_smach
+from auv_smach.common import CancelAlignControllerState
 from std_srvs.srv import Trigger, TriggerRequest
 from auv_smach.initialize import InitializeState
 from auv_smach.gate import NavigateThroughGateState
@@ -44,12 +44,6 @@ def __init__(self):
 
         rospy.Subscriber("/taluy/propulsion_board/status", Bool, self.enabled_callback)
 
-        self.cancel_align_controller_service = rospy.ServiceProxy(
-            "/taluy/control/align_frame/cancel", Trigger
-        )
-
-        signal.signal(signal.SIGINT, self.shutdown_handler)
-
         self.sm = smach.StateMachine(outcomes=["succeeded", "preempted", "aborted"])
 
         with self.sm:
@@ -60,7 +54,7 @@ def __init__(self):
                 transitions={
                     "succeeded": "NAVIGATE_THROUGH_GATE",
                     "preempted": "preempted",
-                    "aborted": "aborted",
+                    "aborted": "CANCEL_ALIGN_CONTROLLER",
                 },
             )
             smach.StateMachine.add(
@@ -69,7 +63,7 @@ def __init__(self):
                 transitions={
                     "succeeded": "NAVIGATE_AROUND_RED_BUOY",
                     "preempted": "preempted",
-                    "aborted": "aborted",
+                    "aborted": "CANCEL_ALIGN_CONTROLLER",
                 },
             )
             smach.StateMachine.add(
@@ -82,7 +76,7 @@ def __init__(self):
                 transitions={
                     "succeeded": "NAVIGATE_TO_TORPEDO_TASK",
                     "preempted": "preempted",
-                    "aborted": "aborted",
+                    "aborted": "CANCEL_ALIGN_CONTROLLER",
                 },
             )
             smach.StateMachine.add(
@@ -94,7 +88,7 @@ def __init__(self):
                 transitions={
                     "succeeded": "NAVIGATE_TO_BIN_TASK",
                     "preempted": "preempted",
-                    "aborted": "aborted",
+                    "aborted": "CANCEL_ALIGN_CONTROLLER",
                 },
             )
             smach.StateMachine.add(
@@ -105,7 +99,7 @@ def __init__(self):
                 transitions={
                     "succeeded": "NAVIGATE_TO_OCTAGON_TASK",
                     "preempted": "preempted",
-                    "aborted": "aborted",
+                    "aborted": "CANCEL_ALIGN_CONTROLLER",
                 },
             )
             smach.StateMachine.add(
@@ -116,6 +110,15 @@ def __init__(self):
                 transitions={
                     "succeeded": "succeeded",
                     "preempted": "preempted",
+                    "aborted": "CANCEL_ALIGN_CONTROLLER",
+                },
+            )
+            smach.StateMachine.add(
+                "CANCEL_ALIGN_CONTROLLER",
+                CancelAlignControllerState(),
+                transitions={
+                    "succeeded": "aborted",
+                    "preempted": "preempted",
                     "aborted": "aborted",
                 },
             )
@@ -138,15 +141,6 @@ def enabled_callback(self, msg):
             # restart
             rospy.Timer(rospy.Duration(0.1), self.start)
 
-    def shutdown_handler(self):
-        rospy.loginfo("Starting cleanup before exiting smach")
-
-        rospy.loginfo("Calling cancel_align_controller service")
-        self.cancel_align_controller_service(TriggerRequest())
-
-        rospy.loginfo("Cleanup complete. Shutting down.")
-        rospy.signal_shutdown("SIGINT received.")
-
 
 if __name__ == "__main__":
     rospy.init_node("main_state_machine")