Guveloglu/smach to bt acoustic

.gitignore

@@ -40,3 +40,6 @@
 
 # ultralytics models
 *.pt
+
+# Personal learning notes
+docs/learning/

.pre-commit-config.yaml

@@ -49,7 +49,16 @@ repos:
       - id: catkin_lint
         name: catkin_lint
         description: Check package.xml and cmake files
-        entry: catkin_lint . --skip-path auv_sim
+        entry: |
+          bash -c 'if [ -z "$ROS_PACKAGE_PATH" ]; then
+            if [ "$CI" = "true" ]; then
+              echo "Warning: ROS_PACKAGE_PATH not set in CI. Skipping catkin_lint.";
+              exit 0;
+            fi;
+            echo "Error: ROS_PACKAGE_PATH is not set. Please source your workspace setup.bash (e.g., source devel/setup.bash) before running git commit/push.";
+            exit 1;
+          fi;
+          catkin_lint . --skip-path auv_sim'
         language: system
         always_run: true
         pass_filenames: false

auv_smach/package.xml

@@ -41,20 +41,28 @@
   <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>auv_navigation</build_depend>
+  <build_depend>py_trees</build_depend>
+  <build_depend>py_trees_ros</build_depend>
   <build_depend>rospy</build_depend>
   <build_depend>smach</build_depend>
   <build_depend>smach_ros</build_depend>
   <build_depend>std_srvs</build_depend>
   <build_export_depend>auv_navigation</build_export_depend>
+  <build_export_depend>py_trees</build_export_depend>
+  <build_export_depend>py_trees_ros</build_export_depend>
   <build_export_depend>rospy</build_export_depend>
   <build_export_depend>smach</build_export_depend>
   <build_export_depend>smach_ros</build_export_depend>
   <build_export_depend>std_srvs</build_export_depend>
   <exec_depend>auv_navigation</exec_depend>
+  <exec_depend>py_trees</exec_depend>
+  <exec_depend>py_trees</exec_depend>
+  <exec_depend>py_trees_ros</exec_depend>
   <exec_depend>rospy</exec_depend>
   <exec_depend>smach</exec_depend>
   <exec_depend>smach_ros</exec_depend>
   <exec_depend>std_srvs</exec_depend>
+  <!-- py_trees for Behavior Tree support -->
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <!-- Other tools can request additional information be placed here -->

auv_smach/src/auv_smach/acoustic.py

@@ -1,100 +1,130 @@
 #!/usr/bin/env python3
+"""
+SMACH State wrappers for Acoustic Behavior Trees.
+
+This module provides SMACH-compatible wrappers that internally use
+py_trees behaviors for acoustic modem communication.
+"""
 
 import rospy
 import smach
-from std_msgs.msg import UInt8
-from auv_msgs.srv import SendAcoustic, SendAcousticRequest
-import smach_ros
-
-
-class AcousticTransmitter(smach_ros.ServiceState):
-    def __init__(self, acoustic_data):
-        super(AcousticTransmitter, self).__init__(
-            "send_acoustic_data",
-            SendAcoustic,
-            request=SendAcousticRequest(data=acoustic_data),
-            outcomes=["succeeded", "preempted", "aborted"],
-        )
+import py_trees
+import py_trees_ros
+from auv_smach.behaviors.actions import (
+    AcousticTransmitBehavior,
+    AcousticReceiveBehavior,
+)
 
 
-class AcousticReceiver(smach.State):
+class AcousticTransmitter(smach.State):
+    """
+    SMACH State wrapper for AcousticTransmitBehavior.
 
-    def __init__(self, expected_data=None, timeout=None):
+    Transmits acoustic data via the acoustic modem using the underlying
+    py_trees behavior.
+    """
+
+    def __init__(self, acoustic_data: int):
         smach.State.__init__(self, outcomes=["succeeded", "preempted", "aborted"])
+        self.acoustic_data = acoustic_data
+        self.behaviour_tree = None
 
-        self.expected_data = expected_data
-        self.timeout = timeout
-        self.data_received = False
+    def execute(self, userdata):
+        rospy.loginfo(f"[AcousticTransmitter] Transmitting data: {self.acoustic_data}")
 
-        # Create subscriber for acoustic modem
-        self.acoustic_sub = rospy.Subscriber(
-            "modem/data/rx", UInt8, self.acoustic_callback
-        )
+        try:
+            # Create the behavior
+            root = AcousticTransmitBehavior(
+                name="TransmitAcoustic",
+                acoustic_data=self.acoustic_data,
+            )
 
-        rospy.loginfo(
-            f"AcousticReceiver initialized - expected: {expected_data}, timeout: {timeout}s"
-        )
+            # Initialize the tree
+            self.behaviour_tree = py_trees_ros.trees.BehaviourTree(root)
+            self.behaviour_tree.setup(timeout=15.0)
+
+            # Single tick for service call
+            self.behaviour_tree.tick()
+            status = root.status
 
-    def acoustic_callback(self, msg):
-        rospy.logdebug(f"Received acoustic data: {msg.data}")
-
-        # Check if this is the expected data
-        if self.expected_data is None:
-            # Accept any data
-            self.data_received = True
-            rospy.loginfo(f"Acoustic data received: {msg.data}")
-        elif isinstance(self.expected_data, list):
-            # Check if data is in the expected list
-            if msg.data in self.expected_data:
-                self.data_received = True
-                rospy.loginfo(f"Expected acoustic data received: {msg.data}")
-        else:
-            # Check if data matches expected value
-            if msg.data == self.expected_data:
-                self.data_received = True
-                rospy.loginfo(f"Expected acoustic data received: {msg.data}")
+            if status == py_trees.common.Status.SUCCESS:
+                rospy.loginfo("[AcousticTransmitter] Transmission succeeded!")
+                return "succeeded"
+
+            if status == py_trees.common.Status.FAILURE:
+                rospy.logerr("[AcousticTransmitter] Transmission failed!")
+                return "aborted"
+
+            return "aborted"
+
+        except rospy.ROSInterruptException:
+            rospy.loginfo("[AcousticTransmitter] Transmission interrupted")
+            return "aborted"
+        except Exception as e:
+            rospy.logerr(f"[AcousticTransmitter] Error during transmission: {e}")
+            return "aborted"
+
+
+class AcousticReceiver(smach.State):
+    """
+    SMACH State wrapper for AcousticReceiveBehavior.
+
+    Waits for acoustic data from the modem using the underlying
+    py_trees behavior.
+    """
+
+    def __init__(self, expected_data=None, timeout=None):
+        smach.State.__init__(self, outcomes=["succeeded", "preempted", "aborted"])
+        self.expected_data = expected_data
+        self.timeout = timeout
+        self.behaviour_tree = None
 
     def execute(self, userdata):
         rospy.loginfo(
-            f"Starting acoustic reception - waiting for: {self.expected_data}"
+            f"[AcousticReceiver] Waiting for data: {self.expected_data}, "
+            f"timeout: {self.timeout}s"
         )
 
-        # Reset state
-        self.data_received = False
+        try:
+            # Create the behavior
+            root = AcousticReceiveBehavior(
+                name="ReceiveAcoustic",
+                expected_data=self.expected_data,
+                timeout=self.timeout,
+            )
 
-        # Calculate timeout
-        if self.timeout is not None:
-            start_time = rospy.Time.now()
-            timeout_time = start_time + rospy.Duration(self.timeout)
-        else:
-            timeout_time = None
+            # Initialize the tree
+            self.behaviour_tree = py_trees_ros.trees.BehaviourTree(root)
+            self.behaviour_tree.setup(timeout=15.0)
 
-        rate = rospy.Rate(10)
+            # Tick loop
+            rate = rospy.Rate(10)  # 10 Hz
 
-        try:
-            while not self.data_received:
-                # Check for preemption
+            while not rospy.is_shutdown():
                 if self.preempt_requested():
                     self.service_preempt()
-                    rospy.loginfo("Acoustic reception preempted")
+                    self.behaviour_tree.interrupt()
+                    rospy.loginfo("[AcousticReceiver] Reception preempted")
                     return "preempted"
 
-                # Check for timeout
-                if timeout_time is not None and rospy.Time.now() >= timeout_time:
-                    rospy.loginfo(
-                        f"Acoustic reception timeout after {self.timeout}s - continuing mission"
-                    )
+                self.behaviour_tree.tick()
+                status = root.status
+
+                if status == py_trees.common.Status.SUCCESS:
+                    rospy.loginfo("[AcousticReceiver] Reception succeeded!")
                     return "succeeded"
 
-                # Sleep and continue waiting
+                if status == py_trees.common.Status.FAILURE:
+                    rospy.logerr("[AcousticReceiver] Reception failed!")
+                    return "aborted"
+
                 rate.sleep()
 
+            return "aborted"
+
         except rospy.ROSInterruptException:
-            rospy.loginfo("Acoustic reception interrupted")
+            rospy.loginfo("[AcousticReceiver] Reception interrupted")
             return "aborted"
         except Exception as e:
-            rospy.logerr(f"Error during acoustic reception: {e}")
+            rospy.logerr(f"[AcousticReceiver] Error during reception: {e}")
             return "aborted"
-
-        rospy.loginfo("Acoustic reception completed - expected data received")
-        return "succeeded"

auv_smach/src/auv_smach/behaviors/__init__.py

@@ -0,0 +1,27 @@
+# Behavior Tree behaviors for AUV tasks
+
+from .actions import (
+    SetDepthBehavior,
+    CancelAlignControllerBehavior,
+    SetDetectionFocusBehavior,
+    SetBoolServiceBehavior,
+    RotateBehavior,
+    SetFrameLookingAtBehavior,
+    AlignFrameBehavior,
+    CreateFrameAtCurrentPositionBehavior,
+    PlanPathBehavior,
+    ExecutePathBehavior,
+    TriggerServiceBehavior,
+    ResetOdometryBehavior,
+    DelayBehavior,
+    ClearObjectMapBehavior,
+    AcousticTransmitBehavior,
+    AcousticReceiveBehavior,
+)
+
+from .conditions import (
+    IsTransformAvailable,
+)
+
+from .gate_tree import create_gate_tree
+from .acoustic_tree import create_acoustic_tree

auv_smach/src/auv_smach/behaviors/acoustic_tree.py

@@ -0,0 +1,68 @@
+import py_trees
+from .actions import AcousticTransmitBehavior, AcousticReceiveBehavior
+
+
+def create_acoustic_tree(
+    transmit_data: int = None,
+    receive_expected: list = None,
+    receive_timeout: float = None,
+    enable_transmit: bool = True,
+    enable_receive: bool = False,
+):
+    """
+    Creates an Acoustic task behavior tree.
+
+    This tree can be used for:
+    - Transmitting acoustic data (status signals)
+    - Receiving acoustic data (coordination with other AUVs)
+    - Both transmit and receive in sequence
+
+    Args:
+        transmit_data: Data value to transmit (1-8). Required if enable_transmit=True.
+        receive_expected: List of expected data values, or None to accept any.
+        receive_timeout: Timeout in seconds for receiving (default: 30s).
+        enable_transmit: Whether to include transmit behavior (default: True).
+        enable_receive: Whether to include receive behavior (default: False).
+
+    Returns:
+        py_trees.composites.Sequence: The acoustic task tree.
+
+    Example usage:
+        # Just transmit
+        tree = create_acoustic_tree(transmit_data=1)
+
+        # Just receive
+        tree = create_acoustic_tree(enable_transmit=False, enable_receive=True,
+                                     receive_expected=[1, 2, 3], receive_timeout=30)
+
+        # Transmit then receive
+        tree = create_acoustic_tree(transmit_data=1, enable_receive=True,
+                                     receive_expected=[1, 2], receive_timeout=60)
+    """
+
+    root = py_trees.composites.Sequence("AcousticTask", memory=True)
+
+    # Add transmit behavior if enabled
+    if enable_transmit:
+        if transmit_data is None:
+            raise ValueError(
+                "transmit_data must be specified when enable_transmit=True"
+            )
+        root.add_child(
+            AcousticTransmitBehavior(
+                name="TransmitAcoustic",
+                acoustic_data=transmit_data,
+            )
+        )
+
+    # Add receive behavior if enabled
+    if enable_receive:
+        root.add_child(
+            AcousticReceiveBehavior(
+                name="ReceiveAcoustic",
+                expected_data=receive_expected,
+                timeout=receive_timeout,
+            )
+        )
+
+    return root