原文链接PX4无人机-Gazebo仿真实现移动物体的跟踪末尾有演示视频

这个学期我们有一个智能机器人系统的课设，我们组分配到的题目是《仿真环境下使用无人机及相机跟踪移动物体》，本文主要记录完成该课设的步骤以及内容。我们采用的最终方案是PX4飞控+gazebo仿真+mavros通讯控制，实现了在gazebo环境下无人机跟踪一个移动的小车。本文所使用的是Ubuntu18.04 + melodic。

试验环境介绍

首先要搞懂各个部分的关系¹，以及各自的作用，才能对控制无人机有个完整的认识，我在一开始做的时候就花了很多时间都没搞懂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文件执行太慢了，我挂那两三个小时都没结束，大概是因为被墙了吧。解决办法如下²：

• 在 /usr/share 下目录新建 GeographicLib 目录。

• 将 geoids gravity magnetic 三个文件夹拷贝到 /usr/share/GeographicLib 文件夹下面。

上述三个文件夹的链接在这，提取码：dje9

5、PX4安装

这里推荐使用gitee安装，非常感谢XTDrone团队³将代码放在了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查看隐藏文件）⁴

[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通信成功。到此，无人机的配置就结束了。

移动小车的安装

由于实验要求的是实现移动物体的跟踪，因此我使用了一个可控制的小车来代替移动物体⁵，通过键盘控制节点可以控制小车在gazebo环境中移动。

本试验使用的是TurtleBot3小车，安装和控制移动都非常方便。

安装小车命令⁶

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添加一个深度摄像机⁷

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小车的。

将文件中所有<material>Gazebo/xxx</material>中的xxx全部改成Black。

到此，无人机与移动小车全部配置完毕执行，我们测试一下。

cd ~/PX4_Firmware
roslaunch px4 mavros_posix_sitl_cp.launch

我们可以看到中间一个无人机和一个黑乎乎的小车，周围的环境已经变成了空的了。然后启动键盘控制节点可以控制小车的运动

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

控制无人机运动

通过阅读PX4官网的一个控制实例⁸，我们可以大致了解无人机运动的控制流程。我将实例代码复制过来并且添加注释，让大家对无人机控制代码有个理解。

/*
头文件，包括常见的geometry_msgs和mavros通信需要的mavros_msgs，添加上就行
*/
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>


/*
current_state表示的是无人机的状态，在主函数中订阅了对应的话题，这个状态就会不断更新，表示无人机当前的状态。state_cb就是对应的回调函数，会不断的执行，更新状态。现在先不用管为什么需要这个状态，后面的代码会解释。
*/
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;
    
	// 订阅无人机的状态
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    
    /* 
    发布一个geometry_msgs::PoseStamped的消息，需要知道的是，这个消息是控制无人机的一种方式，将指定坐标包裹进这个消息，然后发布出去，无人机就能自动飞行到指定的坐标地点
    */ 
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
    
    /*
    无人机有一个锁，如果不解锁，无人机虽然接受了命令但是不会动被锁住了，只有解锁了才能对无人机进行控制，下面这个服务调用就是用来请求解锁无人机。上面的current_state就包含了无人机是否解锁的信息，若没解锁就需要解锁，否则就不用，其用途在这就体现出来
    */
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    
    /*
    无人机飞行有很多种模式，如果需要用代码操控无人机，我们就需要切换到OFFBOARD模式。上面的current_state也包含了无人机当前的飞行模式，若不是OFFBOARD就需要切换到该模式。下面的这个服务调用就是用来请求切换无人机飞行模式。
    */
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");

    //the setpoint publishing rate MUST be faster than 2Hz
    /*
    在OFFBOARD模式下，需要以>=2HZ的频率向无人机发送消息，否则无人机会回退到OFFBOARD模式之前所在的模式，因此这里的rate需要设置的比2大就行
    */
    ros::Rate rate(20.0);
	
    // wait for FCU connection
    /*
    等待无人机与控制站连接（代码的方式就是代理），只有连接了才能发送消息
    */
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    /*
    pose就是坐标，本实例是让无人机在2m处悬空，因此z设置为2，z表示的就是高度
    */
    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 2;

    // 下面这个感觉有没有都无所谓
    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    // 请求的切换模式的消息，设置为OFFBOARD
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    // 请求解锁的消息，arm表示解锁，设置为true，disarm是上锁
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    // 记录上次请求的时间
    ros::Time last_request = ros::Time::now();

    while(ros::ok()){
        // 如果无人机模式不是OFFBOARD并且离上次操作时间大于5秒就发送请求切换，这里的5s是为了演示清楚设置的延时
        if( current_state.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request > ros::Duration(5.0))){
            if( set_mode_client.call(offb_set_mode) &&
                offb_set_mode.response.mode_sent){
                ROS_INFO("Offboard enabled");
            }
            // 更新本次请求的时间
            last_request = ros::Time::now();
        } else {
            // 如果当前未解锁且与请求时间大于5s，就发送请求解锁
            if( !current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(5.0))){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
        }
		
        // 不断发送位置消息，但是只有解锁后才能真正开始运动，如果不发送就会退出OFFBOARD模式，因为请求发送速度要>=2HZ
        local_pos_pub.publish(pose);

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

通过官网的代码就能知道控制无人机飞行的一般流程，只要将核心代码逻辑修改一下，就能实现无人机对小车的跟踪了。可以先将上面的代码运行一次，观察一下无人机的运动。

总体流程为：

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。通过以上流程框架，我们就能设计一个自动跟踪移动小车的代码。

通过网上查阅资料看到，控制无人机运动不仅可以通过发送位置消息⁹，还可以像控制小乌龟一样，发送速度消息¹⁰，这就为跟踪小车的提供了方案。我设计的跟踪思路：

之前案例订阅和发布的话题就不用多说了，全部都要订阅，然后需要额外订阅的是无人机发送的图像，之前设置了10HZ，也就是一秒钟无人机会发送十帧图像过来，在该订阅的回调函数内，对图像进行处理，检查图像内是否有小车，如果有的话，就通过比较小车像素点的位置和图像中心像素点的位置，来判断方位，并相应的设置速度。图像处理回调函数如下：

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/Altitude.h>


#include "sensor_msgs/Image.h"
#include "cv_bridge/cv_bridge.h"

#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgproc/imgproc.hpp>
// 一些全局变量

// 悬空高度（追踪小车的高度）
const double h = 4;
// 调整高度的速度（上升或下降）
const double hv = 0.1;

// 控制无人机的速度
geometry_msgs::Twist velocity;

// 无人机当前的高度
double curH;

// 无人机是否已经稳定在空中的标志
bool start = false;

void doImg(const sensor_msgs::Image::ConstPtr &msg) {
    
    if(!start) return;
    
    // 将无人机发布的图像先转化为灰度图，再进行二值化，就能得到黑白图像，若小车出现，那么在图像内有黑色的像素，否则图像全是白色像素，这也是我将小车改成黑色的原因，若改成其它颜色就不好进行分离
    cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
	cv::Mat img = cv_ptr -> image;
    cv::Mat gray, bin;
    cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
    cv::threshold(gray, bin, 127, 255, cv::THRESH_BINARY);
    
    // 获得图像的宽和高
    static int row = bin.rows, col = bin.cols;
    // 图像中心点的位置，我们假设图像中心点的位置就是无人机的位置，这样就能很方便的发布速度来控制无人机
    static double centX = row / 2, centY = col / 2;
    
    // x y用来记录小车在该帧图像出现的位置
    int x, y;
    // 是否找到小车的标记
    bool findCar = false;
    
    // 遍历图像，若图像内有黑色像素则代表发现了小车，记录下此时的x y位置
    for(int i = 0; i < row; i++) {
        for(int j = 0; j < col; j++) {
            uchar point = bin.at<uchar>(i, j);
            if(point == 0) {
                findCar = true;
                x = i, y = j;
                break;
            }
        }
        if(findCar) break;
    }
    
    // 记录最后一次找到小车的时间
    static ros::Time last_find_time = ros::Time::now();
    if(findCar) {
        ROS_INFO("找到目标位置, x = %d, y = %d", x, y);
        // 将小车（所在像素点）相对无人机（图像中心像素点）的位置归一化到0 ~ 1之间，并以此作为控制无人机的速度，小车离无人机越远，则无人机的速度越大，否则无人机的速度越小
        double vx = abs(centX - x) / centX;
        double vy = abs(centY - y) / centY;
        
        // 经测试，无人机发送的图像的垂直方向是无人机的x方向，图像的水平方向是无人机的y方向
        // 因此，若小车（像素位置）在无人机（像素位置）上方，需要发送一个正的x方向速度，否则要发送一个负方向的速度
        if(x < centX) velocity.linear.x = vx;
        else velocity.linear.x = -vx;
        
		// y方向同理
        if(y < centY) velocity.linear.y = vy;
        else velocity.linear.y = -vy;

        // 若不给无人机发送z方向的速度，无人机会时上时下，因此通过下面这个代码控制无人机高度，若低于一定高度，就发布z方向的速度，若高于某个高度，就发送一个-z方向的速度，让无人机下降
        if(curH < h - 0.5) velocity.linear.z = hv;
        else if(curH < h + 0.5) velocity.linear.z = 0;
        else velocity.linear.z = (curH - h) * -hv;
        ROS_INFO("发布速度 x : %f, y : %f, z : %f", velocity.linear.x, velocity.linear.y, velocity.linear.z);
        // 记录无人机最后一次发现小车的时间，后面有用
        last_find_time = ros::Time::now();
    } else {
        ros::Time now = ros::Time::now();
        velocity.linear.x = 0;
        velocity.linear.y = 0;
        // 无人机丢失目标五秒内，什么都不操作
        if(now - last_find_time < ros::Duration(5)) {
            ROS_INFO("没有找到目标...");
        } else {
            // 无人机丢失目标五秒后，开始向上飞行（扩大视野）来搜寻小车，搜寻的最高高度是无人机跟踪小车高度的两倍，这也是前面代码中控制无人机下降的原因，若无人机在升空过程中发现目标小车，会立刻下降跟踪小车
            if(curH < 2 * h - 1) {
                ROS_INFO("上升高度寻找，当前高度为：%.2f", curH);
                velocity.linear.z = hv;
            } else {
                if(curH > 2 * h + 1) velocity.linear.z = -hv;
                else velocity.linear.z = 0;
                ROS_INFO("目标丢失。。。");
            }
        }
    }
}

上面的回调函数完成了对无人机追踪小车速度的控制，其运行逻辑是：若无人机发现了小车，就通过小车相对无人机的方位，发送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里，就能正常执行。代码中无人机是否稳定悬空是通过一个时间延迟实现的，假定无人机能在五秒内悬停在指定点，五秒前都是通过位置控制无人机，五秒后就一直通过速度控制无人机。

到此，无人机跟踪运动小车的整个实验就完成了。

参考

---

1. APM,PX4,GAZEBO,MAVLINK,MAVROS,ROS之间的关系以及科研设备选型 ↩︎

2. 执行 install_geographiclib_datasets.sh 错误！ ↩︎

3. XTDrone仿真平台基础配置 ↩︎

4. Ubuntu18.04下基于ROS和PX4的无人机仿真平台的基础配置搭建 ↩︎

5. 在gazebo中导入移动小车+二维码 ↩︎

6. ROS-melodic学习turtlebot3笔记＜一功能包导入与测试＞ ↩︎

7. PX4+gazebo仿真给无人机添加摄像头 ↩︎

8. MAVROS Offboard control example ↩︎

9. 使用ROS节点控制PX4——位置控制 ↩︎

10. PX4学习笔记3: 速度控制 ↩︎