DJI_OSDK开发（二）：DJI OSDK开发包详解

大疆官方提供的软件包的主要有3个部分 
1. 核心的API部分dji-sdk-lib，用于串口通信，建立各种任务的线程，读取信息的线程。 
2. 用于封装核心API为ROS接口的dji_sdk 
3. 简单的demo，给出了dji_sdk的使用方法。

由于涉及到的内容比较多，先看一下官方提供的关系图

1.dji_sdk_node.cpp

该节点为dji_sdk包的核心，成员变量包括无人机的状态信息，服务信息，控制信息，话题发布和订阅信息。

1）状态信息

参数列表，包括串口名称，波特率，开发者ID及密码，直接在sdk.launch中进行修改即可。

  nh_private.param("serial_name",   serial_device, std::string("/dev/ttyUSB0"));
  nh_private.param("baud_rate",     baud_rate, 921600);
  nh_private.param("app_id",        app_id,    123456);
  nh_private.param("app_version",   app_version, 1);
  nh_private.param("enc_key",       enc_key, std::string("abcd1234"));
  nh_private.param("drone_version", drone_version, std::string("M100")); // choose M100 as default
  nh_private.param("gravity_const", gravity_const, 9.801);
  nh_private.param("align_time",    align_time_with_FC, true);
  nh_private.param("use_broadcast", user_select_BC, false);

三个经常要使用的位置参数，经度，纬度，海拔（海拔和姿态极易混淆，注意区分）

local_pos_ref_latitude = local_pos_ref_longitude = local_pos_ref_altitude = 0;

2）服务信息

  drone_activation_server   = nh.advertiseService("dji_sdk/activation",                     &DJISDKNode::droneActivationCallback,        this);
  drone_arm_server          = nh.advertiseService("dji_sdk/drone_arm_control",              &DJISDKNode::droneArmCallback,               this);
  drone_task_server         = nh.advertiseService("dji_sdk/drone_task_control",             &DJISDKNode::droneTaskCallback,              this);
  sdk_ctrlAuthority_server  = nh.advertiseService("dji_sdk/sdk_control_authority",          &DJISDKNode::sdkCtrlAuthorityCallback,       this);
  camera_action_server      = nh.advertiseService("dji_sdk/camera_action",                  &DJISDKNode::cameraActionCallback,           this);

对应的回调函数可在dji_sdk_node_services.cpp中找到。

3）控制信息

  flight_control_sub = nh.subscribe<sensor_msgs::Joy>(
    "dji_sdk/flight_control_setpoint_generic", 10, 
    &DJISDKNode::flightControlSetpointCallback,   this);

对应的回调函数可在dji_sdk_node_control中找到。

void DJISDKNode::flightControlSetpointCallback(
  const sensor_msgs::Joy::ConstPtr& pMsg)
{ 
  float xSP    = pMsg->axes[0];
  float ySP    = pMsg->axes[1];
  float zSP    = pMsg->axes[2];
  float yawSP  = pMsg->axes[3];
  uint8_t flag = (uint8_t)(pMsg->axes[4]);

  flightControl(flag, xSP, ySP, zSP, yawSP);
}

如该函数通过订阅JOY消息来将无人机移动到指定位置，其中flag用来表示控制模式，即FAP。

4）话题发布和订阅信息

  height_publisher =
    nh.advertise<std_msgs::Float32>("dji_sdk/height_above_takeoff", 10);

  velocity_publisher =
    nh.advertise<geometry_msgs::Vector3Stamped>("dji_sdk/velocity", 10);

这里发布的许多话题都是我们比较感兴趣的，如attitude（四元组），imu，gps_position（经纬度，海拔），height_above_takeoff，velocity等等。

2.dji_sdk_demo

以demo_flight_control为例

首先是初始化

  // Subscribe to messages from dji_sdk_node
  ros::Subscriber attitudeSub = nh.subscribe("dji_sdk/attitude", 10, &attitude_callback);    //话题，队列长度，回调函数
  ros::Subscriber gpsSub      = nh.subscribe("dji_sdk/gps_position", 10, &gps_callback);
  ros::Subscriber flightStatusSub = nh.subscribe("dji_sdk/flight_status", 10, &flight_status_callback);
  ros::Subscriber displayModeSub = nh.subscribe("dji_sdk/display_mode", 10, &display_mode_callback);

从dji_sdk_node处订阅消息并赋给已经定义好的全局变量。

  // Publish the control signal
  ctrlPosYawPub = nh.advertise<sensor_msgs::Joy>("/dji_sdk/flight_control_setpoint_ENUposition_yaw", 10);
 
  ctrlBrakePub = nh.advertise<sensor_msgs::Joy>("dji_sdk/flight_control_setpoint_generic", 10);
  
  // Basic services
  sdk_ctrl_authority_service = nh.serviceClient<dji_sdk::SDKControlAuthority> ("dji_sdk/sdk_control_authority");
  drone_task_service         = nh.serviceClient<dji_sdk::DroneTaskControl>("dji_sdk/drone_task_control");
  query_version_service      = nh.serviceClient<dji_sdk::QueryDroneVersion>("dji_sdk/query_drone_version");

之后发布控制指令及启动基本服务。

  if(takeoff_result)
  {
    square_mission.reset();
    square_mission.start_gps_location = current_gps;
    square_mission.setTarget(0, 20, 3, 60);
    square_mission.state = 1;
    ROS_INFO("##### Start route %d ....", square_mission.state);
  }

最后是具体任务规划的函数，事实上这里只是定义了初始状态，具体过程是通过不断调用回调函数gps_callback来完成的

void gps_callback(const sensor_msgs::NavSatFix::ConstPtr& msg)
{
  static ros::Time start_time = ros::Time::now();
  ros::Duration elapsed_time = ros::Time::now() - start_time;
  current_gps = *msg;

  // Down sampled to 50Hz loop
  if(elapsed_time > ros::Duration(0.02))
  {
    start_time = ros::Time::now();
    switch(square_mission.state)
    {
      case 0:
        break;

      case 1:
        if(!square_mission.finished)
        {
          square_mission.step();
        }
        else
        {
          square_mission.reset();
          square_mission.start_gps_location = current_gps;
          square_mission.setTarget(20, 0, 0, 0);
          square_mission.state = 2;
          ROS_INFO("##### Start route %d ....", square_mission.state);
        }
        break;

      case 2:
        if(!square_mission.finished)
        {
          square_mission.step();
        }
        else
        {
          square_mission.reset();
          square_mission.start_gps_location = current_gps;
          square_mission.setTarget(0, -20, 0, 0);
          square_mission.state = 3;
          ROS_INFO("##### Start route %d ....", square_mission.state);
        }
        break;
      case 3:
        if(!square_mission.finished)
        {
          square_mission.step();
        }
        else
        {
          square_mission.reset();
          square_mission.start_gps_location = current_gps;
          square_mission.setTarget(-20, 0, 0, 0);
          square_mission.state = 4;
          ROS_INFO("##### Start route %d ....", square_mission.state);
        }
        break;
      case 4:
        if(!square_mission.finished)
        {
          square_mission.step();
        }
        else
        {
          ROS_INFO("##### Mission %d Finished ....", square_mission.state);
          square_mission.state = 0;
        }
        break;
    }
  }
}

通过state来分段控制，通过finished来结束过程。

其他几个demo也是类似的。