Guveloglu/smach to bt gate by OmerFarukGuveloglu · Pull Request #529 · itu-auv/auv-software

OmerFarukGuveloglu · 2026-01-24T09:16:12Z

No description provided.

Copilot

Pull request overview

This pull request migrates the gate navigation task from a SMACH-based state machine implementation to a py_trees-based behavior tree architecture. The refactoring introduces a new behaviors module with reusable behavior tree components while maintaining the same high-level gate navigation logic.

Changes:

Replaced SMACH state machine in gate.py with a behavior tree wrapper that delegates to a new modular behavior tree implementation
Created a new behaviors package with actions, conditions, subtrees, and task-specific behavior trees
Added py_trees and py_trees_ros dependencies to the package

Reviewed changes

Copilot reviewed 10 out of 11 changed files in this pull request and generated 24 comments.

Show a summary per file

File	Description
`auv_smach/src/auv_smach/gate.py`	Simplified to wrapper that creates and executes behavior tree instead of defining SMACH states
`auv_smach/src/auv_smach/behaviors/__init__.py`	New module exposing behavior tree components
`auv_smach/src/auv_smach/behaviors/actions.py`	Core behavior implementations (depth control, alignment, path execution, etc.)
`auv_smach/src/auv_smach/behaviors/conditions.py`	Condition behaviors for checking TF availability
`auv_smach/src/auv_smach/behaviors/subtrees.py`	Reusable composite behaviors (search, look around, dynamic path)
`auv_smach/src/auv_smach/behaviors/roll.py`	Roll and yaw maneuver behaviors with pitch correction
`auv_smach/src/auv_smach/behaviors/coin_flip.py`	Coin flip maneuver subtree
`auv_smach/src/auv_smach/behaviors/gate_tree.py`	Main gate task tree construction logic
`auv_smach/package.xml`	Added py_trees dependencies (with one duplicate entry)
`.gitignore`	Added exclusion for personal learning notes
`auv_navigation/auv_mapping/config/slalom.yaml`	Added YAML document separator

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

Copilot · 2026-01-24T09:21:50Z

auv_smach/src/auv_smach/behaviors/actions.py

+                # Start publishing enable signal (background thread)
+                self._stop_publishing.clear()
+                self._pub_thread = threading.Thread(target=self._publish_enable_loop)
+                self._pub_thread.start()


Potential race condition: The _stop_publishing Event is cleared on line 113 even though it was already cleared in initialise() on line 85. If the behavior is re-initialized while a thread is still running (didn't finish within the 1-second join timeout in terminate), this could lead to undefined behavior. Additionally, if a thread is orphaned and still running when update() is called again, a new thread will be created while the old one is still publishing, causing double-publishing. Consider checking if _pub_thread is still alive before starting a new one, or ensure proper cleanup.

Copilot · 2026-01-24T09:21:50Z

auv_smach/package.xml

  <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>


The py_trees dependency is listed twice in the exec_depend section (lines 58 and 59). Remove one of the duplicate entries.

Suggested change

<exec_depend>py_trees</exec_depend>

Copilot · 2026-01-24T09:21:51Z

auv_smach/src/auv_smach/behaviors/roll.py

+    def setup(self, timeout=15, **kwargs):
+        """Setup ROS connections."""
+        try:
+            self.sub_odom = rospy.Subscriber(
+                self.odometry_topic, Odometry, self._odom_callback
+            )
+            self.sub_kill = rospy.Subscriber(
+                self.killswitch_topic, Bool, self._killswitch_callback
+            )
+            self.pub_wrench = rospy.Publisher(
+                self.wrench_topic, WrenchStamped, queue_size=1
+            )
+            return True
+        except Exception as e:
+            rospy.logerr(f"[{self.name}] Setup error: {str(e)}")
+            return False


The subscribers created in the setup() method (lines 54-58) are never unregistered. In behavior trees, behaviors can be setup once and executed multiple times. If the behavior is used in a tree that runs repeatedly, these subscribers will persist and continue receiving callbacks even when the behavior is not active. Consider adding a shutdown() method or cleaning up subscribers in the terminate() method to prevent resource leaks and unwanted callbacks.

Copilot · 2026-01-24T09:21:51Z

auv_smach/src/auv_smach/behaviors/actions.py

+        except Exception as e:
+            rospy.logerr(f"[{self.name}] Setup error: {e}")
+            return False
+


The subscribers created in the setup() method (lines 292-300) for odometry and killswitch are never unregistered. These should be cleaned up when the behavior is no longer needed to prevent resource leaks and unwanted callbacks.

Suggested change

def terminate(self, new_status):

"""

Cleanup subscribers when the behavior is terminated.

This prevents resource leaks and unwanted callbacks after the

behavior is no longer active.

"""

# Unregister odometry subscriber if it was created

if hasattr(self, "odom_sub") and self.odom_sub is not None:

try:

self.odom_sub.unregister()

except Exception as e:

rospy.logwarn(f"[{self.name}] Error unregistering odom_sub: {e}")

finally:

self.odom_sub = None

# Unregister killswitch subscriber if it was created

if hasattr(self, "killswitch_sub") and self.killswitch_sub is not None:

try:

self.killswitch_sub.unregister()

except Exception as e:

rospy.logwarn(f"[{self.name}] Error unregistering killswitch_sub: {e}")

finally:

self.killswitch_sub = None

Copilot · 2026-01-24T09:21:51Z

auv_smach/src/auv_smach/gate.py

+        self.tree = None
+        self.behaviour_tree = None


The instance variables self.tree (line 32) and self.behaviour_tree (line 33) are initialized to None in init but self.tree is never used. The local variable root is created on line 39 and used throughout the execute method. Either remove the unused self.tree instance variable or assign root to it if it's intended for future use or debugging.

Copilot · 2026-01-24T09:21:54Z

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-01-24T09:21:54Z

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-01-24T09:21:54Z

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-01-24T09:21:55Z

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-01-24T09:21:55Z

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(f"[{self.name}] Failed to compute pitch from odometry: {e}")

…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

* test fixes * pre

* bag filename arg * x

* kaydet.py :D * chmod * Update Realsense camera image topic mapping

* add debug feature for frames in gazebo * rename topics to better names * delete vim swp file * Set z position of pose to 0.0 --------- Co-authored-by: Melih Okur <melihokur.okur@gmail.com>

OmerFarukGuveloglu requested review from Copilot and eminmeydanoglu January 24, 2026 09:16

Copilot started reviewing on behalf of OmerFarukGuveloglu January 24, 2026 09:16 View session

Copilot AI reviewed Jan 24, 2026

View reviewed changes

OmerFarukGuveloglu and others added 7 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

OmerFarukGuveloglu force-pushed the guveloglu/smach_to_BT_gate branch from 03fae9c to b557190 Compare February 2, 2026 13:10

OmerFarukGuveloglu and others added 10 commits February 2, 2026 13:19

style: format slalom.yaml

20a9412

feat: update gate behaviors

09e3082

Forgotten Config Changes for urdf, cameras etc. (#538)

3afba07

* test fixes * pre

Bag filename arg (#536)

02a064e

* bag filename arg * x

Image saver (#537)

c0aabd3

* kaydet.py :D * chmod * Update Realsense camera image topic mapping

(feat): add frame debugger for gazebo (#534)

588a89a

* add debug feature for frames in gazebo * rename topics to better names * delete vim swp file * Set z position of pose to 0.0 --------- Co-authored-by: Melih Okur <melihokur.okur@gmail.com>

fix: Gate BT service paths and alignment behavior

a381f87

precommit

8f36157

Gate tree and roll behavior updates

174e8a4

roll behavior update

62a8ef2

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

Guveloglu/smach to bt gate#529

Guveloglu/smach to bt gate#529
OmerFarukGuveloglu wants to merge 17 commits intomainfrom
guveloglu/smach_to_BT_gate

OmerFarukGuveloglu commented Jan 24, 2026

Uh oh!

Copilot AI left a comment

Uh oh!

Copilot AI Jan 24, 2026

Uh oh!

Copilot AI Jan 24, 2026

Uh oh!

Copilot AI Jan 24, 2026

Uh oh!

Copilot AI Jan 24, 2026

Uh oh!

Copilot AI Jan 24, 2026

Uh oh!

Copilot AI Jan 24, 2026

Uh oh!

Copilot AI Jan 24, 2026

Uh oh!

Copilot AI Jan 24, 2026

Uh oh!

Copilot AI Jan 24, 2026

Uh oh!

Copilot AI Jan 24, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

6 participants

+    def terminate(self, new_status):
+        """
+        Cleanup subscribers when the behavior is terminated.
+        This prevents resource leaks and unwanted callbacks after the
+        behavior is no longer active.
+        """
+        # Unregister odometry subscriber if it was created
+        if hasattr(self, "odom_sub") and self.odom_sub is not None:
+            try:
+                self.odom_sub.unregister()
+            except Exception as e:
+                rospy.logwarn(f"[{self.name}] Error unregistering odom_sub: {e}")
+            finally:
+                self.odom_sub = None
+        # Unregister killswitch subscriber if it was created
+        if hasattr(self, "killswitch_sub") and self.killswitch_sub is not None:
+            try:
+                self.killswitch_sub.unregister()
+            except Exception as e:
+                rospy.logwarn(f"[{self.name}] Error unregistering killswitch_sub: {e}")
+            finally:
+                self.killswitch_sub = None

Uh oh!

Conversation

OmerFarukGuveloglu commented Jan 24, 2026

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Copilot AI Jan 24, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Jan 24, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Jan 24, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Jan 24, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Jan 24, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Jan 24, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Jan 24, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Jan 24, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Jan 24, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Jan 24, 2026

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

6 participants