玩转四旋翼无人机（DJI ROS SDK开发包及使用方法)

本文主要介绍Onboard-SDK-ROS的代码设计思想和使用方法，该软件包主要用来完成M100的开发等。
该软件包的主要有3个部分
1. 核心的API部分dji-sdk-lib，用于串口通信，建立各种任务的线程，读取信息的线程。
2. 用于封装核心API为ROS接口的dji_sdk
3. 简单的demo，给出了dji_sdk的使用方法。

Onboard-SDK，顾名思义，这个包是用来给机载的系统用的。飞机记载的系统（如manifold,tk1 etc）中运行了的dji_sdk_node，该节点就是要把DJI飞机的状态信息和飞机的控制导航等功能封装。为什么要封装之后会介绍，所有的控制和信息都是从一个串口读写的，封装的过程我们定义了一些类，类的定义在文件dji_sdk_node.h中。这些类是不需要用来创建实例化的，我们不要调用或者使用这些累的成员和成员函数。因为我们在类的内部把这些变量转化为message和service，使用ros的message和service等机制将这些状态和服务发布出去，因此定义为了私有变量。另外如果我们想要完成飞机的控制、导航和查看飞机的状态等等，可以构造另一个ros节点，订阅相应的topic，service等，这样该节点既可以运行在飞机机载的处理器上，也可以在地面站的PC上，这样每个节点对应一个进程，他们是分离的。这就使用ros封装的便利性。我们自己构造的这个ros节点可以包含dji-drone.h文件，因为该文件已经订阅了了飞机的各种功能服务和状态变量，这样我们又看到了刚才被定义为私有化的那些变量以及那些导航控制的功能函数，因此这些变量现在定义为了public，上层的应用程序还要使用他们。所以ROS的message和service机制可以看做一个编码器和解码器把相应的功能编码并传输至另外一个线程，这样我们能够很方便的使用多进程，部署多个多机器等等。这里给一张大疆的说明文档中的图帮助大家理解。链接在这里

设计模式

所有的控制和信息都是从一个串口读写的，因此在解析串口的API的里边上边做了一个Adapter，将原来的cpp的接口转换为ROS的订阅广播机制。
因此构造了三种ROS 通信机制分别是messages, services, actionlib. 第一种用来广播飞机的状态数据，第二种用来对飞机完成任务的控制。第三种同样是任务控制，但是增加了任务的监控。

dji_sdk

构造了一个节点，名字叫这个DJISDKNode，成员变量有飞机的状态信息，如位姿，加速度，速度等；信息发布者，上述飞机的状态信息通过ros的广播发布出去。服务提供者；飞机的位置姿态控制服务，速度控制服务。action提供者，功能同服务提供这类似，增加了反馈和监控。
我们首先来看该节点的构造函数

init_publishers(nh);
init_services(nh);
init_actions(nh);

首先是初始化发布者，服务提供者和action提供者。发布者主要是初始化需要广播的内容飞机的状态如激活状态，位姿，速度，磁罗盘，GPS数据等。服务提供者主要是初始化一些serviceServer
代码如下

activation_service = nh.advertiseService("dji_sdk/activation", &DJISDKNode::activation_callback, this);
attitude_control_service = nh.advertiseService("dji_sdk/attitude_control", &DJISDKNode::attitude_control_callback, this);
camera_action_control_service = nh.advertiseService("dji_sdk/camera_action_control",&DJISDKNode::camera_action_control_callback, this);
drone_task_control_service = nh.advertiseService("dji_sdk/drone_task_control", &DJISDKNode::drone_task_control_callback, this);
gimbal_angle_control_service = nh.advertiseService("dji_sdk/gimbal_angle_control", &DJISDKNode::gimbal_angle_control_callback, this);
gimbal_speed_control_service = nh.advertiseService("dji_sdk/gimbal_speed_control", &DJISDKNode::gimbal_speed_control_callback, this);
global_position_control_service = nh.advertiseService("dji_sdk/global_position_control", &DJISDKNode::global_position_control_callback, this);
local_position_control_service = nh.advertiseService("dji_sdk/local_position_control", &DJISDKNode::local_position_control_callback, this);
sdk_permission_control_service = nh.advertiseService("dji_sdk/sdk_permission_control", &DJISDKNode::sdk_permission_control_callback, this);
velocity_control_service = nh.advertiseService("dji_sdk/velocity_control", &DJISDKNode::velocity_control_callback, this);
version_check_service = nh.advertiseService("dji_sdk/version_check", &DJISDKNode::version_check_callback, this);
virtual_rc_enable_control_service = nh.advertiseService("dji_sdk/virtual_rc_enable_control", &DJISDKNode::virtual_rc_enable_control_callback, this);
virtual_rc_data_control_service = nh.advertiseService("dji_sdk/virtual_rc_data_control", &DJISDKNode::virtual_rc_data_control_callback,this);
drone_arm_control_service = nh.advertiseService("dji_sdk/drone_arm_control", &DJISDKNode::drone_arm_control_callback, this);
sync_flag_control_service = nh.advertiseService("dji_sdk/sync_flag_control", &DJISDKNode::sync_flag_control_callback, this);
message_frequency_control_service = nh.advertiseService("dji_sdk/message_frequency_control", &DJISDKNode::message_frequency_control_callback, this);
send_data_to_remote_device_service = nh.advertiseService("dji_sdk/send_data_to_remote_device", &DJISDKNode::send_data_to_remote_device_callback,this);

对应每个 serviceServer有一个回调函数，这些service回调函数的实现在文件dji_sdk_node_service.cppz中。这些服务是一般的飞机起飞降落等任务。
action提供者初始化了四个action： drone_task_action_server，local_position_navigation_action_server，global_position_navigation_action_server， waypoint_navigation_action_server。定义如下

DroneTaskActionServer* drone_task_action_server;
LocalPositionNavigationActionServer* local_position_navigation_action_server;
GlobalPositionNavigationActionServer* global_position_navigation_action_server;
WaypointNavigationActionServer* waypoint_navigation_action_server;

对应每个actionServer有一个回调函数，这些action的回调函数的实现在文件dji_sdk_node_action.cpp中。回调函数同样是s使用rosAdapter调用核心API函数，实现相应的功能。
在DJISDKNode的构造函数中还定义了一个类叫做DJISDKMission，

 init_parameters(nh_private);
 int groundstation_enable;
 nh_private.param("groundstation_enable", groundstation_enable, 1);
 if(groundstation_enable)
 {
     dji_sdk_mission = new DJISDKMission(nh);
 }

该类中定义了很多mission相关的回调函数，如下所示。

bool mission_start_callback(dji_sdk::MissionStart::Request& request, dji_sdk::MissionStart::Response& response);
bool mission_pause_callback(dji_sdk::MissionPause::Request& request, dji_sdk::MissionPause::Response& response);
bool mission_cancel_callback(dji_sdk::MissionCancel::Request& request, dji_sdk::MissionCancel::Response& response);
bool mission_wp_upload_callback(dji_sdk::MissionWpUpload::Request& request, dji_sdk::MissionWpUpload::Response& response);
bool mission_wp_download_callback(dji_sdk::MissionWpDownload::Request& request, dji_sdk::MissionWpDownload::Response& response);
bool mission_wp_get_speed_callback(dji_sdk::MissionWpGetSpeed::Request& request, dji_sdk::MissionWpGetSpeed::Response& response);
bool mission_wp_set_speed_callback(dji_sdk::MissionWpSetSpeed::Request& request, dji_sdk::MissionWpSetSpeed::Response& response);
bool mission_hp_upload_callback(dji_sdk::MissionHpUpload::Request& request, dji_sdk::MissionHpUpload::Response& response);
bool mission_hp_download_callback(dji_sdk::MissionHpDownload::Request& request, dji_sdk::MissionHpDownload::Response& response);
bool mission_hp_set_speed_callback(dji_sdk::MissionHpSetSpeed::Request& request, dji_sdk::MissionHpSetSpeed::Response& response);
bool mission_hp_set_radius_callback(dji_sdk::MissionHpSetRadius::Request& request, dji_sdk::MissionHpSetRadius::Response& response);
bool mission_hp_reset_yaw_callback(dji_sdk::MissionHpResetYaw::Request& request, dji_sdk::MissionHpResetYaw::Response& response);
bool mission_fm_upload_callback(dji_sdk::MissionFmUpload::Request& request, dji_sdk::MissionFmUpload::Response& response);
bool mission_fm_set_target_callback(dji_sdk::MissionFmSetTarget::Request& request, dji_sdk::MissionFmSetTarget::Response& response);

用的是serviceServer，回调函数的实现在文件dji_sdk_node_mission.cpp中，调用核心API，完成mission。这些服务是一些特殊的高级的任务，带有路径规划的特点。

使用方法

包含文件dji_drone.h，这个文件中定义了DJIDrone，我们看一下他的构造函数可以发现对应上边的所有server，实现了相应的client, 订阅了上边广播的信息。因此调用了改文件之后实际上在程序了订阅了ros服务节点的全部信息，服务和action。因此只需要调用其中的函数就可以完成需要的功能。
举例介绍demo中一段程序

drone->takeoff();
sleep(8);
for(int i = 0; i < 100; i ++)
{
  if(i < 90)
      drone->attitude_control(0x40, 0, 2, 0, 0);
  else
      drone->attitude_control(0x40, 0, 0, 0, 0);
  usleep(20000);
}
sleep(1);
for(int i = 0; i < 200; i ++)
{
  if(i < 180)
      drone->attitude_control(0x40, 2, 0, 0, 0);
  else
      drone->attitude_control(0x40, 0, 0, 0, 0);
  usleep(20000);
}
sleep(1);

...
drone->landing();

首先是起飞，控制飞机达到的空间点，然后降落。
函数attitude_control的signature是这样的

bool attitude_control(unsigned char ctrl_flag, float x, float y, float z, float yaw)

local_position_control vs attitude_control

我们首先看两个函数的函数头，
bool local_position_control(float x, float y, float z, float yaw)
bool attitude_control(unsigned char ctrl_flag, float x, float y, float z, float yaw)
可以看到他们比较类似，但是attitude_control中多了一个标志变量，这个变量控制飞机的飞行模式，here给出了飞机的飞行模式以及他们之间的组合。文中写到只有GPS信号良好（health_flag >=3也就搜到超过3颗星的时候）的时候才能使用水平位置控制模式（HORI_POS）。之后在GPS信号良好或者Guidance工作正常的情况下，才能呢更实用水平速度控制模式（HORI_VEL）。
前者订阅的是attitude_control_service，或者订阅的是local_position_control_service，而在sdk的源文件里我们可以看到attitude_control_service的回调函数attitude_control_callback调用的API为

DJI::onboardSDK::FlightData flight_ctrl_data;
flight_ctrl_data.flag = request.flag;
flight_ctrl_data.x = request.x;
flight_ctrl_data.y = request.y;
flight_ctrl_data.z = request.z;
flight_ctrl_data.yaw = request.yaw;

rosAdapter->flight->setFlight(&flight_ctrl_data);

response.result = true;
return true;

而local_position_control_service对应的回调函数中是这样的

DJI::onboardSDK::FlightData flight_ctrl_data;
flight_ctrl_data.flag = 0x90;
flight_ctrl_data.x = dst_x - local_position.x;
flight_ctrl_data.y = dst_y - local_position.y;
flight_ctrl_data.z = dst_z;
flight_ctrl_data.yaw = request.yaw;

rosAdapter->flight->setFlight(&flight_ctrl_data);

那么模式0x90对应的是那个呢？
刚才那个网页里可以看到

因此我们可以看到local_position_control函数实现了attitude_control函数的一种特殊的控制模式，那就是水平速度，竖直速度和偏航角速度的控制模式。