add all the nodes

ros_ws/behavior/behavior/base_behavior.py

 import rclpy
-from rclpy.node import Node
 from behavior_interface.msg import BehaviorStatus
+from rclpy.node import Node
 
+# 基础行为类
 class BaseBehavior(Node):
     def __init__(self, name):
-        super().__init__('base_behavior_' + name)  # 使用行为名称创建唯一节点
+        super().__init__('base_behavior_' + name)
         self.name = name
         self.status = False
 
-        # subscribe 'behavior' topic
+        # 订阅 'behavior' 主题
         self.subscription = self.create_subscription(
             BehaviorStatus,
             'behavior',
             self.listener_callback,
             10
         )
-        # publish 'behaviors_status' topic
+
+        # 发布 'behaviors_status' 主题
         self.status_publisher = self.create_publisher(
             BehaviorStatus,
             'behaviors_status',
             10
         )
+        self.get_logger().info(f'{self.name} behavior node initialized')
 
     def listener_callback(self, msg):
-        # check if it's "ping"
         if msg.name == 'ping':
-            self.get_logger().info(f'{self.name} received ping, publishing status.')
             self.publish_status()
         elif msg.name == self.name:
             self.status = msg.status
@@ -36,32 +37,105 @@ class BaseBehavior(Node):
         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}')
 
+# 定义具体行为类
+class Hover(BaseBehavior):
+    def __init__(self):
+        super().__init__('Hover')
+
+    def on_status(self):
+        if self.status:
+            self.get_logger().info("Hover behavior activated")
+        else:
+            self.get_logger().info("Hover behavior deactivated")
+
+class MoveLeft(BaseBehavior):
+    def __init__(self):
+        super().__init__('MoveLeft')
+
+    def on_status(self):
+        if self.status:
+            self.get_logger().info("MoveLeft behavior activated")
+        else:
+            self.get_logger().info("MoveLeft behavior deactivated")
+
+class MoveUp(BaseBehavior):
+    def __init__(self):
+        super().__init__('MoveUp')
+
+    def on_status(self):
+        if self.status:
+            self.get_logger().info("MoveUp behavior activated")
+        else:
+            self.get_logger().info("MoveUp behavior deactivated")
+
+class MoveRight(BaseBehavior):
+    def __init__(self):
+        super().__init__('MoveRight')
 
-def create_fake_behaviors():
+    def on_status(self):
+        if self.status:
+            self.get_logger().info("MoveRight behavior activated")
+        else:
+            self.get_logger().info("MoveRight behavior deactivated")
+
+class Takeoff(BaseBehavior):
+    def __init__(self):
+        super().__init__('Takeoff')
+
+    def on_status(self):
+        if self.status:
+            self.get_logger().info("Takeoff behavior activated")
+        else:
+            self.get_logger().info("Takeoff behavior deactivated")
+
+class StopAndLand(BaseBehavior):
+    def __init__(self):
+        super().__init__('StopAndLand')
+
+    def on_status(self):
+        if self.status:
+            self.get_logger().info("StopAndLand behavior activated")
+        else:
+            self.get_logger().info("StopAndLand behavior deactivated")
+
+# 创建并运行所有行为节点
+def create_all_behaviors():
     rclpy.init()
-    hover_behavior = BaseBehavior('Hover')
-    forward_behavior = BaseBehavior('Forward')
 
+    # 创建所有行为实例
+    hover = Hover()
+    move_left = MoveLeft()
+    move_up = MoveUp()
+    move_right = MoveRight()
+    takeoff = Takeoff()
+    stop_and_land = StopAndLand()
+
+    # 使用多线程执行器
     executor = rclpy.executors.MultiThreadedExecutor()
-    executor.add_node(hover_behavior)
-    executor.add_node(forward_behavior)
+    executor.add_node(hover)
+    executor.add_node(move_left)
+    executor.add_node(move_up)
+    executor.add_node(move_right)
+    executor.add_node(takeoff)
+    executor.add_node(stop_and_land)
 
     try:
-        executor.spin()  
+        executor.spin()  # 让所有节点保持运行
     finally:
-        hover_behavior.destroy_node()
-        forward_behavior.destroy_node()
+        hover.destroy_node()
+        move_left.destroy_node()
+        move_up.destroy_node()
+        move_right.destroy_node()
+        takeoff.destroy_node()
+        stop_and_land.destroy_node()
         rclpy.shutdown()
 
-
 def main(args=None):
-    rclpy.init(args=args)
-    behavior_node = BaseBehavior('Hover')
-    rclpy.spin(behavior_node)
-    behavior_node.destroy_node()
-    rclpy.shutdown()
+    create_all_behaviors()
+
+if __name__ == '__main__':
+    main()

ros_ws/behavior/launch/behaviors_launch.py

+import os
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        # 启动 base_behavior 节点
+        Node(
+            package='behavior',
+            executable='base_behavior',
+            name='base_behavior_Hover',
+            output='screen'
+        ),
+        Node(
+            package='behavior',
+            executable='base_behavior',
+            name='base_behavior_MoveLeft',
+            output='screen'
+        ),
+        Node(
+            package='behavior',
+            executable='base_behavior',
+            name='base_behavior_MoveUp',
+            output='screen'
+        ),
+        Node(
+            package='behavior',
+            executable='base_behavior',
+            name='base_behavior_MoveRight',
+            output='screen'
+        ),
+        Node(
+            package='behavior',
+            executable='base_behavior',
+            name='base_behavior_Takeoff',
+            output='screen'
+        ),
+        Node(
+            package='behavior',
+            executable='base_behavior',
+            name='base_behavior_StopAndLand',
+            output='screen'
+        ),
+        # 启动 joy_node
+        Node(
+            package='joy',
+            executable='joy_node',
+            name='joy_node',
+            output='screen'
+        ),
+        # 启动 joy_teleop 节点
+        Node(
+            package='behavior',
+            executable='joy_teleop',
+            name='joy_teleop',
+            output='screen'
+        ),
+        # 启动 command_node
+        Node(
+            package='behavior',
+            executable='command_node',
+            name='command_node',
+            output='screen'
+        ),
+    ])

ros_ws/behavior/setup.py

@@ -10,6 +10,7 @@ setup(
         ('share/ament_index/resource_index/packages',
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
+        ('share/' + package_name + '/launch', ['launch/behaviors_launch.py']),
     ],
     install_requires=['setuptools'],
     zip_safe=True,