Guveloglu/smach to bt acoustic by OmerFarukGuveloglu · Pull Request #539 · itu-auv/auv-software

OmerFarukGuveloglu · 2026-02-02T23:44:45Z

No description provided.

…er runtime)

…NaNs after runtime)" This reverts commit ef06274.

- Reimplement Gate logic using BT wrapper - Implement Coin Flip, Roll, and Yaw maneuvers - Refactor common behaviors - Update dependencies

Copilot

Pull request overview

This pull request refactors the AUV gate navigation task from a SMACH state machine implementation to a py_trees behavior tree implementation, while maintaining SMACH compatibility through wrapper classes. The PR also adds acoustic modem communication support using behavior trees.

Changes:

Refactored NavigateThroughGateState to use py_trees behavior trees internally while maintaining SMACH interface
Created comprehensive behavior tree library including actions, conditions, and subtrees for AUV tasks
Migrated acoustic communication from pure SMACH to behavior tree-based implementation with SMACH wrappers
Added py_trees and py_trees_ros dependencies to package.xml
Enhanced pre-commit configuration with CI-aware ROS environment checking

Reviewed changes

Copilot reviewed 12 out of 13 changed files in this pull request and generated 13 comments.

Show a summary per file

File	Description
auv_smach/src/auv_smach/gate.py	Simplified gate state by replacing internal SMACH state machine with behavior tree execution via create_gate_tree()
auv_smach/src/auv_smach/behaviors/gate_tree.py	Defines complete gate task behavior tree structure with configurable maneuvers (roll, yaw, coin flip)
auv_smach/src/auv_smach/behaviors/subtrees.py	Reusable behavior subtrees for search, look-around, and dynamic path planning patterns
auv_smach/src/auv_smach/behaviors/roll.py	California Roll and Two Yaw maneuver implementations with pitch correction and odometry-based rotation tracking
auv_smach/src/auv_smach/behaviors/coin_flip.py	Coin flip maneuver subtree for rescue positioning
auv_smach/src/auv_smach/behaviors/conditions.py	TF transform availability condition for behavior tree decision making
auv_smach/src/auv_smach/behaviors/actions.py	Comprehensive library of atomic behaviors including depth control, alignment, rotation, path planning, and acoustic communication
auv_smach/src/auv_smach/behaviors/acoustic_tree.py	Configurable acoustic task tree for transmit/receive operations
auv_smach/src/auv_smach/behaviors/init.py	Module exports for behaviors package
auv_smach/src/auv_smach/acoustic.py	Refactored SMACH acoustic states to wrap py_trees behaviors internally
auv_smach/package.xml	Added py_trees and py_trees_ros dependencies
.pre-commit-config.yaml	Enhanced catkin_lint hook with ROS environment checks and CI skip logic
.gitignore	Added exclusion for personal learning notes directory

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

Copilot · 2026-02-02T23:49:40Z

auv_smach/package.xml

  <exec_depend>smach</exec_depend>
  <exec_depend>smach_ros</exec_depend>
  <exec_depend>std_srvs</exec_depend>
+  <!-- py_trees for Behavior Tree support -->


Misplaced comment. The comment "py_trees for Behavior Tree support" on line 65 appears after all the dependency declarations and doesn't provide useful context at this location. It would be more helpful if placed near the py_trees dependency declarations (around lines 44-45 or 58-60), or it can be removed entirely as the dependency purpose is self-evident from the package name.

Copilot · 2026-02-02T23:49:40Z

auv_smach/src/auv_smach/behaviors/actions.py

+            self._thread.join(timeout=2.0)
+            if self._thread.is_alive():
+                rospy.logerr(
+                    f"[{self.name}] CRITICAL: Thread did not finish ignoring cancel!"


Grammar error in log message. The message "Thread did not finish ignoring cancel!" is missing punctuation. It should be "Thread did not finish, ignoring cancel!" or "Thread did not finish - ignoring cancel!" to make the meaning clearer.

Suggested change

f"[{self.name}] CRITICAL: Thread did not finish ignoring cancel!"

f"[{self.name}] CRITICAL: Thread did not finish, ignoring cancel!"

Copilot · 2026-02-02T23:49:41Z

auv_smach/src/auv_smach/behaviors/actions.py

+class RotateBehavior(py_trees.behaviour.Behaviour):
+    """
+    Rotates the vehicle until a TF frame is found or for a specific duration.
+    Can perform a 'blind' rotation (Twist) if needed.
+    SMACH equivalent: RotationState (common.py:408)
+    """
+
+    def __init__(
+        self,
+        name: str,
+        source_frame: str,
+        look_at_frame: str,
+        rotation_speed: float = 0.2,
+        timeout: float = 25.0,
+        rotation_radian: float = None,  # Total angle to rotate (if None, rotate until TF found)
+        velocity_topic: str = "cmd_vel",
+    ):
+        super().__init__(name)
+        self.source_frame = source_frame
+        self.look_at_frame = look_at_frame
+        self.rotation_speed = rotation_speed
+        self.timeout = timeout
+        self.rotation_radian = rotation_radian
+        self.velocity_topic = velocity_topic
+
+        # Runtime
+        self._start_time = None
+        self._tf_buffer = None
+        self._tf_listener = None
+        self.pub_vel = None
+
+    def setup(self, timeout=15, **kwargs):
+        try:
+            self._tf_buffer = tf2_ros.Buffer()
+            self._tf_listener = tf2_ros.TransformListener(self._tf_buffer)
+            self.pub_vel = rospy.Publisher(self.velocity_topic, Twist, queue_size=1)
+            return True
+        except Exception as e:
+            rospy.logerr(f"[{self.name}] Setup error: {e}")
+            return False
+
+    def initialise(self):
+        self._start_time = rospy.Time.now()
+        rospy.loginfo(f"[{self.name}] Starting rotation...")
+
+    def update(self):
+        # 0. Check Timeout
+        elapsed = (rospy.Time.now() - self._start_time).to_sec()
+        if elapsed > self.timeout:
+            rospy.logwarn(f"[{self.name}] Timeout ({elapsed:.1f}s)")
+            self._stop()
+            return py_trees.common.Status.FAILURE
+
+        # 1. If we just want to find a TF frame (classic usage)
+        if self.rotation_radian is None:
+            if self._is_transform_available():
+                rospy.loginfo(f"[{self.name}] Target frame found via TF")
+                self._stop()
+                return py_trees.common.Status.SUCCESS
+
+            # Spin
+            self._spin()
+            return py_trees.common.Status.RUNNING
+
+        # 2. If we want to rotate for a specific angle (e.g. 360 spin)
+        # Simply rotate for calculated time (open loop approximation as per SMACH)
+        # SMACH calculates time = radian / speed
+        duration_needed = abs(self.rotation_radian / self.rotation_speed)
+
+        if elapsed >= duration_needed:
+            rospy.loginfo(f"[{self.name}] Rotation complete (Time-based)")
+            self._stop()
+            return py_trees.common.Status.SUCCESS
+
+        self._spin()
+        return py_trees.common.Status.RUNNING
+
+    def terminate(self, new_status):
+        if new_status != py_trees.common.Status.RUNNING:
+            self._stop()
+
+    def _spin(self):
+        cmd = Twist()
+        cmd.angular.z = self.rotation_speed
+        self.pub_vel.publish(cmd)
+
+    def _stop(self):
+        cmd = Twist()
+        cmd.angular.z = 0.0
+        if self.pub_vel:
+            self.pub_vel.publish(cmd)
+
+    def _is_transform_available(self):
+        try:
+            self._tf_buffer.lookup_transform(
+                self.source_frame,
+                self.look_at_frame,
+                rospy.Time(0),
+                rospy.Duration(0.0),  # Non-blocking check
+            )
+            return True
+        except (
+            tf2_ros.LookupException,
+            tf2_ros.ConnectivityException,
+            tf2_ros.ExtrapolationException,
+        ):
+            return False


Duplicate class definition detected. The RotateBehavior class is defined twice in this file (lines 249 and 781). The first definition (lines 249-429) includes odometry tracking, killswitch monitoring, and enable signal publishing. The second definition (lines 781-887) is simpler and uses time-based rotation. These two implementations have different functionality and cannot coexist with the same name. One should be renamed or removed, or their functionality should be consolidated.

Copilot · 2026-02-02T23:49:41Z

auv_smach/src/auv_smach/behaviors/actions.py

+                res = self.align_srv(req)
+                if res.success:
+                    self._alignment_requested = True
+                    self._alignment_requested = True


Duplicate assignment detected. The variable self._alignment_requested is assigned True twice on consecutive lines. Remove the duplicate assignment on line 706.

Suggested change

self._alignment_requested = True

Copilot · 2026-02-02T23:49:41Z

auv_smach/src/auv_smach/behaviors/actions.py

+class CreateFrameBehavior(py_trees.behaviour.Behaviour):
+    """
+    Calls the /create_frame service to create a new TF frame.
+    SMACH equivalent: CreateFrameState (common.py:1000)
+    """
+
+    def __init__(
+        self,
+        name: str,
+        parent_frame: str,
+        child_frame: str,
+        x: float,
+        y: float,
+        yaw: float,
+    ):
+        super().__init__(name)
+        self.parent_frame = parent_frame
+        self.child_frame = child_frame
+        self.x = x
+        self.y = y
+        self.yaw = yaw
+        self._done = False
+
+    def setup(self, timeout=15, **kwargs):
+        try:
+            self.create_frame_srv = rospy.ServiceProxy("/create_frame", CreateFrame)
+            return True
+        except Exception as e:
+            rospy.logerr(f"[{self.name}] Setup error: {e}")
+            return False
+
+    def initialise(self):
+        self._done = False
+
+    def update(self):
+        if self._done:
+            return py_trees.common.Status.SUCCESS
+
+        try:
+            req = CreateFrameRequest(
+                parent_frame=self.parent_frame,
+                child_frame=self.child_frame,
+                x=self.x,
+                y=self.y,
+                yaw=self.yaw,
+            )
+            res = self.create_frame_srv(req)
+            if res.success:
+                rospy.loginfo(
+                    f"[{self.name}] Frame '{self.child_frame}' created relative to '{self.parent_frame}'"
+                )
+                self._done = True
+                return py_trees.common.Status.SUCCESS
+            else:
+                rospy.logerr(f"[{self.name}] Failed to create frame: {res.message}")
+                return py_trees.common.Status.FAILURE
+
+        except (
+            tf2_ros.LookupException,
+            tf2_ros.ConnectivityException,
+            tf2_ros.ExtrapolationException,
+        ) as e:
+            rospy.logwarn(f"[{self.name}] TF error: {e}")
+            return py_trees.common.Status.FAILURE
+
+        except rospy.ServiceException as e:
+            rospy.logerr(f"[{self.name}] Service error: {e}")
+            return py_trees.common.Status.FAILURE


Missing imports for CreateFrame and CreateFrameRequest. The CreateFrameBehavior class uses CreateFrame service type and CreateFrameRequest on lines 915 and 929 respectively, but these are not imported at the top of the file. This will cause a NameError at runtime when this behavior is instantiated.

Copilot · 2026-02-02T23:49:42Z

auv_smach/src/auv_smach/behaviors/gate_tree.py

+    RotateBehavior,
+    SetFrameLookingAtBehavior,
+    AlignFrameBehavior,
+    CreateFrameAtCurrentPositionBehavior,


Import of 'RotateBehavior' is not used.
Import of 'SetFrameLookingAtBehavior' is not used.
Import of 'CreateFrameAtCurrentPositionBehavior' is not used.

Suggested change

RotateBehavior,

SetFrameLookingAtBehavior,

AlignFrameBehavior,

CreateFrameAtCurrentPositionBehavior,

AlignFrameBehavior,

Copilot · 2026-02-02T23:49:42Z

auv_smach/src/auv_smach/behaviors/roll.py

+import py_trees
+import rospy
+import math
+import tf2_ros


Import of 'tf2_ros' is not used.

Suggested change

import tf2_ros

Copilot · 2026-02-02T23:49:42Z

auv_smach/src/auv_smach/behaviors/roll.py

+        RotateBehavior(
+            name="YawFirst360",
+            source_frame="taluy/base_link",
+            look_at_frame=yaw_frame,
+            rotation_speed=0.5,
+            rotation_radian=2 * math.pi,
+            timeout=45.0,
+            full_rotation=True,
+        )


Keyword argument 'full_rotation' is not a supported parameter name of RotateBehavior.init.

Copilot · 2026-02-02T23:49:43Z

auv_smach/src/auv_smach/behaviors/roll.py

+        RotateBehavior(
+            name="YawSecond360",
+            source_frame="taluy/base_link",
+            look_at_frame=yaw_frame,
+            rotation_speed=0.5,
+            rotation_radian=2 * math.pi,
+            timeout=45.0,
+            full_rotation=True,
+        )


Keyword argument 'full_rotation' is not a supported parameter name of RotateBehavior.init.

Copilot · 2026-02-02T23:49:43Z

auv_smach/src/auv_smach/behaviors/roll.py

+        except Exception:
+            pass


'except' clause does nothing but pass and there is no explanatory comment.

Suggested change

except Exception:

pass

except Exception as e:

rospy.logwarn_throttle(

5.0,

f"[{self.name}] Failed to convert odometry orientation to pitch: {e}",

)

OmerFarukGuveloglu and others added 8 commits February 2, 2026 13:08

Update UKF config and launch files (Known Issue: UKF outputs NaNs aft…

6455df4

…er runtime)

Revert "Update UKF config and launch files (Known Issue: UKF outputs …

ccb32c6

…NaNs after runtime)" This reverts commit ef06274.

slalom

cf66348

feat: port Gate task to Behavior Tree

29f1691

- Reimplement Gate logic using BT wrapper - Implement Coin Flip, Roll, and Yaw maneuvers - Refactor common behaviors - Update dependencies

setup functions parameter update

eedba58

precommit problem solved

32eb143

config

b557190

style: format slalom.yaml

20a9412

OmerFarukGuveloglu requested review from Copilot and eminmeydanoglu February 2, 2026 23:44

Copilot started reviewing on behalf of OmerFarukGuveloglu February 2, 2026 23:45 View session

Copilot AI reviewed Feb 2, 2026

View reviewed changes

OmerFarukGuveloglu added 3 commits February 3, 2026 13:27

feat: update gate behaviors

09e3082

feat: Add acoustic behavior tree implementation

4241171

feat: Add acoustic behavior tree implementation with SMACH wrapper

aa29a8b

OmerFarukGuveloglu force-pushed the guveloglu/smach_to_BT_acoustic branch from 51b422e to aa29a8b Compare February 3, 2026 13:28

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

Guveloglu/smach to bt acoustic#539

Guveloglu/smach to bt acoustic#539
OmerFarukGuveloglu wants to merge 11 commits intomainfrom
guveloglu/smach_to_BT_acoustic

OmerFarukGuveloglu commented Feb 2, 2026

Uh oh!

Copilot AI left a comment

Uh oh!

Copilot AI Feb 2, 2026

Uh oh!

Copilot AI Feb 2, 2026

Uh oh!

Copilot AI Feb 2, 2026

Uh oh!

Copilot AI Feb 2, 2026

Uh oh!

Copilot AI Feb 2, 2026

Uh oh!

Copilot AI Feb 2, 2026

Uh oh!

Copilot AI Feb 2, 2026

Uh oh!

Copilot AI Feb 2, 2026

Uh oh!

Copilot AI Feb 2, 2026

Uh oh!

Copilot AI Feb 2, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

	f"[{self.name}] CRITICAL: Thread did not finish ignoring cancel!"
	f"[{self.name}] CRITICAL: Thread did not finish, ignoring cancel!"

Uh oh!

Conversation

OmerFarukGuveloglu commented Feb 2, 2026

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Copilot AI Feb 2, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 2, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 2, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 2, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 2, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 2, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 2, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 2, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 2, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Feb 2, 2026

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants