编队飞行控制 leader.py

@@@先选择无人机的数量以确定相应的队形。

XTDrone平台只支持 6/9/18架无人机吗？其他数量的无人机编队怎么实现？

@@@创建leader类

首先初始化及创建了一些对象+传建了一些订阅者和发布者对象。

它初始化了该对象的各种属性，包括无人机的类型、领航者ID、无人机数量、位置和速度信息、避障速度、编队信息、通信拓扑、目标高度记录、命令信息、频率和控制参数等。此外，它还订阅了一些ROS主题，以接收来自其他节点的消息，并发布消息到其他节点上。这些消息包括位置和速度信息、避障速度、编队信息、通信拓扑和命令信息等。这个构造函数的主要功能是设置无人机对象的初始状态和配置ROS节点的订阅者和发布者。

@@@定义各回调函数

1。def cmd_callback(self, msg):

实现了一个无人机对象的编队控制逻辑。它使用KM算法（Kuhn-Munkres算法）对当前编队和目标编队之间的无人机匹配问题进行求解，并生成一个新的编队。

2.def build_graph(self, orig_formation, change_formation):

实现了一个方法build_graph，用于根据原始编队和目标编队之间的距离计算无人机之间的通信关系，并返回一个邻接矩阵。

3.def find_path(self, i):

该方法的作用是在当前增广路图中寻找一条增广路，并将无人机之间的匹配关系存储在对象的match_right属性中。    没弄明白是什么意思。。。。。这里应该是KM算法的实现原理 。

def KM(self):该方法的作用是使用KM算法求解一个最大权匹配问题，并返回一个代表匹配结果的NumPy数组。

def get_new_formation(self, changed_id, change_formation):该方法的作用是根据KM算法的匹配结果生成一个新的编队，并返回一个代表新编队的NumPy数组。

def get_communication_topology(self, rel_posi):该方法的作用是根据新编队计算无人机之间的通信拓扑，并返回一个代表通信拓扑的NumPy数组。

要在XTDrone平台中的`leader.py`程序中加入几何跟踪控制器的代码，您可以按照以下步骤进行：

1. 导入必要的ROS消息类型和Python库：

```python
from geometry_msgs.msg import PoseStamped, TwistStamped
from tf.transformations import quaternion_from_euler
import math
```

2. 在`class Leader`中增加一个`follow_trajectory`方法：

```python
def follow_trajectory(self, t):
    # 计算领航者无人机的目标位置和速度
    x_desired = math.sin(0.1 * t)
    y_desired = math.cos(0.1 * t)
    z_desired = 1.5
    vx_desired = 0.1 * math.cos(0.1 * t)
    vy_desired = -0.1 * math.sin(0.1 * t)
    vz_desired = 0.0

    # 将目标位置和速度转换为ROS消息
    pose_msg = PoseStamped()
    pose_msg.header.stamp = rospy.Time.now()
    pose_msg.header.frame_id = "map"
    pose_msg.pose.position.x = x_desired
    pose_msg.pose.position.y = y_desired
    pose_msg.pose.position.z = z_desired
    quat = quaternion_from_euler(0.0, 0.0, 0.0)
    pose_msg.pose.orientation.x = quat[0]
    pose_msg.pose.orientation.y = quat[1]
    pose_msg.pose.orientation.z = quat[2]
    pose_msg.pose.orientation.w = quat[3]

    vel_msg = TwistStamped()
    vel_msg.header.stamp = rospy.Time.now()
    vel_msg.header.frame_id = "map"
    vel_msg.twist.linear.x = vx_desired
    vel_msg.twist.linear.y = vy_desired
    vel_msg.twist.linear.z = vz_desired

    # 发布目标位置和速度到ROS话题中
    self.local_position_pub.publish(pose_msg)
    self.local_velocity_pub.publish(vel_msg)
```

这段代码的作用是计算领航者无人机的目标位置和速度，并将其转换为ROS消息。然后，将目标位置和速度分别发布到`/uavX/mavros/setpoint_position/local`和`/uavX/mavros/setpoint_velocity/cmd_vel_unstamped`话题中，以控制领航者无人机的运动轨迹。

需要注意的是，在实际应用中，您需要根据具体的控制策略，修改`follow_trajectory`方法中的目标位置和速度计算方式，以确保编队能够稳定运动。
 
3. 在`run`方法中调用`follow_trajectory`方法：

```python
def run(self):
    rate = rospy.Rate(10.0)
    t = 0
    while not rospy.is_shutdown():
        self.follow_trajectory(t)
        t += 1
        rate.sleep()
```

这段代码的作用是在主循环中调用`follow_trajectory`方法，并更新时间变量`t`。在本例中，我们假设领航者无人机需要沿着一条正弦曲线运动，因此我们将`t`作为正弦函数的参数。在实际应用中，您需要根据具体的控制策略，修改时间变量`t`的更新方式。

4. 启动ROS节点：

```bash
roslaunch xtdrone leader.launch
```

运行以上代码后，领航者无人机将沿着一条正弦曲线运动，并尝试控制从跟随者无人机接收到的消息，以保持编队的稳定性。您可以根据需要修改`follow_trajectory`方法中的目标位置和速度计算方式，以实现不同的控制策略。

 

要在这段代码中加入几何跟踪控制器的代码，您可以按照以下步骤进行：

1. 导入必要的ROS消息类型和Python库：

```python
from geometry_msgs.msg import PoseStamped, TwistStamped
from tf.transformations import quaternion_from_euler
import math
```

2. 在`while not rospy.is_shutdown():`循环中，修改领航者无人机的位置和速度：

```python
# 计算领航者无人机的目标位置和速度
x_desired = math.sin(0.1 * t)
y_desired = math.cos(0.1 * t)
z_desired = 1.5
vx_desired = 0.1 * math.cos(0.1 * t)
vy_desired = -0.1 * math.sin(0.1 * t)
vz_desired = 0.0

# 将目标位置和速度转换为ROS消息
pose_msg = PoseStamped()
pose_msg.header.stamp = rospy.Time.now()
pose_msg.header.frame_id = "map"
pose_msg.pose.position.x = x_desired
pose_msg.pose.position.y = y_desired
pose_msg.pose.position.z = z_desired
quat = quaternion_from_euler(0.0, 0.0, 0.0)
pose_msg.pose.orientation.x = quat[0]
pose_msg.pose.orientation.y = quat[1]
pose_msg.pose.orientation.z = quat[2]
pose_msg.pose.orientation.w = quat[3]

vel_msg = TwistStamped()
vel_msg.header.stamp = rospy.Time.now()
vel_msg.header.frame_id = "map"
vel_msg.twist.linear.x = vx_desired
vel_msg.twist.linear.y = vy_desired
vel_msg.twist.linear.z = vz_desired

# 发布目标位置和速度到ROS话题中
self.local_position_pub.publish(pose_msg)
self.local_velocity_pub.publish(vel_msg)

# 更新时间变量
t += 1
```

这段代码的作用是计算领航者无人机的目标位置和速度，并将其转换为ROS消息。然后，将目标位置和速度分别发布到`/uavX/mavros/setpoint_position/local`和`/uavX/mavros/setpoint_velocity/cmd_vel_unstamped`话题中，以控制领航者无人机的运动轨迹。在本例中，我们假设领航者无人机需要沿着一条正弦曲线运动，因此我们将`t`作为正弦函数的参数。在实际应用中，您需要根据具体的控制策略，修改时间变量`t`的更新方式。

3. 在`while not rospy.is_shutdown():`循环中，删除以下代码：

```python
self.cmd_vel_enu.linear.x = self.cmd_vel_enu.linear.x
self.cmd_vel_enu.linear.y = self.cmd_vel_enu.linear.y
self.cmd_vel_enu.linear.z = self.cmd_vel_enu.linear.z
```

这些代码似乎是无用的，因为它们没有实际改变`self.cmd_vel_enu`对象的值。

4. 在`while not rospy.is_shutdown():`循环中，注释掉以下代码：

```python
#self.vel_enu_pub.publish(self.cmd_vel_enu)
#self.pose_pub.publish(self.pose)
#self.cmd_pub.publish(self.cmd)
```

这些代码将无人机的速度、位置和指令发布到`/uavX/mavros/setpoint_velocity/cmd_vel`、`/uavX/mavros/local_position/pose`和`/uavX/mavros/cmd`话题中。在加入几何跟踪控制器之后，我们将直接发布目标位置和速度，因此不需要再发布这些信息。

5. 在`while not rospy.is_shutdown():`循环中，添加以下代码：

```python
# 发布编队形态信息到ROS话题中
formation_pattern = Int32MultiArray()
formation_pattern.data = self.formation.flatten().tolist()
self.formation_pattern_pub.publish(formation_pattern)

# 发布通信拓扑信息到ROS话题中
if not self.communication_topology is None:
    communication_topology = Int32MultiArray()
    communication_topology.data = self.communication_topology.flatten().tolist()
    self.communication_topology_pub.publish(communication_topology)
```

这段代码的作用是将编队形态信息和通信拓扑信息分别发布到`/formation_pattern`和`/communication_topology`话题中。这些信息可能会在其他节点中使用，以实现更复杂的编队控制策略。需要注意的是，在实际应用中，您需要根据需要修改这些信息的内容和格式，以确保它们能够被其他节点正确地解析和使用。

6. 在`while not rospy.is_shutdown():`循环中，添加以下代码：

```python
# 等待一段时间
rate.sleep()
```

这段代码的作用是让程序暂停一段时间，以控制程序的执行速度。在本例中，我们将程序的执行速度设置为10Hz，因此程序将每0.1秒执行一次主循环。在实际应用中，您需要根据需要修改程序的执行速度，以确保编队能够稳定运动。

最终修改后的`while not rospy.is_shutdown():`循环的代码如下所示：

```python
while not rospy.is_shutdown():
    # 计算领航者无人机的目标位置和速度
    x_desired = math.sin(0.1 * t)
    y_desired = math.cos(0.1 * t)
    z_desired = 1.5
    vx_desired = 0.1 * math.cos(0.1 * t)
    vy_desired = -0.1 * math.sin(0.1 * t)
    vz_desired = 0.0

    # 将目标位置和速度转换为ROS消息
    pose_msg = PoseStamped()
    pose_msg.header.stamp = rospy.Time.now()
    pose_msg.header.frame_id = "map"
    pose_msg.pose.position.x = x_desired
    pose_msg.pose.position.y = y_desired
    pose_msg.pose.position.z = z_desired
    quat = quaternion_from_euler(0.0, 0.0, 0.0)
    pose_msg.pose.orientation.x = quat[0]
    pose_msg.pose.orientation.y = quat[1]
    pose_msg.pose.orientation.z = quat[2]
    pose_msg.pose.orientation.w = quat[3]

    vel_msg = TwistStamped()
    vel_msg.header.stamp = rospy.Time.now()
    vel_msg.header.frame_id = "map"
    vel_msg.twist.linear.x = vx_desired
    vel_msg.twist.linear.y = vy_desired
    vel_msg.twist.linear.z = vz_desired

    # 发布目标位置和速度到ROS话题中
    self.local_position_pub.publish(pose_msg)
    self.local_velocity_pub.publish(vel_msg)

    # 发布编队形态信息到ROS话题中
    formation_pattern = Int32MultiArray()
    formation_pattern.data = self.formation.flatten().tolist()
    self.formation_pattern_pub.publish(formation_pattern)

    # 发布通信拓扑信息到ROS话题中
    if not self.communication_topology is None:
        communication_topology = Int32MultiArray()
        communication_topology.data = self.communication_topology.flatten().tolist()
        self.communication_topology_pub.publish(communication_topology)

    # 更新时间变量
    t += 1

    # 等待一段时间
    rate.sleep()
```

运行以上代码后，领航者无人机将沿着一条正弦曲线运动，并尝试控制从跟随者无人机接收到的消息，以保持编队的稳定性。您可以根据需要修改`while not rospy.is_shutdown():`循环中的代码，以实现不同的编队控制策略。

要在XTDrone仿真平台中实现多无人机编队控制，并加入几何跟踪控制器的代码，您可以按照以下步骤进行：

1. 导入必要的ROS消息类型和Python库：

```python
from geometry_msgs.msg import PoseStamped, TwistStamped
from nav_msgs.msg import Path
from std_msgs.msg import Int32MultiArray
from tf.transformations import quaternion_from_euler
import math
```

2. 在`class Follower`中增加一个`follow_leader`方法：

```python
def follow_leader(self, t):
    # 计算领航者无人机的目标位置和速度
    x_desired = math.sin(0.1 * t) + self.leader_position.x
    y_desired = math.cos(0.1 * t) + self.leader_position.y
    z_desired = self.leader_position.z
    vx_desired = 0.1 * math.cos(0.1 * t)
    vy_desired = -0.1 * math.sin(0.1 * t)
    vz_desired = 0.0

    # 将目标位置和速度转换为ROS消息
    pose_msg = PoseStamped()
    pose_msg.header.stamp = rospy.Time.now()
    pose_msg.header.frame_id = "map"
    pose_msg.pose.position.x = x_desired
    pose_msg.pose.position.y = y_desired
    pose_msg.pose.position.z = z_desired
    quat = quaternion_from_euler(0.0, 0.0, 0.0)
    pose_msg.pose.orientation.x = quat[0]
    pose_msg.pose.orientation.y = quat[1]
    pose_msg.pose.orientation.z = quat[2]
    pose_msg.pose.orientation.w = quat[3]

    vel_msg = TwistStamped()
    vel_msg.header.stamp = rospy.Time.now()
    vel_msg.header.frame_id = "map"
    vel_msg.twist.linear.x = vx_desired
    vel_msg.twist.linear.y = vy_desired
    vel_msg.twist.linear.z = vz_desired

    # 发布目标位置和速度到ROS话题中
    self.local_position_pub.publish(pose_msg)
    self.local_velocity_pub.publish(vel_msg)
```

这段代码的作用是计算跟随者无人机需要沿着的轨迹，并将其转换为ROS消息。其中，目标位置的计算方式是在当前时刻沿着领航者无人机的轨迹前进一定距离。具体来说，我们将领航者无人机沿着正弦曲线运动，跟随者无人机沿着相同的正弦曲线运动，但是在每个时刻，跟随者无人机的位置比领航者无人机的位置向前推进了一定距离。这样，跟随者无人机就可以保持在领航者无人机的后面，形成一个编队。

需要注意的是，在实际应用中，您需要根据具体的控制策略，修改`follow_leader`方法中的目标位置和速度计算方式，以确保编队能够稳定运动。
 
3. 在`class Leader`中增加一个`follow_trajectory`方法：

```python
def follow_trajectory(self, t):
    # 计算领航者无人机的目标位置和速度
    x_desired = math.sin(0.1 * t)
    y_desired = math.cos(0.1 * t)
    z_desired = 1.5
    vx_desired = 0.1 * math.cos(0.1 * t)
    vy_desired = -0.1 * math.sin(0.1 * t)
    vz_desired = 0.0

    # 将目标位置和速度转换为ROS消息
    pose_msg = PoseStamped()
    pose_msg.header.stamp = rospy.Time.now()
    pose_msg.header.frame_id = "map"
    pose_msg.pose.position.x = x_desired
    pose_msg.pose.position.y = y_desired
    pose_msg.pose.position.z = z_desired
    quat = quaternion_from_euler(0.0, 0.0, 0.0)
    pose_msg.pose.orientation.x = quat[0]
    pose_msg.pose.orientation.y = quat[1]
    pose_msg.pose.orientation.z = quat[2]
    pose_msg.pose.orientation.w = quat[3]

    vel_msg = TwistStamped()
    vel_msg.header.stamp = rospy.Time.now()
    vel_msg.header.frame_id = "map"
    vel_msg.twist.linear.x = vx_desired
    vel_msg.twist.linear.y = vy_desired
    vel_msg.twist.linear.z = vz_desired

    # 发布目标位置和速度到ROS话题中
    self.local_position_pub.publish(pose_msg)
    self.local_velocity_pub.publish(vel_msg)
```

这段代码的作用是计算领航者无人机的目标位置和速度，并将其转换为ROS消息。然后，将目标位置和速度分别发布到`/uavX/mavros/setpoint_position/local`和`/uavX/mavros/setpoint_velocity/cmd_vel_unstamped`话题中，以控制领航者无人机的运动轨迹。

需要注意的是，在实际应用中，您需要根据具体的控制策略，修改`follow_trajectory`方法中的目标位置和速度计算方式，以确保编队能够稳定运动。
 
4. 在`class FormationController`中增加一个`update`方法：

```python
def update(self):
    # 计算编队形态误差
    formation_error = self.formation - self.current_formation
    formation_error_norm = np.linalg.norm(formation_error)

    # 计算通信拓扑误差
    if not self.communication_topology is None:
        topology_error = self.communication_topology - self.current_topology
        topology_error_norm = np.linalg.norm(topology_error)
    else:
        topology_error_norm = 0.0

    # 计算几何跟踪控制指令
    if formation_error_norm > self.threshold:
        # 如果编队形态误差较大，则执行几何跟踪控制
        self.follow_path()
    else:
        # 如果编队形态误差较小，则执行通信拓扑控制
        if topology_error_norm > self.threshold:
            self.adjust_topology()

    # 发布编队形态信息到ROS话题中
    formation_pattern = Int32MultiArray()
    formation_pattern.data = self.formation.flatten().tolist()
    self.formation_pattern_pub.publish(formation_pattern)

    # 发布通信拓扑信息到ROS话题中
    if not self.communication_topology is None:
        communication_topology = Int32MultiArray()
        communication_topology.data = self.communication_topology.flatten().tolist()
        self.communication_topology_pub.publish(communication_topology)
```

这段代码的作用是根据当前的编队形态误差和通信拓扑误差，选择执行几何跟踪控制还是通信拓扑控制。如果编队形态误差较大，则执行几何跟踪控制；如果编队形态误差较小，则执行通信拓扑控制。需要注意的是，在实际应用中，您需要根据具体的控制策略，修改阈值参数和控制方法。

5. 在`class FormationController`中增加一个`follow_path`方法：

```python
def follow_path(self):
    # 计算路径点
    t = 0
    path = []
    for i in range(self.num_uavs):
        x = math.sin(0.1 * t + 2 * math.pi * i / self.num_uavs) + self.formation[i][0]
        y = math.cos(0.1 * t + 2 * math.pi * i / self.num_uavs) + self.formation[i][1]
        z = self.formation[i][2]
        path.append([x, y, z])
    path_msg = Path()
    path_msg.header.stamp = rospy.Time.now()
    path_msg.header.frame_id = "map"
    for p in path:
        pose_msg = PoseStamped()
        pose_msg.header.stamp = rospy.Time.now()
        pose_msg.header.frame_id = "map"
        pose_msg.pose.position.x = p[0]
        pose_msg.pose.position.y = p[1]
        pose_msg.pose.position.z = p[2]
        quat = quaternion_from_euler(0.0, 0.0, 0.0)
        pose_msg.pose.orientation.x =