smach shutdown handler

auv_control/auv_control/scripts/prop_transform_publisher.py

@@ -22,6 +22,7 @@
 import time
 import copy
 import threading
+from std_srvs.srv import Trigger, TriggerRequest, TriggerResponse
 
 
 class Scene:
@@ -311,6 +312,10 @@ def __init__(self):
         )
         self.set_object_transform_service.wait_for_service()
 
+        self.clear_transforms_service = rospy.Service(
+            "/taluy/clear_prop_transforms", Trigger, self.handle_clear_transforms
+        )
+
         # Subscriptions
         yolo_result_subscriber = message_filters.Subscriber("/yolo_result", YoloResult)
         altitude_subscriber = message_filters.Subscriber(
@@ -799,6 +804,10 @@ def process_altitude_projection(self, detection, altitude: float):
         # transform_message.transform.rotation.z = 0.0
         # transform_message.transform.rotation.w = 1.0
 
+    def handle_clear_transforms(self, req: TriggerRequest):
+        self.scene = Scene()
+        return TriggerResponse(success=True, message="Prop transforms cleared")
+
     def run(self):
         self.rate = rospy.Rate(30.0)
         while rospy.is_shutdown() is False:

auv_navigation/auv_mapping/include/auv_mapping/object_map_tf_server_ros.hpp

@@ -3,6 +3,7 @@
 #include <auv_msgs/SetObjectTransform.h>
 #include <geometry_msgs/TransformStamped.h>
 #include <ros/ros.h>
+#include <std_srvs/Trigger.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <tf2_ros/transform_broadcaster.h>
 #include <tf2_ros/transform_listener.h>
@@ -35,6 +36,10 @@ class ObjectMapTFServerROS {
         "set_object_transform", &ObjectMapTFServerROS::set_transform_handler,
         this);
 
+    clear_transforms_service_ = nh_.advertiseService(
+        "clear_object_transforms",
+        &ObjectMapTFServerROS::clear_transforms_handler, this);
+
     ROS_INFO("ObjectMapTFServerROS initialized. Static frame: %s",
              static_frame_.c_str());
   }
@@ -110,6 +115,22 @@ class ObjectMapTFServerROS {
     return true;
   }
 
+  bool clear_transforms_handler(std_srvs::Trigger::Request &req,
+                                std_srvs::Trigger::Response &res) {
+    size_t size_before;
+    {
+      auto lock = std::scoped_lock(mutex_);
+      size_before = transforms_.size();
+      transforms_.clear();
+    }
+
+    ROS_INFO("Cleared all stored transforms (previously stored: %zu)",
+             size_before);
+    res.success = true;
+    res.message = "Cleared all stored transforms";
+    return true;
+  }
+
   void publishTransforms() {
     auto rate = ros::Rate{rate_};
     while (ros::ok()) {
@@ -150,6 +171,7 @@ class ObjectMapTFServerROS {
   //
   std::mutex mutex_;
   ros::ServiceServer service_;
+  ros::ServiceServer clear_transforms_service_;
 };
 
 }  // namespace auv_mapping

auv_navigation/auv_mapping/launch/start.launch

@@ -9,6 +9,7 @@
       <param name="static_frame" value="odom" />
 
       <remap from="set_object_transform" to="map/set_object_transform" />
+      <remap from="clear_object_transforms" to="map/clear_object_transforms" />
     </node>
   </group>
 

auv_smach/scripts/main_state_machine.py

@@ -9,6 +9,7 @@
 from auv_smach.torpedo import TorpedoTaskState
 from auv_smach.bin import BinTaskState
 from auv_smach.octagon import OctagonTaskState
+from auv_smach.deinitialize import DeinitializeState
 from std_msgs.msg import Bool
 import threading
 
@@ -51,17 +52,17 @@ def __init__(self):
                 InitializeState(),
                 transitions={
                     "succeeded": "NAVIGATE_THROUGH_GATE",
-                    "preempted": "preempted",
-                    "aborted": "aborted",
+                    "preempted": "DEINITIALIZE",
+                    "aborted": "DEINITIALIZE",
                 },
             )
             smach.StateMachine.add(
                 "NAVIGATE_THROUGH_GATE",
                 NavigateThroughGateState(gate_depth=gate_depth),
                 transitions={
                     "succeeded": "NAVIGATE_AROUND_RED_BUOY",
-                    "preempted": "preempted",
-                    "aborted": "aborted",
+                    "preempted": "DEINITIALIZE",
+                    "aborted": "DEINITIALIZE",
                 },
             )
             smach.StateMachine.add(
@@ -73,8 +74,8 @@ def __init__(self):
                 ),
                 transitions={
                     "succeeded": "NAVIGATE_TO_TORPEDO_TASK",
-                    "preempted": "preempted",
-                    "aborted": "aborted",
+                    "preempted": "DEINITIALIZE",
+                    "aborted": "DEINITIALIZE",
                 },
             )
             smach.StateMachine.add(
@@ -85,8 +86,8 @@ def __init__(self):
                 ),
                 transitions={
                     "succeeded": "NAVIGATE_TO_BIN_TASK",
-                    "preempted": "preempted",
-                    "aborted": "aborted",
+                    "preempted": "DEINITIALIZE",
+                    "aborted": "DEINITIALIZE",
                 },
             )
             smach.StateMachine.add(
@@ -96,8 +97,8 @@ def __init__(self):
                 ),
                 transitions={
                     "succeeded": "NAVIGATE_TO_OCTAGON_TASK",
-                    "preempted": "preempted",
-                    "aborted": "aborted",
+                    "preempted": "DEINITIALIZE",
+                    "aborted": "DEINITIALIZE",
                 },
             )
             smach.StateMachine.add(
@@ -107,6 +108,15 @@ def __init__(self):
                 ),
                 transitions={
                     "succeeded": "succeeded",
+                    "preempted": "DEINITIALIZE",
+                    "aborted": "DEINITIALIZE",
+                },
+            )
+            smach.StateMachine.add(
+                "DEINITIALIZE",
+                DeinitializeState(),
+                transitions={
+                    "succeeded": "aborted",
                     "preempted": "preempted",
                     "aborted": "aborted",
                 },

auv_smach/src/auv_smach/common.py

@@ -250,3 +250,23 @@ def execute(self, userdata):
         ) as e:
             rospy.logwarn(f"TF lookup exception: {e}")
             return "aborted"
+
+
+class ClearObjectTransformsState(smach_ros.ServiceState):
+    def __init__(self):
+        smach_ros.ServiceState.__init__(
+            self,
+            "/taluy/map/clear_object_transforms",
+            Trigger,
+            request=TriggerRequest(),
+        )
+
+
+class ClearPropTransformsState(smach_ros.ServiceState):
+    def __init__(self):
+        smach_ros.ServiceState.__init__(
+            self,
+            "/taluy/clear_prop_transforms",
+            Trigger,
+            request=TriggerRequest(),
+        )

auv_smach/src/auv_smach/deinitialize.py

@@ -0,0 +1,57 @@
+import smach
+
+from auv_smach.common import (
+    CancelAlignControllerState,
+    ClearObjectTransformsState,
+    ClearPropTransformsState,
+)
+
+
+class DeinitializeState(smach.State):
+    def __init__(self):
+        smach.State.__init__(self, outcomes=["succeeded", "preempted", "aborted"])
+
+        # Initialize the state machine
+        self.state_machine = smach.StateMachine(
+            outcomes=["succeeded", "preempted", "aborted"]
+        )
+
+        # Open the container for adding states
+        with self.state_machine:
+            smach.StateMachine.add(
+                "CANCEL_ALIGN_CONTROLLER",
+                CancelAlignControllerState(),
+                transitions={
+                    "succeeded": "CLEAR_OBJECT_TRANSFORMS",
+                    "preempted": "CLEAR_OBJECT_TRANSFORMS",
+                    "aborted": "CLEAR_OBJECT_TRANSFORMS",
+                },
+            )
+            smach.StateMachine.add(
+                "CLEAR_OBJECT_TRANSFORMS",
+                ClearObjectTransformsState(),
+                transitions={
+                    "succeeded": "CLEAR_PROP_TRANSFORMS",
+                    "preempted": "CLEAR_PROP_TRANSFORMS",
+                    "aborted": "CLEAR_PROP_TRANSFORMS",
+                },
+            )
+            smach.StateMachine.add(
+                "CLEAR_PROP_TRANSFORMS",
+                ClearPropTransformsState(),
+                transitions={
+                    "succeeded": "aborted",
+                    "preempted": "preempted",
+                    "aborted": "aborted",
+                },
+            )
+
+    def execute(self, userdata):
+        # Execute the state machine
+        outcome = self.state_machine.execute()
+
+        if outcome is None:
+            return "preempted"
+
+        # Return the outcome of the state machine
+        return outcome