基于 PX4 与 ROS 2 的多机分布式编队：Gazebo 仿真搭建与通信拓扑测试

[PX4 SITL #1] <--> [Micro XRCE-DDS #1] <--> [ROS 2 Node (Agent 1)]
                                                    ^
                                                    | (Ad-hoc P2P / MAVLink)
                                                    v
[PX4 SITL #2] <--> [Micro XRCE-DDS #2] <--> [ROS 2 Node (Agent 2)]

#!/bin/bash
# 启动 3 架 x500 仿真多旋翼

NUM_DRONES=3
for i in $(seq 1 $NUM_DRONES); do
    export PX4_SYS_ID=$i
    export PX4_SIM_MODEL="x500"
    
    # 设置初始摆放坐标，防止重叠炸机
    X_POS=$(( (i - 1) * 2 ))
    Y_POS=0
    
    echo "Starting Drone $i at X: $X_POS, Y: $Y_POS"
    
    # 独立实例端口与启动
    gnome-terminal --tab --title="Drone $i" -- bash -c \
    "px4-autopilot/build/px4_sitl_default/bin/px4 -i $i \
     -d px4-autopilot/ROMFS/px4fmu_common \
     -w sitl_instance_$i \
     -s etc/init.d-posix/rcS \
     -t $X_POS,$Y_POS,0,0,0,0"
done

import rclpy
from rclpy.node import Node
from px4_msgs.msg import TrajectorySetpoint, VehicleLocalPosition
import numpy as np

class DistributedFormationAgent(Node):
    def __init__(self, agent_id, neighbors):
        super().__init__(f'formation_agent_{agent_id}')
        self.agent_id = agent_id
        self.neighbors = neighbors  # 邻居 ID 列表，例如 [2] 或 [1, 3]
        
        self.pos = np.zeros(3)
        self.vel = np.zeros(3)
        self.neighbor_pos = {nid: np.zeros(3) for nid in neighbors}
        self.neighbor_vel = {nid: np.zeros(3) for nid in neighbors}

        # 编队相对几何偏移 (NED 坐标系: [North, East, Down])
        self.offsets = {
            1: np.array([0.0, 0.0, -5.0]),
            2: np.array([-2.0, 2.0, -5.0]),
            3: np.array([-2.0, -2.0, -5.0]),
        }

        # 订阅自身位置 (Micro XRCE-DDS 转换后的 Topic)
        self.create_subscription(
            VehicleLocalPosition,
            f'/px4_{agent_id}/fmu/out/vehicle_local_position',
            self.self_pos_callback,
            10
        )

        # 订阅邻居位置广播
        for nid in neighbors:
            self.create_subscription(
                VehicleLocalPosition,
                f'/px4_{nid}/fmu/out/vehicle_local_position',
                lambda msg, nid=nid: self.neighbor_callback(msg, nid),
                10
            )

        # 发布自身期望速度/位置指令
        self.setpoint_pub = self.create_publisher(
            TrajectorySetpoint,
            f'/px4_{agent_id}/fmu/in/trajectory_setpoint',
            10
        )
        self.timer = self.create_timer(0.05, self.control_loop)  # 20 Hz 控制率

    def self_pos_callback(self, msg):
        self.pos = np.array([msg.x, msg.y, msg.z])
        self.vel = np.array([msg.vx, msg.vy, msg.vz])

    def neighbor_callback(self, msg, nid):
        self.neighbor_pos[nid] = np.array([msg.x, msg.y, msg.z])
        self.neighbor_vel[nid] = np.array([msg.vx, msg.vy, msg.vz])

    def control_loop(self):
        kp = 0.8
        kv = 0.4
        vel_cmd = np.zeros(3)

        # 计算拉普拉斯图一致性控制量
        for nid in self.neighbors:
            target_offset = self.offsets[self.agent_id] - self.offsets[nid]
            pos_error = (self.pos - self.neighbor_pos[nid]) - target_offset
            vel_error = self.vel - self.neighbor_vel[nid]
            
            # 分布式二阶一致性核心公式
            vel_cmd -= kp * pos_error + kv * vel_error

        # 限速滤波与安全饱和处理 (水平最大 3 m/s, 垂直最大 1.5 m/s)
        vel_cmd[0:2] = np.clip(vel_cmd[0:2], -3.0, 3.0)
        vel_cmd[2] = np.clip(vel_cmd[2], -1.5, 1.5)

        # 发布 Offboard 速度控制
        msg = TrajectorySetpoint()
        msg.velocity = [float(vel_cmd[0]), float(vel_cmd[1]), float(vel_cmd[2])]
        msg.yaw = 0.0  # 锁定正北或朝向速度方向
        self.setpoint_pub.publish(msg)