Gazebo多无人机编队仿真

上篇文章中我们主要讲到了怎么利用Matlab根据仿真多无人机编队的情况，最终的效果通过Matlab的可视化工具展示了出来，这篇文章，我们就来介绍怎么将Matlab的多无人机编队的效果通过Gazebo的3d物理仿真软件展示出来，这里我们主要利用了rotor_simulition的无人机仿真工具。

多无人机仿真环境搭建

在多无人机仿真环境搭建中我们主要利用了rotors_simulation的无人机仿真平台，下面将介绍一下如何搭建rotors_simulation无人机仿真平台
本项目的主要实现环境是在ubuntu 18.04、ROS melodic版本下实现的
1 . 下载安装好ROS相关功能包、wstool、catkin-tools
2 . 创建工作文件夹，下载rotor_simulations

mkdir -p ~/catkin_ws/src #创建文件夹
catkin_init_workspace  #初始化工作空间
wstool init #工具初始化
wget 
https://raw.githubusercontent.com/ethz-asl/rotors_simulator/master/rotors_hil.rosinstall #下载rotor_simulation工具
wstool merge rotors_hil.rosinstall
wstool update

3 . 编译rotors_simulation功能包

cd ~/catkin_ws/
catkin build

Gazebo多无人机编队实现

多无人机的仿真实现与多机器人类似，具体的原理操作大家可以参考古月居中的多机器人的配置。在本项目中，我们通过编写launch文件，调用rotor_simulation中的文件，以及实现多无人机Gazebo仿真，具体的代码可以参考：
multi_uav_simulation.launch

 <!-- 参数声明-->
 <arg name="mav_name" default="firefly"/>
  <arg name="world_name" default="cloister"/>
  <arg name="enable_logging" default="false" />
  <arg name="enable_ground_truth" default="true" />
  <arg name="log_file" default="$(arg mav_name)" />
  <arg name="debug" default="false"/>
  <arg name="gui" default="true"/>
  <arg name="paused" default="true"/>
  <arg name="verbose" default="false"/>
  <env name="GAZEBO_MODEL_PATH" value="${GAZEBO_MODEL_PATH}:$(find rotors_gazebo)/models"/>
  <env name="GAZEBO_RESOURCE_PATH" value="${GAZEBO_RESOURCE_PATH}:$(find rotors_gazebo)/models"/>
    <!-- 启动Gazebo-->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find rotors_gazebo)/worlds/$(arg world_name).world" />
    <arg name="debug" value="$(arg debug)" />
    <arg name="paused" value="$(arg paused)" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="verbose" value="$(arg verbose)"/>
  </include>
      <!-- 多无人机模型UAV1-->
  <group ns="UAV1">
        <!--启用UAV的模型-->
    <include file="$(find rotors_gazebo)/launch/multi/multi_spawn_mav.launch">
      <arg name="namespace" default="UAV1"/>
      <arg name="mav_name" value="$(arg mav_name)" />
      <arg name="model" value="$(find rotors_description)/urdf/mav_generic_odometry_sensor.gazebo" />
      <arg name="enable_logging" value="$(arg enable_logging)" />
      <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
      <arg name="log_file" value="$(arg log_file)"/>
      <arg name="x" default="0.0"/>
      <arg name="y" default="4.0"/>
      <arg name="z" default="0.1"/>
    </include>
          <!-- 无人机位置式控制-->
    <node name="lee_position_controller_node" pkg="rotors_control" type="lee_position_controller_node" output="screen">
      <rosparam command="load" file="$(find rotors_gazebo)/resource/lee_controller_$(arg mav_name).yaml" />
      <rosparam command="load" file="$(find rotors_gazebo)/resource/$(arg mav_name).yaml" />
      <remap from="odometry" to="odometry_sensor1/odometry" />
    </node>
            <!-- 无人机控制器-->
    <node name="motion_controller" pkg="rotors_teleop" type="rotors_formation_son.py" output="screen">
      <param name = "UAV" type="string" value = "/UAV1/"/>
      <param name = "seq" type="int" value = "0"/>
    </node>
            <!-- 开启相关节点-->
    <node name="hovering_example" pkg="rotors_gazebo" type="hovering_example" output="screen"/>
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
  </group>
  <!-- 第二架无人机-->
  <group ns="UAV2">
    <include file="$(find rotors_gazebo)/launch/multi/multi_spawn_mav.launch">
      <arg name="namespace" default="UAV2"/>
      <arg name="mav_name" value="$(arg mav_name)" />
      <arg name="model" value="$(find rotors_description)/urdf/mav_generic_odometry_sensor.gazebo" />
      <arg name="enable_logging" value="$(arg enable_logging)" />
      <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
      <arg name="log_file" value="$(arg log_file)"/>
      <arg name="x" default="0.0"/>
      <arg name="y" default="2.4"/>
      <arg name="z" default="0.1"/>
    </include>
    <node name="lee_position_controller_node" pkg="rotors_control" type="lee_position_controller_node" output="screen">
      <rosparam command="load" file="$(find rotors_gazebo)/resource/lee_controller_$(arg mav_name).yaml" />
      <rosparam command="load" file="$(find rotors_gazebo)/resource/$(arg mav_name).yaml" />
      <remap from="odometry" to="odometry_sensor1/odometry" />
    </node>
    <node name="motion_controller" pkg="rotors_teleop" type="rotors_formation_son.py" output="screen">
      <param name = "UAV" type="string" value = "/UAV2/"/>
      <param name = "seq" type="int" value = "1"/>
    </node>
    <node name="hovering_example" pkg="rotors_gazebo" type="hovering_example" output="screen"/>
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
  </group>

以上是在rotor_simulation的环境下启用多无人机的launch文件，我们如果想改变无人机的数量的话，只需要更换namespace，增加对应的即可。
根据多无人机编队的需求，这里我们使用了六架无人机进行仿真模拟，其中展示效果如下所示：

那么我们怎么让这六架无人机按照我们想要的编队效果进行运动呢？这就需要我们写一个位置式的无人机控制器，通过读取Matlab仿真得到的无人机位置变化，分别对无人机进行控制，可以参考下面的python控制代码：
rotors_formation_son.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-
#导入相关的库
import rospy
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Pose
import sys, select, termios, tty
from numpy import *
import operator
from os import listdir
import time

#建立结构体
twist= Twist()
pose=PoseStamped()
#Create Publisher
#初始化ros节点
rospy.init_node('formation_publish')
r=rospy.Rate(1)
UAV=rospy.get_param('~UAV')
seq=rospy.get_param('~seq')
print(UAV+'command/pose')
print(seq)
num=seq
pose_pub = rospy.Publisher(UAV+'command/pose',PoseStamped,queue_size=1)
#初始化函数
def __init__():
        pose.pose.position.x=0
        pose.pose.position.y=0
        pose.pose.position.z=0
#读取Matlab中的数据        
def file2matrix(filename):
    fr = open(filename)
    numberOfLines = len(fr.readlines())         #get the number of lines in the file
    returnMat = zeros((numberOfLines,3))        #prepare matrix to return
    classLabelVector = []                       #prepare labels return   
    fr = open(filename)
    index = 0
    for line in fr.readlines():
        line = line.strip()
        listFromLine = line.split('\t')
        classLabelVector.append(int(listFromLine[0]))
        returnMat[index,:] = listFromLine[1:4]
        index += 1
    return returnMat,numberOfLines 
#计算无人机位置    
def odomCallback(msg):
    global num
    if msg.position.x>=pose.pose.position.x-0.1 and num<numberOfLines:
        num += 6
        time.sleep(0.01)
        pose.pose.position.x=dataMat[num][0]
        pose.pose.position.y=dataMat[num][1]*2
        pose.pose.position.z=dataMat[num][2]*0.5
        pose_pub.publish(pose)


__init__();
dataMat,numberOfLines = file2matrix("/home/zdz/source/data/pos_data.txt")
#Create subscriber
odom_sub = rospy.Subscriber('odometry_sensor1/pose',Pose, odomCallback,queue_size = 1)

# spin
rospy.spin()

上述代码主要实现了读取Matlab中无人机编队的位置，并实时发布无人机位置的指令“command/pose”，最终通过分别给六架无人机发布位置式的指令，控制无人机的运动，达到编队飞行的效果（其中pos_data.txt是通过将Matlab中运行后得到的数据vehicle_pos转存为txt格式得到）
最终运行起来的节点图如下所示：

多无人机编队仿真演示
我们将编写的launch文件放入rotor_gazebo文件夹中，编译之后使用运行指令：

roslaunch rotor_gazebo multi_uav_simulation.launch

即可启动多无人机Gazebo仿真程序，多无人机编队飞行的效果可以参考下面录制的视频，在Gazebo仿真结果我们可以看到，六架无人机可以比较好的保持队形进行平移运动与旋转运动，大家有兴趣的话也可以自己动手尝试。期间有遇到任何问题可以在古月居论坛的无人机版块与笔者一起探讨。