无人机自动驾驶软件系列 E06：简单全局目标追踪

本讲中，我会简要介绍如何通过 GAAS-Object-Tracking 模块进行全局目标跟踪。注意，此实现比较简单，实际情况下需要使用更为复杂的方法。

前几讲中，我们讲述了如何设置仿真环境，如何通过 python 控制无人机，如何使用双目进行深度估计。本讲中，我会简要介绍如何通过 GAAS-Object-Tracking  模块进行全局目标跟踪。注意，此实现比较简单，实际情况下需要使用更为复杂的方法。
为了方便维护，我们将跟踪算法放入了另一个 repo，你可以通过如下命令克隆跟踪模块到本地环境：
git clone https://github.com/generalized-intelligence/GAAS-Object-Tracking.git
GAAS 的全局跟踪模块包含四个算法，分别是TLD， KCF，GOTURN， 以及基于 pysot 的 SiamRPN等算法。其中后两种为神经网络算法，前两种为传统算法。在本教程，我们使用 KCF 算法作为程序的追踪算法。
请注意，该全局跟踪是 GAAS：https://github.com/generalized-intelligence/GAAS 的一个模块，无法独立使用。请熟悉 GAAS 后再进行本教程。
更新环境
首先请更新 GAAS:
git pull origin master
接下来，将 Gazebo 仿真所用的文件拷贝到 PX4 文件所在的目录中：
cp -r (GAAS_PATH)/simulator/launch/* ~/catkin_ws/src/Firmware/launch/
cp -r (GAAS_PATH)/simulator/models/* ~/catkin_ws/src/Firmware/Tools/sitl_gazebo/models/
cp -r (GAAS_PATH)/simulator/worlds/* ~/catkin_ws/src/Firmware/Tools/sitl_gazebo/worlds/
cp -r (GAAS_PATH)/simulator/posix-config/* ~/catkin_ws/src/Firmware/posix-configs/SITL/init/ekf2/
cp -r (GAAS_PATH)/simulator/urdf ~/catkin_ws/src/Firmware/Tools/sitl_gazebo/
接下来进入下一步
启动仿真
本教程的要达到的效果是在仿真环境下，无人机追踪一个可以移动的小车。首先启动仿真：
roslaunch px4 car_tracking.launch
上述命令会启动仿真，生成一个可移动的小车，以及无人机。
仿真环境下的无人机和小车
这个小车将会是你追踪的目标。小车订阅了 /cmd_vel 的 topic 所以我们可以通过代码（这里不详述了），或者使用插件：
sudo apt install ros-kinetic-teleop-twist-keyboard
安装好插件后，使用：
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
运行插件。使用方法如下图，在启动插件后会自动打印说明，只需在启动插件的终端按相应的按键，即可控制小车移动。
teleop_twist 使用说明
本教程所用的小车是一个非常简单的小车模型，如果你需要一个更加精美的小车模型，可以选择使用 husky_gazebo 模型。控制都是通过 /cmd_vel 进行。
编译跟踪模块
本教程使用跟踪模块的 KCF 方法，此方法使用 c++ 实现，所以需要编译：
cd (GAAS-Object-Tracking_PATH)/KCF
mkdir build
cd build
cmake ..
make -j6
如果遇到 "fatal error: ros_kcf/InitRect.h: No such file or directory..." 的问题，调大编译所用线程的数量即可。比如从 make -j1 到 make -j6 。
测试
1. 初始化仿真环境
首先打开仿真环境：
roslaunch px4 car_tracking.launch
让飞机起飞并悬停，此时悬停的高度为 3 米：
cd (GAAS_PATH)/demo/tutorial_6/6_object_tracking/
python python px4_mavros_run.py
打开另一个终端控制飞机飞到小车上方：
python init_drone.py
飞机在小车上方悬停
或者参考 第一课 的方法自行编辑指令控制飞机飞到小车上方。打开 rviz 确认小车出现在摄像机的视野。
摄像机视角
2. 开启全局追踪算法
打开新的终端，开启全局追踪算法：
cd (GAAS-Object-Tracking_PATH)/KCF
./bin/startRosService
注：此时是没有任何输出的，算法正在等待通过 ROS Service 给算法设置起始位置。
GAAS 提供了一个小的 GUI 程序来快速设置初始坐标点。使用如下命令即可运行：
cd (GAAS_PATH)/demo/tutorial_6/6_object_tracking/
python set_init.py
为了不使我们的环境变量太过冗余，我们人工修改下 set_init.py 将 Service 的依赖加入到程序的系统变量。首先用编辑器打开 set_init.py ：
gedit (GAAS_PATH)/demo/tutorial_6/6_object_tracking/set_init.py
将 ROS Service 依赖加载到运行时的变量：
import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
import numpy as np 
​
import sys 
sys.path.append('(GAAS-Object-Tracking_PATH)/KCF/build/devel/lib/python2.7/dist-packages')  #
from ros_kcf.srv import InitRect    #Import this ROS Service dependency
from std_msgs.msg import Int32MultiArray
设置以后保存，关闭 gedit ，运行 set_init.py ，在弹出的界面中画一个矩形框住目标，然后在键盘上点击 s 将目标初始位置通过 ROS Service 传递给全局追踪算法。
设置小车初始位置
此时在算法运行的终端界面中，可以看到终端在打印检测到的每帧的结果，此时可以按 ctrl + c 关闭程序。
实时追踪结果
追踪算法默认订阅 /gi/simulation/left/image_raw topic，如果您想修改，请修改配置文件：
gedit (GAAS-Object-Tracking_PATH)/KCF/config.yaml
此 GUI 小程序是一个非常简单的程序，我们未来会推出功能更全面的 GUI 程序，也欢迎你为 GAAS 做贡献提供代码。
3. 开始全局追踪
首先设置参数，你可以设置飞机在一个固定的高度，也可以根据双目摄像头的景深估计来使飞机与小车保持固定高度距离。目前软件支持使用 StereoBM 算法计算视差图，你可以使用 这个程序 进行调参。为了方便使用，本教程默认使用设置固定高度的方法来做。在这种方法下，如果你想控制飞机飞得更高，请修改相应参数。
然后启动算法：
python track_and_move.py
启动小车控制插件：
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
控制小车即可。
飞机跟随小车走
在追踪过程中，请勿关闭追踪算法。如果你觉得需要重新追踪物体，请重启追踪算法，并运行 set_init.py 重新设置物体起始坐标。
在本教程中飞机和小车的朝向是 X 轴正方向，如果你需要修改初始朝向，请修改代码。如果你想使用世界坐标系，请参考 课程一 的代码。我们也会不断完善目标追踪的功能，加入精度更高的双目测距，以及加入追迹算法。本教程旨在提供一个简单算法实现基本的追踪小车。如果你发现任何问题，或有不明白的地方，欢迎提 issue 或者加入我们的微信群聊。