From 12c55f48f2c99080c1ffabe308eb94e72f516e37 Mon Sep 17 00:00:00 2001 From: pariaspe Date: Tue, 20 Jun 2023 10:14:39 +0200 Subject: [PATCH] More robust mission behavior tree script --- mission_behavior_tree.py | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/mission_behavior_tree.py b/mission_behavior_tree.py index df07998..c864bcd 100644 --- a/mission_behavior_tree.py +++ b/mission_behavior_tree.py @@ -6,6 +6,7 @@ from rclpy.node import Node from rclpy.qos import qos_profile_system_default from std_msgs.msg import String +from as2_msgs.msg import PlatformInfo, PlatformStatus class StartBehaviorTree(Node): @@ -14,14 +15,21 @@ class StartBehaviorTree(Node): def __init__(self, namespace): super().__init__('start_bt', namespace=namespace) + self.stop = False + + self.status_sub = self.create_subscription( + PlatformInfo, "platform/info", self.status_cbk, qos_profile_system_default) self.start_pub = self.create_publisher( String, "start", qos_profile_system_default) + def status_cbk(self, msg: PlatformInfo): + self.stop = True if msg.status.state == PlatformStatus.TAKING_OFF else False self.start_pub.publish(String()) if __name__ == "__main__": rclpy.init() start_bt = StartBehaviorTree(namespace="drone0") - rclpy.spin_once(start_bt, timeout_sec=1) + while not start_bt.stop: + rclpy.spin_once(start_bt, timeout_sec=1) rclpy.shutdown()