add viewer.py

ros_ws/behavior/behavior/base_behavior.py

@@ -4,7 +4,7 @@ from behavior_interface.msg import BehaviorStatus
 
 class BaseBehavior(Node):
     def __init__(self, name):
-        super().__init__('base_behavior')
+        super().__init__('base_behavior_' + name)  # 使用行为名称创建唯一节点
         self.name = name
         self.status = False
 
@@ -23,21 +23,49 @@ class BaseBehavior(Node):
         )
 
     def listener_callback(self, msg):
-        if msg.name == 'ping':  # 检查是否为 'ping' 消息
+        # 检查是否为 'ping' 消息
+        if msg.name == 'ping':
+            self.get_logger().info(f'{self.name} received ping, publishing status.')
             self.publish_status()
+        # 如果消息名称与当前行为名称匹配，更新状态并调用 on_status
         elif msg.name == self.name:
             self.status = msg.status
             self.on_status()
 
     def publish_status(self):
+        # 创建并发布 BehaviorStatus 消息
         status_msg = BehaviorStatus()
         status_msg.name = self.name
         status_msg.status = self.status
         self.status_publisher.publish(status_msg)
+        self.get_logger().info(f'{self.name} status published: {self.status}')
 
     def on_status(self):
+        # 子类可以重写的方法，默认输出状态变化信息
         self.get_logger().info(f'{self.name} status changed to {self.status}')
 
+
+# 创建虚假的行为节点并运行
+def create_fake_behaviors():
+    rclpy.init()
+    # 创建多个虚假行为节点
+    hover_behavior = BaseBehavior('Hover')
+    forward_behavior = BaseBehavior('Forward')
+
+    # 使用多线程执行器运行多个节点
+    executor = rclpy.executors.MultiThreadedExecutor()
+    executor.add_node(hover_behavior)
+    executor.add_node(forward_behavior)
+
+    try:
+        executor.spin()  # 让节点保持运行
+    finally:
+        hover_behavior.destroy_node()
+        forward_behavior.destroy_node()
+        rclpy.shutdown()
+
+
+# 主函数入口
 def main(args=None):
     rclpy.init(args=args)
     behavior_node = BaseBehavior('Hover')

ros_ws/behavior/setup.py

@@ -20,7 +20,9 @@ setup(
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
-            'base_behavior = behavior.base_behavior:main'
+            'base_behavior = behavior.base_behavior:main',
+            'fake_behaviors = behavior.base_behavior:create_fake_behaviors',
+            'status_viewer = behavior.status_viewer:main' 
         ],
     },
 )