PX4无人机-Gazebo仿真实现移动物体的跟踪
原文链接PX4无人机-Gazebo仿真实现移动物体的跟踪末尾有演示视频
这个学期我们有一个智能机器人系统的课设，我们组分配到的题目是《仿真环境下使用无人机及相机跟踪移动物体》，本文主要记录完成该课设的步骤以及内容。我们采用的最终方案是PX4飞控+gazebo仿真+mavros通讯控制，实现了在gazebo环境下无人机跟踪一个移动的小车。本文所使用的是Ubuntu18.04 + melodic。
试验环境介绍
首先要搞懂各个部分的关系1，以及各自的作用，才能对控制无人机有个完整的认识，我在一开始做的时候就花了很多时间都没搞懂PX4到底是个无人机还是个什么东西，mavros又是干什么的。下面我简要介绍一下各个部分的关系，让大家有个大致的了解。
PX4飞控
PX4是一个飞控固件，所谓的飞控固件，就是能够向无人机发出控制命令，控制无人机的位姿、飞行速度以及螺旋桨的转速等等。无人机的运动就需要通过飞控固件发出命令来控制。官网的用户手册在这，推荐看英文版本，中文版本有的地方翻译的实在是太烂了，我看的时候感觉像是机翻的，而且与原文的位置都不太一样。
Gazebo仿真
gazebo仿真就不用多说了吧，在学ros基本的操作的时候就应该接触过gazebo。这就是一个能够模拟现实世界的仿真软件，PX4的源代码里就提供了PX4无人机的gazebo模型，通过launch文件直接运行就能得到一个gazebo下的无人机。
MAVROS通讯
mavros里面有个ros，一看就是和ros相关的。我们看官网的介绍MAVROS – MAVLink extendable communication node for ROS with proxy for Ground Control Station’，这句话的意思是，MAVROS是MAVLink为了让ROS代理控制站的扩展交流节点。首先MAVLink是一个无人机通讯协议，也就是说与无人机交流所发出的信号或数据格式都要符合该协议，与HTTP等协议是一个道理。然后控制站其实是PX4为了控制无人机所开发的一个图形化控制站，可以通过GUI的形式来操作无人机，给一般用户很好的体验。而这里是用来代理控制站，也就是说充当控制站来控制无人机。到这里就很明显了，MAVROS就相当于代码版的控制站，若想要通过ros节点开控制无人机的飞行，那就必须通过mavros这个包，在这个包内包含了控制无人机的消息格式等。
综上所述，整个无人机的控制逻辑就是，通过mavros向PX4飞控发送控制命令，PX4再将命令发送到无人机的各个组件，以控制无人机按照用户的逻辑进行运动。而该无人机就在gazebo中，在gazebo中可以看到无人机的运动情况。
实验过程
PX4无人机的安装
1、安装环境依赖
sudo apt install -y ninja-build exiftool python-argparse python-empy python-toml python-numpy python-yaml python-dev python-pip ninja-build protobuf-compiler libeigen3-dev genromfs xmlstarlet libgstreamer1.0-dev libgstreamer-plugins-base1.0-dev
2、安装python依赖（melodic默认的是python2）
pip install pandas jinja2 pyserial cerberus pyulog numpy toml pyquaternion -i https://pypi.tuna.tsinghua.edu.cn/simple
我安装的时候pyulog没装上，其他的都装上了，如果你们也有这个问题，就把其他的装上，pyulog后面还有个步骤会自动安装
3、安装ros与gazebo
这两个的安装就不多说了，如果没安装的话可以在网上先安装好这两个再继续操作。
4、安装mavros
sudo apt install ros-melodic-mavros ros-melodic-mavros-extras
wget https://gitee.com/robin_shaun/XTDrone/raw/master/sitl_config/mavros/install_geographiclib_datasets.sh
sudo chmod a+x ./install_geographiclib_datasets.sh
sudo ./install_geographiclib_datasets.sh #这步需要装一段时间,请耐心等待PX4配置
我在安装的时候出现两个问题。
第一个问题是sudo apt install包的时候一直出现404，未找到这个包，解决方案时sudo apt update，将软件源更新一下，很可能是原始的位置已经过期了，需要更新才能找到最新的位置。
第二个问题是最后一步的.sh文件执行太慢了，我挂那两三个小时都没结束，大概是因为被墙了吧。解决办法如下2：
-
在 /usr/share 下目录新建 GeographicLib 目录。
-
将 geoids gravity magnetic 三个文件夹拷贝到 /usr/share/GeographicLib 文件夹下面。
上述三个文件夹的链接在这，提取码：dje9
5、PX4安装
这里推荐使用gitee安装，非常感谢XTDrone团队3将代码放在了gitee上，我也使用过github安装，但总是断开连接，无数次重试才完整安装完成。
cd ~
git clone https://gitee.com/robin_shaun/PX4_Firmware
cd PX4_Firmware
git checkout -b xtdrone/dev v1.11.0-beta1
bash ./Tools/setup/ubuntu.sh --no-nuttx --no-sim-tools
将.gitmodules替换为如下内容（在PX4_Firmware文件夹中ctrl+h查看隐藏文件）4
[submodule "mavlink/include/mavlink/v2.0"]
path = mavlink/include/mavlink/v2.0
url = https://gitee.com/robin_shaun/c_library_v2.git
branch = master
[submodule "src/drivers/uavcan/libuavcan"]
path = src/drivers/uavcan/libuavcan
url = https://gitee.com/robin_shaun/uavcan.git
branch = px4
[submodule "Tools/jMAVSim"]
path = Tools/jMAVSim
url = https://gitee.com/robin_shaun/jMAVSim.git
branch = master
[submodule "Tools/sitl_gazebo"]
path = Tools/sitl_gazebo
url = https://gitee.com/robin_shaun/sitl_gazebo.git
branch = master
[submodule "src/lib/matrix"]
path = src/lib/matrix
url = https://gitee.com/robin_shaun/Matrix.git
branch = master
[submodule "src/lib/ecl"]
path = src/lib/ecl
url = https://gitee.com/robin_shaun/ecl.git
branch = master
[submodule "boards/atlflight/cmake_hexagon"]
path = boards/atlflight/cmake_hexagon
url = https://gitee.com/robin_shaun/cmake_hexagon.git
branch = px4
[submodule "src/drivers/gps/devices"]
path = src/drivers/gps/devices
url = https://gitee.com/robin_shaun/GpsDrivers.git
branch = master
[submodule "src/modules/micrortps_bridge/micro-CDR"]
path = src/modules/micrortps_bridge/micro-CDR
url = https://gitee.com/robin_shaun/micro-CDR.git
branch = px4
[submodule "platforms/nuttx/NuttX/nuttx"]
path = platforms/nuttx/NuttX/nuttx
url = https://gitee.com/robin_shaun/NuttX.git
branch = px4_firmware_nuttx-9.1.0+
[submodule "platforms/nuttx/NuttX/apps"]
path = platforms/nuttx/NuttX/apps
url = https://gitee.com/robin_shaun/NuttX-apps.git
branch = px4_firmware_nuttx-9.1.0+
[submodule "platforms/qurt/dspal"]
path = platforms/qurt/dspal
url = https://gitee.com/robin_shaun/dspal.git
[submodule "Tools/flightgear_bridge"]
path = Tools/flightgear_bridge
url = https://gitee.com/robin_shaun/PX4-FlightGear-Bridge.git
branch = master
[submodule "Tools/jsbsim_bridge"]
path = Tools/jsbsim_bridge
url = https://gitee.com/robin_shaun/px4-jsbsim-bridge.git
[submodule "src/examples/gyro_fft/CMSIS_5"]
path = src/examples/gyro_fft/CMSIS_5
url = https://gitee.com/mirrors/CMSIS_5
再次执行子模块更新指令
git submodule update --init --recursive
编译
make px4_sitl_default gazebo
配置环境变量，注意路径的匹配，你若修改了文件夹名要进行对应的修改，第一个catkin_ws是自己的工作目录。
source ~/catkin_ws/devel/setup.bash
source ~/PX4_Firmware/Tools/setup_gazebo.bash ~/PX4_Firmware/ ~/PX4_Firmware/build/px4_sitl_default
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/PX4_Firmware
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/PX4_Firmware/Tools/sitl_gazebo
下面我们就可以测试PX4无人机了，执行下面的命令
cd ~/PX4_Firmware
roslaunch px4 mavros_posix_sitl.launch
此时会打开gazebo环境，里面地面上有一个无人机。
最后一步，测试无人机通讯
rostopic echo /mavros/state
若显示的消息中出现connected: True,则说明MAVROS与SITL通信成功。到此，无人机的配置就结束了。
移动小车的安装
由于实验要求的是实现移动物体的跟踪，因此我使用了一个可控制的小车来代替移动物体5，通过键盘控制节点可以控制小车在gazebo环境中移动。
本试验使用的是TurtleBot3小车，安装和控制移动都非常方便。
安装小车命令6
sudo apt-get install ros-melodic-turtlebot3-*
通过上述命令就安装好了TurtleBot小车，是不是很方便。
由于该小车有三种形态，所以还需要通过环境变量指定一种形态，否则无法运行，我实验中使用的是 burger形态，其他两种形态你们可以自己去修改，我下面都以burger形态小车来讲解。通过环境变量指定小车有一下两种方式，推荐第二种一劳永逸，但若要修改就需要进入.bashrc文件中修改
export TURTLEBOT3_MODEL=burger # 每次打开新的终端都要执行
echo "export TURTLEBOT3_MODEL=burger" >> ~/.bashrc #直接写入环境变量，打开终端每次都会自动执行
下面我们运行小车，测试一下小车控制
roslaunch turtlebot3_gazebo turtlebot3_world.launch
然后运行键盘控制节点
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
若没安装该节点需要先安装，执行下面的命令
sudo apt-get install ros-melodic-teleop-twist-keyboard
通过I L J K ,四个键可以控制小车的运动，详细的运动控制自己看控制台的输出。到此，移动小车的安装就已经完成。
PX4无人机添加摄像头以及配置的修改
通过上面的工作，我们完成了无人机和移动物体的配置，若要完成无人机通过相机追踪小车，那相机怎么能少得了呢？默认的PX4无人机是不带摄像头的，我们需要修改配置文件使其带上一个摄像头。
给PX4添加一个深度摄像机7
cd ~/PX4_Firmware/launch
cp mavros_posix_sitl.launch mavros_posix_sitl_cp.launch # 不修改原始无人机文件，复制一个副本进行修改
gedit mavros_posix_sitl_cp.launch
我后面的修改操作都是基于副本，先复制一份然后操作副本，以保持源代码的结构
做如下改动
添加
<arg name="my_model" default="iris_downward_depth_camera"/>
修改
<arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf"/>
为
<arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg my_model).sdf"/>
添加摄像头就完成了，但是该摄像头默认的分辨率是48 * 64，非常低，飞高一些就看不清地面的小车了，我们还需要修改一下摄像头的分辨率。
cd ~/PX4_Firmware/Tools/sitl_gazebo/models/depth_camera
gedit depth_camera.sdf
将对应部分修改为
<update_rate>10</update_rate>
...
<image>
<format>R8G8B8</format>
<width>400</width>
<height>400</height>
</image>
width与height对应的就是摄像头的分辨率，update_rate是图像的发布频率，由于把像素改高了，怕系统处理速度慢，因此把频率降低一倍。
下面我们来测试一下摄像头是否配置成功。
cd ~/PX4_Firmware
roslaunch px4 mavros_posix_sitl_cp.launch # 注意我修改的都是副本，不要运行错了
然后打开rviz，新开一个终端输入
rviz
先添加一个接收图像的窗口
将图像的话题选择为/camera/rgb/image_raw，另一个对应的是深度图像，可以自己切换看看效果。
这个时候image窗口就会显示无人机摄像机拍摄下来的图像，然后在运行无人机的那个终端输入命令commander takeoff，观察该图像窗口，会随着无人机起飞变化。
由于这个地面平坦，将小车放到这个环境中的话，小车会动不了，所以要将仿真环境改一下，改成原始的空仿真环境。
PX4仿真环境的配置文件是/home/ljw/PX4_Firmware/launch/posix_sitl.launch，与之前一样，我们不对原始文件进行修改，我们修改副本。
cd ~/PX4_Firmware/launch
cp posix_sitl.launch posix_sitl_cp.launch
gedit posix_sitl_cp.launch
将
<!-- Gazebo sim -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="gui" value="$(arg gui)"/>
<arg name="world_name" value="$(arg world)"/>
<arg name="debug" value="$(arg debug)"/>
<arg name="verbose" value="$(arg verbose)"/>
<arg name="paused" value="$(arg paused)"/>
<arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
</include>
修改为
<!-- Gazebo sim -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="gui" value="$(arg gui)"/>
<arg name="world_name" value="$(find turtlebot3_gazebo)/worlds/empty.world"/>
<arg name="debug" value="$(arg debug)"/>
<arg name="verbose" value="$(arg verbose)"/>
<arg name="paused" value="$(arg paused)"/>
<arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
</include>
也就是将原本的gazebo的.world文件换成turtlebot3小车的empty.world文件，这个world里什么都没有。光这样修改还没有生效，因为我们修改的是副本，原始调用这个文件的文件也需要修改，调用这个文件的文件就是之前的mavros_posix_sitl_cp.launch
gedit mavros_posix_sitl_cp.launch
将
<include file="$(find px4)/launch/posix_sitl.launch">
修改为
<include file="$(find px4)/launch/posix_sitl_cp.launch">
到此，无人机的配置就已经完成，可以再次运行一次无人机，看看环境是否发生变化。
合并无人机和移动小车
在前面的步骤中，我们将无人机和移动小车都准备好了，下面我们就只要将无人机和小车放在同一环境中，就能开始我们的跟踪实验了。
通过阅读turtlebot3的启动文件/opt/ros/melodic/share/turtlebot3_gazebo/turtlebot3_empty_world.launch，可以看到启动小车的代码，我们将该代码添加到启动无人机的文件中，就能同时启动小车和无人机。
cd ~/PX4_Firmware/launch
gedit posix_sitl_cp.launch
添加如下代码
<!-- car model and parameter -->
<arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
<arg name="x_pos" default="1.0"/>
<arg name="y_pos" default="1.0"/>
<arg name="z_pos" default="0.0"/>
<param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_burger.urdf.xacro" />
<!-- gazebo car model -->
<node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf" args="-urdf -model turtlebot3_$(arg model) -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param robot_description" />
我们让小车出生在1 1位置，无人机默认出生在0 0 位置。为了让小车更容易被无人机识别，我们将小车全身改成黑色。
roscd turtlebot3_description
cd urdf
sudo gedit turtlebot3_burger.gazebo.xacro
注意选择自己的小车文件进行修改，我这里修改的是burger小车的。
将文件中所有
到此，无人机与移动小车全部配置完毕执行，我们测试一下。
cd ~/PX4_Firmware
roslaunch px4 mavros_posix_sitl_cp.launch
我们可以看到中间一个无人机和一个黑乎乎的小车，周围的环境已经变成了空的了。然后启动键盘控制节点可以控制小车的运动
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
控制无人机运动
通过阅读PX4官网的一个控制实例8，我们可以大致了解无人机运动的控制流程。我将实例代码复制过来并且添加注释，让大家对无人机控制代码有个理解。
/*
头文件，包括常见的geometry_msgs和mavros通信需要的mavros_msgs，添加上就行
*/
#include 通过官网的代码就能知道控制无人机飞行的一般流程，只要将核心代码逻辑修改一下，就能实现无人机对小车的跟踪了。可以先将上面的代码运行一次，观察一下无人机的运动。
总体流程为：
1、通过launch文件启动无人机的gazebo环境
2、新建一个工作空间，包含的依赖有roscpp、std_msgs、geometry_msgs、mavros、cv_bridge、image_transport、sensor_msgs
其中后面几个是为了后续图像处理用的，这里一并导入
3、创建一个cpp文件，将上述代码复制进去
4、修改CMakeLists
5、rosrun该节点
在无人机解锁后就能看到无人机飞到了空中2m处
控制无人机跟踪运动小车
通过上述无人机悬空的代码我们已经了解了控制无人机飞行的代码流程：首先定义好需要发送与接收的话题消息，并且定义好各个请求，然后将无人机切换到OFFBOARD模式，接着解锁无人机，同时需要一直给无人机发送运动控制的消息，包括位置控制或速度控制，并且频率要大于2HZ。通过以上流程框架，我们就能设计一个自动跟踪移动小车的代码。
通过网上查阅资料看到，控制无人机运动不仅可以通过发送位置消息9，还可以像控制小乌龟一样，发送速度消息10，这就为跟踪小车的提供了方案。我设计的跟踪思路：
之前案例订阅和发布的话题就不用多说了，全部都要订阅，然后需要额外订阅的是无人机发送的图像，之前设置了10HZ，也就是一秒钟无人机会发送十帧图像过来，在该订阅的回调函数内，对图像进行处理，检查图像内是否有小车，如果有的话，就通过比较小车像素点的位置和图像中心像素点的位置，来判断方位，并相应的设置速度。图像处理回调函数如下：
#include 上面的回调函数完成了对无人机追踪小车速度的控制，其运行逻辑是：若无人机发现了小车，就通过小车相对无人机的方位，发送x y方向的速度，否则如果丢失的话，在五秒内不进行任何操作，超过五秒后，开始提升无人机的高度扩大视野来寻找小车，最大高度是跟踪小车高度的两倍，一旦发现小车立刻下降并跟踪小车。
上面只是一个回调函数，还要主函数来控制无人机。
void do_H(const mavros_msgs::Altitude::ConstPtr& msg) {
curH = msg->local;
}
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
current_state = *msg;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "offb_node");
ros::NodeHandle nh;
setlocale(LC_ALL, "");
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
("mavros/state", 10, state_cb);
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
("mavros/setpoint_position/local", 10);
ros::Publisher local_vec_pub = nh.advertise<geometry_msgs::Twist>
("/mavros/setpoint_velocity/cmd_vel_unstamped", 10);
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
("mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
("mavros/set_mode");
ros::Subscriber img_sub = nh.subscribe<sensor_msgs::Image>("/camera/rgb/image_raw", 10, doImg);
ros::Subscriber height_sub = nh.subscribe<mavros_msgs::Altitude>
("/mavros/altitude", 10, do_H);
//the setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(20.0);
// wait for FCU connection
while(ros::ok() && !current_state.connected){
ros::spinOnce();
rate.sleep();
}
geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = h;
velocity.linear.x = 0;
velocity.linear.y = 0;
velocity.linear.z = 0;
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
ros::Time last_request = ros::Time::now();
bool takeoff = false;
while(ros::ok()){
if(!takeoff) {
if( current_state.mode != "OFFBOARD" &&
(ros::Time::now() - last_request > ros::Duration(2.0))){
if( set_mode_client.call(offb_set_mode) &&
offb_set_mode.response.mode_sent){
ROS_INFO("Offboard enabled");
}
last_request = ros::Time::now();
}
if( !current_state.armed &&
(ros::Time::now() - last_request > ros::Duration(2.0))){
if( arming_client.call(arm_cmd) &&
arm_cmd.response.success){
ROS_INFO("Vehicle armed");
}
last_request = ros::Time::now();
}
if( current_state.armed &&
(ros::Time::now() - last_request > ros::Duration(5.0))) {
takeoff = true;
ROS_INFO("Vehicle stabled");
start = true;
ROS_INFO("开始追踪...");
last_request = ros::Time::now();
}
local_pos_pub.publish(pose);
} else {
local_vec_pub.publish(velocity);
}
ros::spinOnce();
rate.sleep();
}
return 0;
}
上述代码的逻辑比较好理解，我就不加注释，流程是：先通过位置控制无人机，让无人机在高度h处稳定悬空，当无人机稳定悬停在空中时，将控制无人机的方式改为速度控制，也就是通过发送速度来控制无人机。
为了方便讲解，将代码分成了两部分贴，将两部分合起来放在一个cpp里，就能正常执行。代码中无人机是否稳定悬空是通过一个时间延迟实现的，假定无人机能在五秒内悬停在指定点，五秒前都是通过位置控制无人机，五秒后就一直通过速度控制无人机。
到此，无人机跟踪运动小车的整个实验就完成了。
参考
APM,PX4,GAZEBO,MAVLINK,MAVROS,ROS之间的关系以及科研设备选型 ↩︎
执行 install_geographiclib_datasets.sh 错误！ ↩︎
XTDrone仿真平台基础配置 ↩︎
Ubuntu18.04下基于ROS和PX4的无人机仿真平台的基础配置搭建 ↩︎
在gazebo中导入移动小车+二维码 ↩︎
ROS-melodic学习turtlebot3笔记＜一功能包导入与测试＞ ↩︎
PX4+gazebo仿真给无人机添加摄像头 ↩︎
MAVROS Offboard control example ↩︎
使用ROS节点控制PX4——位置控制 ↩︎
PX4学习笔记3: 速度控制 ↩︎