多旋翼无人机ROS&C++开发例程(二)：DJI M600无人机ROS开发

这篇博客主要讲DJI M600 Pro无人机的相关ROS开发。

1.飞控UART接线

进行机载ROS程序的开发需要连接机载计算机与飞控，因此需要先连接飞控与机载计算机。
DJI M600 Pro的飞控位于无人机顶部，拆开顶盖后可以看到，如下图所示。

飞控接口的右侧可以看到API的标记，以靠近API的管脚为1号，依次往左排序。1号管脚为UART R×D，图上接线为橙色，2号管脚为UART T×D，图上接线为黄色，3号管脚为GND，图上接线为灰色，4号管脚为VCC 9V，不需要连接。1号管脚R×D接USB转ttl工具的T×D，2号管脚T×D接转换工具的R×D，3号管脚的GND接转换工具的GND即可。而后串口转换器和机载计算机通过USB连接，并按照前文所述的串口绑定方法进行串口绑定，并修改ROS包中对应的串口号等(详细在后续的ROS包介绍中)。

2.DJI OSDK安装

进行DJI的ROS开发首先需要在系统中安装OSDK包，ROS包在编译时需要用到相关库文件。
首先下载OSDK3.9.0版，源码文件从DJI的Github仓库Release中下载。
下载完成后进入OSDK文件夹，执行以下命令：

mkdir build

cd build

cmake ..

make djiosdk-core

安装osdk-core library

sudo make install djiosdk-core

安装完成后下载DJI的ROS SDK，配置ROS工作区间即可进行程序编写、编译与运行。

3.DJI ROS接口

在编写自己的ROS程序时，需要读取无人机的位置、姿态等数据作为算法输入，然后计算后得到速度等指令发布至无人机，从而实现对无人机的控制，下面介绍用到的相关ROS话题，主要参考了DJI-ROS包中提供的相关例程和ROS代码中的相关代码与注释说明。

DJI ROS包主要包括了两部分，dji_sdk中提供了各种话题与服务的接口，dji_sdk_demo中提供了相关的示例程序。我们所编写的程序主要是与dji_sdk中的各种ROS话题与服务进行交互，从而实现对无人机的控制。具体的ROS话题与服务可以在dji_sdk文件夹下的各个文件自行查看。

3.1 service相关

这一部分常用的主要包括获取控制权，任务请求，设置惯性系位置参考点等，可以参考例程中的demo_mission.cpp和demo_flight_control.cpp。

3.1.1 获取与释放控制权限

获取控制权在程序启动，机载计算机获取对无人机的控制权时使用，按照下面的方式建立服务客户端

ros::ServiceClient m_sdk_ctrl_authority_service;
m_sdk_ctrl_authority_service = m_nh.serviceClient("dji_sdk/sdk_control_authority");

在程序启动时调用可以如下的函数发布获取控制权的请求

dji_sdk::SDKControlAuthority authority;
authority.request.control_enable = 1;
m_sdk_ctrl_authority_service.call(authority);

if (!authority.response.result)
{
    ROS_ERROR("obtain control failed!");
    return false;
}

return true;

需要注意，在获取控制权后通过遥控器可能无法立刻对进行控制，若程序出现意外，
可以通过返航按钮或者用遥控器左上角的模式切换，中断程序对无人机的控制。

在完成程序后可以调用如下的函数释放控制权限

dji_sdk::SDKControlAuthority authority;
authority.request.control_enable = 0;
m_sdk_ctrl_authority_service.call(authority);

if (!authority.response.result)
{
    ROS_ERROR("release control failed!");
    return false;
}

return true;

3.1.2 发布起飞、着陆与返航请求

通过下面的服务客户端可以向无人机发布起飞、着陆和返航请求。

m_drone_task_service = m_nh.serviceClient("dji_sdk/drone_task_control");

通过下面的函数可以发布起飞，返航或降落请求

dji_sdk::DroneTaskControl droneTaskControl;
// 起飞
droneTaskControl.request.task = dji_sdk::DroneTaskControl::Request::TASK_TAKEOFF;
// 降落
droneTaskControl.request.task = dji_sdk::DroneTaskControl::Request::TASK_LAND;
// 返航
droneTaskControl.request.task = dji_sdk::DroneTaskControl::Request::TASK_GOHOME;

m_drone_task_service.call(droneTaskControl);

if (!droneTaskControl.response.result)
{
    return false;
}

return true;

3.1.3 设置惯性系参考点

通过如下所示的服务客户端可以建立惯性系位置参考点，便于建立坐标系，订阅无人机的惯性系位置。

m_set_local_pos_reference = m_nh.serviceClient("dji_sdk/set_local_pos_ref");

在程序开始时发布请求，便于后续从ROS话题中读取惯性系坐标。

dji_sdk::SetLocalPosRef localPosReferenceSetter;
m_set_local_pos_reference.call(localPosReferenceSetter);
return localPosReferenceSetter.response.result;

上述的服务请求代码只是简要说明，完整的流程可以参考相关demo程序。

3.2 话题订阅相关

DJI SDK发布了无人机各种状态至对应的话题，供开发者使用，下面介绍一些比较常用的。

3.2.1 惯性坐标

m_localPosition_Sub = m_nh.subscribe("dji_sdk/local_position", 10, &zdlab_mpc_for_M600::localPosition_Sub_callback, this);

对应存储惯性坐标的变量和回调函数为

geometry_msgs::Point m_current_local_pos;

void zdlab_mpc_for_M600::localPosition_Sub_callback(const geometry_msgs::PointStamped::ConstPtr &msg)
{
    m_current_local_pos = msg->point;
}

需要注意的是惯性系位置需要在程序开始时进行设置参考点才可用，m_current_local_pos.x,m_current_local_pos.y和m_current_local_pos.z分别表示了无人机的惯性系坐标。

3.2.2 GPS

m_gps_Sub = m_nh.subscribe("dji_sdk/gps_position", 10, &zdlab_mpc_for_M600::gps_Sub_callback, this);

对应存储GPS变量和回调函数为

sensor_msgs::NavSatFix m_current_gps;

void zdlab_mpc_for_M600::gps_Sub_callback(const sensor_msgs::NavSatFix::ConstPtr &msg)
{
    m_current_gps = *msg;
}

GPS数据可用于计算两点的GPS偏移量，然后做简单的线性变换为惯性系下的坐标，在不设置惯性系参考点时可使用。

3.2.3 欧拉角

m_attitude_Sub = m_nh.subscribe("dji_sdk/attitude", 10, &zdlab_mpc_for_M600::attitude_Sub_callback, this);

对应的存储欧拉角变量和回调函数为

geometry_msgs::Quaternion m_current_atti;

void zdlab_mpc_for_M600::attitude_Sub_callback(const geometry_msgs::QuaternionStamped::ConstPtr &msg)
{
    m_current_atti = msg->quaternion;
    m_current_euler_angle = toEulerAngle(m_current_atti);
}

这里订阅的数据m_current_atti为四元数，需要调用函数进行转换。

geometry_msgs::Vector3 zdlab_mpc_for_M600::toEulerAngle(geometry_msgs::Quaternion quat)
{
    geometry_msgs::Vector3 ans;
    tf::Matrix3x3 R_FLU2ENU(tf::Quaternion(quat.x, quat.y, quat.z, quat.w));
    R_FLU2ENU.getRPY(ans.x, ans.y, ans.z);
    return ans;
}

3.2.4 线速度

 m_velocity_Sub = m_nh.subscribe("dji_sdk/velocity", 10, &zdlab_mpc_for_M600::velocity_Sub_callback, this);

对应的存储线速度变量和回调函数为

geometry_msgs::Vector3 m_current_velocity_ground_frame;
geometry_msgs::Vector3 m_current_velocity_body_frame;

void zdlab_mpc_for_M600::velocity_Sub_callback(const geometry_msgs::Vector3Stamped &msg)
{
    // 地面系速度
    m_current_velocity_ground_frame.x = msg.vector.x;
    m_current_velocity_ground_frame.y = msg.vector.y;
    m_current_velocity_ground_frame.z = msg.vector.z;

    // 根据转换方程得到机体系速度
    // 当然也可以用Eigen库采用矩阵运算方便表述与计算
    m_current_velocity_body_frame.x = msg.vector.x * std::cos(m_current_euler_angle.z) + msg.vector.y * std::sin(m_current_euler_angle.z);
    m_current_velocity_body_frame.y = -msg.vector.x * std::sin(m_current_euler_angle.z) + msg.vector.y * std::cos(m_current_euler_angle.z);
    m_current_velocity_body_frame.z = m_current_velocity_ground_frame.z;
}

3.2.5 角速率

m_angularRate_Sub = m_nh.subscribe("dji_sdk/angular_velocity_fused", 10, &zdlab_mpc_for_M600::angularRate_Sub_callback, this);

对应的存储角速率变量和回调函数为

geometry_msgs::Vector3 m_current_angularRate;

void zdlab_mpc_for_M600::angularRate_Sub_callback(const geometry_msgs::Vector3Stamped::ConstPtr &msg)
{
    m_current_angularRate.x = msg->vector.x;
    m_current_angularRate.y = msg->vector.y;
    m_current_angularRate.z = msg->vector.z;
}

3.3 发布话题相关

通过向指定的话题发布期望的位置、速度和姿态等，可以实现对飞机的控制，结合订阅无人机数据与发布指令作为自己编写的算法的输入输出，即可实现对无人机的控制，下面介绍一些常用的发布话题。

3.3.1 位置指令

通过下面的方法可以向无人机发布期望的位置与偏航角。

ctrlPosYawPub = m_nh.advertise("dji_sdk/flight_control_setpoint_ENUposition_yaw", 10);

controlPosYaw.axes.push_back(xOffset);
controlPosYaw.axes.push_back(yOffset);
controlPosYaw.axes.push_back(zOffset);
controlPosYaw.axes.push_back(yawDesiredRad);
ctrlPosYawPub.publish(controlPosYaw);

3.3.2 线速度和偏航角速率指令

通过下面的方法可以向无人机发布期望的速度(惯性系)与偏航角速率指令。

m_control_velocity_yawRate_Pub = m_nh.advertise("dji_sdk/flight_control_setpoint_ENUvelocity_yawrate", 10);

sensor_msgs::Joy controlVelYawRate;

controlVelYawRate.axes.push_back(control_vel_x);
controlVelYawRate.axes.push_back(control_vel_y);
controlVelYawRate.axes.push_back(control_vel_z);
controlVelYawRate.axes.push_back(control_yaw_rate);

m_control_velocity_yawRate_Pub.publish(controlVelYawRate);

如果自己的算法求解的是机体系的速度，则需要乘以转换矩阵来获得惯性系的速度指令。

4. 硬件在环仿真与实飞相关

在进行硬件在环仿真与实飞之前，需要对dji_sdk文件夹下的launch文件sdk.launch进行修改(不同版本的DJI ROS包可能该launch文件名字不同)。按照前文所述的串口绑定方式修改串口号，即“serial_name”一项。比如下面的例子

<param name="serial_name" type="string" value="/dev/ttytousb"/>

同时还需要修改app_id和enc_key，这两项需要在DJI开发者平台中在激活了M600无人机后获取。

运行程序时首先需要运行sdk.launch，以开启DJI的相关ROS服务。

roslaunch dji_sdk sdk.launch

启动后最好等待一段时间再启动自己的算法程序，如果启动太早可能相关ROS的服务尚未启动完毕，会出现起飞失败等情况。

通过DJI所提供的DJI Assistant软件可以连接M600无人机进行硬件在环仿真测试，便于检验自己的程序是否正确。值得注意的是，在仿真界面左下角显示的信息与dji_sdk发布的相关话题数据并不完全一致，比如水平位置和速度等，这是因为选取坐标系不同导致的，统一按照ROS话题发布的数据就好。下面的截图左下角是DJI Assistant的仿真界面，上面的两个是机载计算机运行的程序，右下角是遥控器app的界面。

M600通过USB线与安装了DJI Assistant的电脑进行连接，接口在标记为4的电池仓下方，在起飞的指示灯底部。

为了保证安全，进行硬件在环仿真的时候切记取下所有螺旋桨再进行仿真调试！！！

实飞的时候因为要从机载计算机的终端启动程序，然后进行实验测试，因此需要远程的工具来操控机载计算机。比较方便的方法是采用局域网下的远程桌面，为了提供可靠的地面站与机载计算机之间的网络连接，可以采用一对无线网桥，具体设备可以在某宝或者某东搜索相关物品即可。通过无线网桥可以保证地面的计算机与机载计算机能够处于同一局域网下，并保证可靠的连接。然后在两台电脑上均安装nomachine软件，从而实现在地面对空中的机载计算机的控制。

最后，实飞有风险，一定要谨慎操作注意安全，程序有问题及时手动退出并用遥控器纠正。