mission_behavior_tree.py doesn't start the behavior reliably

When running mission_behavior_tree.py, I run into the issue that the actual behavior won't reliably start. In order to fix this, I modified the script like this

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from std_msgs.msg import String
import time

class StartBehaviorTree(Node):
    """Drone interface base node"""
    def __init__(self, namespace):
        super().__init__('start_bt', namespace=namespace)

        # Create a reliable QoS profile
        qos_profile = QoSProfile(
            reliability=ReliabilityPolicy.RELIABLE,
            history=HistoryPolicy.KEEP_ALL,
            depth=10
        )

        self.start_pub = self.create_publisher(
            String, "start", qos_profile)

        # Wait for publisher to be ready
        while self.start_pub.get_subscription_count() == 0:
            self.get_logger().info('Waiting for subscribers...')
            time.sleep(0.1)

        # Publish message
        msg = String()
        msg.data = "Start behavior tree"
        self.start_pub.publish(msg)
        self.get_logger().info('Published start message')

        # Ensure message is sent before shutting down
        time.sleep(0.5)

        # Initiate shutdown
        self.get_logger().info('Initiating node shutdown')
        self.destroy_node()

if __name__ == "__main__":
    rclpy.init()
    start_bt = StartBehaviorTree(namespace="drone0")
    rclpy.spin_once(start_bt)
    rclpy.shutdown()

This makes the behavior launch every time. The one downside is that this node won't shutdown properly.

It seems that the publisher's message to /drone0/start in the original script isn't broadcasting properly. This might be due to the node shutting down too fast, or because of a lack of connection to subscribers. Whenever I publish manually using the ROS2 CLI, the behavior always launches, so I'm assuming there is some kind of timing issue going on.

aerostack2 / project_gazebo

mission_behavior_tree.py doesn't start the behavior reliably #10