PSDK的编译与ROS包封装

本文档讲述在NIVIDIA开发板上使用大疆提供的Payload SDK获取无人机实时GPS信息的方法，以及基于Payload SDK发布ROS GPS话题信息的方法。

0 实现目标

在大疆Matrice 350 RTK无人机上安装E-Port 开发者套件，再通过E-Port 开发者套件连接到英伟达开发板，便可以读取当前无人机飞行时的RTK数据和GPS数据。RTK数据与GPS数据在组合导航算法中具有重要重要作用。

本文将要实现的，一是编译大疆提供的Payload SDK，来获取实时GPS数据；二是将获取的GPS信息，以ROS话题的形式，发布出来，供组合导航中其他的ROS节点使用。

1 Payload SDK

Payload 是大疆提供的，用于在M350 RTK等机型上获取无人机自带传感器设备信息的SDK，所获取的信息包括无人机姿态、GPS信息、机载图像等，其可获取的所有信息见文末参考文档中的遥测数据订阅。

Payload SDK代码可以从Github上的大疆官方页下载：

git clone https://github.com/dji-sdk/Payload-SDK.git

1.1 PSDK 源码的编译

用户使用SDK进行无人机二次开发前，需要编译源码，来产生后续开发用到的相关库文件。

SDK源码的结构如下：

CMakeLists.txt  doc  EULA.txt  psdk_lib  samples  tools

psdk_lib中包括了SDK中所有库的头文件，以及不同平台下的编译器；samples是大疆官方给出的代码示例，有C和C++两种代码，包含了GPS信息获取和图像传输等功能，后续的ROS代码也是在samples的基础上进行修改的；CMakeLists.txt则是使用CMake编译源码时的编译配置文件。

下面讲述本人在英伟达开发板上的PSDK的编译历程：

首先，在PSDK源码所在的路径下创建一个build文件夹，并切换到build目录下：

XXX@YYY:~/Payload-SDK$ ls
CMakeLists.txt  doc  EULA.txt  psdk_lib  samples  tools
XXX@YYY:~/Payload-SDK$ mkdir build
XXX@YYY:~/Payload-SDK$ cd build

随后使用cmake命令生成Makefile文件：

XXX@YYY:~/Payload-SDK/build$ cmake ..

以下命令执行的结果和出现的问题：

什么，OPUS？想必是缺少一个库，本人不知所云，然后发邮箱求助大疆客服：

照着客服的引导，我在英伟达开发板上源码安装了ffmpeg 4.1.3，结果还是报了那个错误。这真的让人哭笑不得，其实这个问题只需要安装opus的库就行了：

XXX@YYY:~/Payload-SDK/build$ sudo apt-get install libopus-dev

然后就cmake成功了：

dji_sdk_demo_linux_cxx...
...
-- Configuring done
-- Generating done
-- Build files have been written to: XXX/Payload-SDK/build
XXX@YYY:~/Payload-SDK/build$ ls
bin  CMakeCache.txt  CMakeFiles  cmake_install.cmake  Makefile  samples

可以看到，build路径下生成了Makefile，那下一步当然就是使用make命令进行编译啦：

make -j24

果然没猜错，又出现了问题，该问题的简要描述为：

fatal error: opencv2/dnn.hpp: No such file or directory(opencv4)

这应该是OpenCV的版本不满足要求。但是，版本为多少才是合适的呢，后来我新安装了OpenCV 4.8，并在CMakelists.txt中添加了下面的语句：

find_package(OpenCV 4.8 REQUIRED)

安装OpenCV 4.8时的cmake命令为：

sudo cmake -D CMAKE_BUILD_TYPE=Release -D CMAKE_INSTALL_PREFIX=/usr/local -DENABLE_PRECOMPILED_HEADERS=OFF ..

之后就会发现编译成功了。

当然啦，上述本人遇到的OpenCV问题其他读者不一定遇见，还是安装并使用OpenCV 4.9为好，这是本人咨询大疆客服获知的：

好了，现在应该在自己的开发板上能看到对应的库了：

XXX@YYY:~$ ls /usr/local/lib
cmake                        libopencv_features2d.so.4.8.0  libopencv_imgproc.so          libopencv_stitching.so.408   libopencv_flann.so             libopencv_imgproc.so.408      libopencv_stitching.so.4.8.0
libopencv_calib3d.so         libopencv_flann.so.408         libopencv_imgproc.so.4.8.0    libopencv_videoio.so
libopencv_calib3d.so.408     libopencv_flann.so.4.8.0       libopencv_ml.so               libopencv_videoio.so.408
libopencv_calib3d.so.4.8.0   libopencv_gapi.so              libopencv_ml.so.408           libopencv_videoio.so.4.8.0   libopencv_core.so              libopencv_gapi.so.408          libopencv_ml.so.4.8.0        libopencv_video.so
libopencv_core.so.408        libopencv_gapi.so.4.8.0        libopencv_objdetect.so        libopencv_video.so.408
libopencv_core.so.4.8.0      libopencv_highgui.so           libopencv_objdetect.so.408    libopencv_video.so.4.8.0     libopencv_dnn.so               libopencv_highgui.so.408       libopencv_objdetect.so.4.8.0  								 libopus.a
libopencv_dnn.so.408         libopencv_highgui.so.4.8.0     libopencv_photo.so            
libopencv_dnn.so.4.8.0       libopencv_imgcodecs.so         libopencv_photo.so.408       
libopencv_features2d.so      libopencv_imgcodecs.so.408     libopencv_photo.so.4.8.0      python2.7
libopencv_features2d.so.408  libopencv_imgcodecs.so.4.8.0   libopencv_stitching.so        python3.8

好了，总算把编译的问题彻底解决了。

1.2 PSDK 的使用

开始运行程序前，需要解决E-Port 开发者套件与英伟达开发板之间的连接问题，在大疆开发者社区里，本人发现了以下信息：

建议把”推荐“去掉，本人一开始使用的普通的TTL串口线，没使用FT232串口，以为能直接获取到GPS信息，结果屡次插拔串口线都未能如愿，这才着重注意到了这一句话。害，官方未能着重突出这条信息，就真只能多走弯路。

在运行程序之前，还需使用大疆官方的上位机来设置信息传输的相关参数（包括传输的波特率），详细的方法见参考文档【1】。

这下可以开始运行程序看看效果了，先找出编译成功的可执行文件dji_sdk_demo_linux，这个文件在编译好的文件目录bin中：

shallwing@9d57f9229b66:~/Payload-SDK/build$ ls
bin  CMakeCache.txt  CMakeFiles  cmake_install.cmake  Makefile  samples
shallwing@9d57f9229b66:~/Payload-SDK/build$ cd bin
shallwing@9d57f9229b66:~/Payload-SDK/build/bin$ ls
dji_sdk_demo_linux  dji_sdk_demo_linux_cxx
shallwing@9d57f9229b66:~/Payload-SDK/build/bin$ sudo ./dji_sdk_demo_linux

两个可执行文件里面，dji_sdk_demo_linux是C程序编译生成的，dji_sdk_demo_linux_cxx则是C++程序编译生成的。本人先运行了dji_sdk_demo_linux，然后报了下面的错误：

XXX@YYY:$ sudo ./dji_sdk_demo_linux
'Logs/latest.log' -> 'DJI_0001_20241220_00-22-55.log'
[0.004][user]-[Error]-[DjiUser_FillInUserInfo:578) Please fill in correct user information to 'samples/sample_c/platform/linux/manifold2/application/dji_sdk_app_info.h' file. 

破案了，大疆这是需要每个使用其SDK的开发者要填写自己的相关信息啊。好吧，那就在大疆官网上注册信息，网上搜了一下注册的方法，注册成功之后，便填写文件samples/sample_c/platform/linux/manifold2/application/dji_sdk_app_info.h中的信息：

/* Exported constants --------------------------------------------------------*/
// ATTENTION: User must goto https://developer.dji.com/user/apps/#all to create your own dji sdk application, get dji sdk        application
// information then fill in the application information here.
#define USER_APP_NAME               "your_app_name"
#define USER_APP_ID                 "your_app_id"
#define USER_APP_KEY                "your_app_key"
#define USER_APP_LICENSE            "your_app_license"
#define USER_DEVELOPER_ACCOUNT      "your_developer_account"
#define USER_BAUD_RATE              "460800"

请注意，这里的USER_BAUD_RATE 一定需要与上位机中设置的波特率保持一直。

填写完了，再次运行，终端打印的信息如下图：

“Waiting payload negotiate finish”？，这是什么问题，去网上找了很多帖子，包括大疆开发社区，都没有与此问题详细的解决方案。算了，还是问客服来得快一点：

果然解铃还需系铃人啊，问题交到PSDK的作者们手中一下就解决了，那就找到这个dji_sdk_config.h文件，然后修改CONFIG_HARDWARE_CONNECTION宏的内容吧。

dji_sdk_config.h在PSDK中的位置为：

Payload-SDK/samples/sample_c/platform/linux/manifold2/application

找到的这个宏定义：

/* Exported constants --------------------------------------------------------*/
#define DJI_USE_ONLY_UART                  (0)
#define DJI_USE_UART_AND_USB_BULK_DEVICE   (1)
#define DJI_USE_UART_AND_NETWORK_DEVICE    (2)

/*!< Attention: Select your hardware connection mode here.
* */
#define CONFIG_HARDWARE_CONNECTION         DJI_USE_UART_AND_NETWORK_DEVICE

将其中的DJI_USE_UART_AND_NETWORK_DEVICE修改为DJI_USE_ONLY_UART，随后运行就正常了。好了，demo程序的运行没有问题了，下面就可以开始深入了解demo源码的含义，来自己编写代码获取想要的信息了。

2 遥测数据的读取

本章节以C++程序为例，讲述如何在main函数中订阅读取GPS信息和RTK信息。

2.1 示例代码结构

PSDK源码中给出的C++代码结构如下：

├── module_sample
│   ├── camera_manager
│   ├── flight_controller
│   ├── gimbal
│   ├── hms_manager
│   ├── liveview
│   └── perception
└── platform
    └── linux
        ├── common
        ├── manifold2
        │   ├── application
        │   │   ├── application.cpp
        │   │   ├── application.hpp
        │   │   ├── dji_sdk_app_info.h
        │   │   ├── dji_sdk_config.h
        │   │   ├── dji_sdk_config.json
        │   │   └── main.cpp
        │   ├── CMakeLists.txt
        │   └── hal
        └── nvidia_jetson

可以看到，代码分为机载传感器模块代码，和主函数实现的代码，其中传感器模块代码实现了每个传感器数据接收的功能，这包括机载相机管理、飞行控制、云台控制（gimal），健康管理系统（HMS）、实时视频流（liveview）、**视觉图像（perception）**等。

通过多次阅读PSDK的C++源码，可以得知，C++源码调用了C代码中的部分代码，这个发现影响了下一章节将PSDK源码改写为ROS代码的工作。

2.2 读取机载GPS信息的逻辑

先给出本人改写后的GPS信息和RTK信息的订阅代码：

int main(int argc, char **argv)
{
    Application application(argc, argv);
    T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
    T_DjiReturnCode returnCode;

    T_DjiDataTimestamp timestamp = {0};
    T_DjiFcSubscriptionRtkPosition data = {0};
    T_DjiFcSubscriptionGpsPosition postion = {0};

    returnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_RTK_POSITION, \
    DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ, NULL);
    if(returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
        USER_LOG_ERROR("Subscribe RTK topic error.\n");
    else
        printf("Subscribe RTK topic success.\n");

    returnCode = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_POSITION, \
    DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ, NULL);
    if(returnCode != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS)
        USER_LOG_ERROR("Subscribe GPS topic error.\n");
    else
        printf("Subscribe GPS topic success.\n");


    while (true) {
        returnCode = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_RTK_POSITION, (uint8_t *)&data, sizeof(T_DjiFcSubscriptionRtkPosition), &timestamp);
        printf("microstamp == %u\n", timestamp.microsecond);
        printf("millistamp == %u\n", timestamp.millisecond);
        printf("longitude == %lf\n", data.longitude);
        printf("latitude == %lf\n", data.latitude);
        printf("hfsl == %lf\n", data.hfsl);

        returnCode = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_POSITION, (uint8_t *)&postion, sizeof(T_DjiFcSubscriptionGpsPosition), &timestamp);
        printf("GPS X == %d\n", postion.x);
        printf("GPS Y == %d\n", postion.y);
        printf("GPS Z == %d\n\n", postion.z);
        
        osalHandler->TaskSleepMs(2000);
    }
    return 0;
}

首先，程序需要创建一个Application，这个Application是一个类，当Application类创建时会调用相应的构造函数来创建一个Dji应用程序核心，这个核心与ROS核心类似。可以推测出，大疆对机载传感器的管理借鉴了ROS中传感器节点的管理方法，即将每一个传感器节点看作一个应用线程。若要了解更多与Application有关的信息，请直接阅读源码。

之后，程序开始定义各种后续需要使用的变量，其中T_DjiOsalHandler是一个**操作系统抽象层(Opertating System Level)**类，这个类用于实现与不同操作系统之间的兼容，本人在代码中使用了其延时的函数。

T_DjiDataTimestamp, T_DjiFcSubscriptionRtkPosition, T_DjiFcSubscriptionGpsPosition分别记录了时间戳、RTK和GPS信息，而此处的时间戳是指的程序启动到当前的运行时间间隔，并非是UNIX时间戳。很遗憾，大疆官方并没有在源码的头文件里面告诉大家后两个结构体中的详细成员，但是我们可以从源码文件sample_c/module_sample/fc_subscription/test_fc_subscription.c中窥视出其中些许的信息。

test_fc_subscription.c中的第152至第175行中，给出了FC话题订阅的方法，我们可以运用该示例代码，来实现GPS信息的订阅：

djiStat = DjiFcSubscription_SubscribeTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_POSITION,DJI_DATA_SUBSCRIPTION_TOPIC_1_HZ,
                                               NULL);
    if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
        USER_LOG_ERROR("Subscribe topic gps position error.");
        return DJI_ERROR_SYSTEM_MODULE_CODE_UNKNOWN;
    } 
djiStat = DjiFcSubscription_GetLatestValueOfTopic(DJI_FC_SUBSCRIPTION_TOPIC_GPS_POSITION,
                                                          (uint8_t *) &gpsPosition,
                                                          sizeof(T_DjiFcSubscriptionGpsPosition),
                                                          &timestamp);
        if (djiStat != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
            USER_LOG_ERROR("get value of topic gps position error.");
        } else {
            USER_LOG_INFO("gps position: x = %d y = %d z = %d.", gpsPosition.x, gpsPosition.y, gpsPosition.z);
        }   

可以看到，GPS具有X,Y,Z三个信息，其中X表示经度（Longitude），Y表示纬度（Latitude），Z代表海拔高度。RTK与GPS的信息类似。PSDK中任何机载传感器的遥测数据都通过FC话题进行发布与订阅，订阅FC话题的函数为**DjiFcSubscription_SubscribeTopic，函数的第一个参数决定了遥测数据的类型，是一个宏，所有遥测数据及其对应的宏见参考文档中的遥测数据订阅；第二个参数则用于指定消息获取的频率，也是一个宏，本人在代码中设为了1HZ。通过订阅函数指定订阅话题之后，便可以使用DjiFcSubscription_GetLatestValueOfTopic函数来获取话题消息中的信息值，该函数的第一个参数和订阅函数一样，都是遥测数据对应的宏，第二个参数则是GPS结构体T_DjiFcSubscriptionGpsPosition对应的对象的地址；当使用DjiFcSubscription_GetLatestValueOfTopic**函数获取到GPS信息之后，便可以在GPS结构体对象gpsPosition中读取其GPS信息，

2.3 运行效果

以下是在英伟达开发板上的运行效果，供读者参考：

至此，读取机载GPS信息的逻辑介绍完毕。

3 将 PSDK 源码转化为 ROS 形式

在一般的项目中，通常是使用ROS来实现传感器数据的收集与管理，大疆无人机在使用时也是这样，本章节讲述如何将原始PSDK改写为ROS包。

3.1 文件的布局

一个PSDK中各个模块功能的实现需要多个头文件，以及头文件中定义的函数的C文件组成，因此，在ROS包下的应该具有这些头文件和C文件，把所有头文件放在ROS中的include文件夹中，并将所有.c文件放在src文件夹中。本人整理好的文件结构如下：

XXX@YYY:~/dji_ros/src/include$ ls
application.hpp     dji_sdk_config.h     gimbal          interest_point    utils
camera_emu          dji_sdk_config.json  gimbal_emu      liveview          waypoint_v2
camera_manager      fc_subscription      gimbal_manager  osal              waypoint_v3
data_transmission   flight_control       hms             perception        widget
dji_sdk_app_info.h  flight_controller    hms_manager     power_management  widget_interaction_test

以下是src的结构：

XXX@YYY:~/dji_ros/src/src$ ls
application.cpp      fc_subscription    gimbal_manager  liveview               positioning       widget
camera_emu           flight_control     hal             main.cpp               power_management  widget_interaction_test
camera_manager       flight_controller  hms             osal                   utils
data_transmission    gimbal             hms_manager     payload_collaboration  waypoint_v2
dji_sdk_config.json  gimbal_emu         interest_point  perception             waypoint_v3

好了，绝对会有人告诉我这是如何整理的。没有什么技巧，全靠蛮干，哈哈哈哈，就是把PSDK下sample_c和sample_c++的所有.c文件和.cpp文件全部整理好放在include和src文件里面就好了，慢慢来吧，不要心急，慢工出细活，哈哈哈哈。

3.2 主函数的改写

因为是ROS是基于C++或Python语言进行开发的，所以主函数应该选择PSDK中的main.cpp。修改main.cpp的方法也很简单，只需要把GPS的信息对应发布到消息类型为sensor_msgs/NavSatFix的话题中。

本人修改的地方主要集中在While死循环里：

#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>

	//......

	ros::init(argc, argv, "dji_ros_gps");
    ros::NodeHandle nh;
    sensor_msgs::NavSatFix      msg_gps, msg_rtk;
    ros::Publisher pub_rtk = nh.advertise<sensor_msgs::NavSatFix>("/dji_ros/rtk_info",10);
    ros::Publisher pub_gps = nh.advertise<sensor_msgs::NavSatFix>("/dji_ros/gps_info",10);
    
    while(1)
    {
    	//......
        
    	msg_gps.header.stamp = ros::Time::now();
        msg_gps.header.frame_id = "DJI-GPS";
        msg_gps.status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
        msg_gps.status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
        msg_gps.latitude = postion.x;
        msg_gps.longitude = postion.y;
        msg_gps.altitude = postion.z;

        msg_rtk.header.stamp = ros::Time::now();
        msg_rtk.header.frame_id = "DJI-RTK";
        msg_rtk.status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
        msg_rtk.status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
        msg_rtk.latitude = data.latitude;
        msg_rtk.longitude = data.longitude;
        msg_rtk.altitude = data.hfsl;

        pub_rtk.publish(msg_rtk);     
        pub_gps.publish(msg_gps);
        
        ROS_INFO("longitude == %lf, latitude == %lf, hfsl == %lf, GPS X == %d, GPS Y == %d, GPS Z == %d", \
        data.longitude, data.latitude, data.hfsl, postion.x, postion.y, postion.z);
        
        r.sleep();
        ros::spinOnce();
    }

NavSatFix是一个与地理坐标有关的消息类型，ROS官方给出的NavSatFix信息很齐全：

sensor_msgs是ROS中一个最重要的消息大类，除了NavSatFix之外，还有IMU、CameraInfo、Image等，它包含了当前常用传感器的数据类型定义。

3.3 CMakeLists.txt的编写

到了全文档中最难解决的部分了，CMakeLists.txt是全ROS包中最关键的文档，在这个文档里，我们需要添加源码的包含目录，使之能正常编译不报出undefined reference XXX的错误。最终形成的CMakeLists.txt如下：

cmake_minimum_required(VERSION 3.0.2)
project(dji_ros)

find_package(catkin REQUIRED COMPONENTS
  nav_msgs
  roscpp
  sensor_msgs
  std_msgs
)

aux_source_directory(./src SRC_LIST)
aux_source_directory(./src/fc_subscription SRC_LIST)
aux_source_directory(./src/hal SRC_LIST)
aux_source_directory(./src/data_transmission SRC_LIST)
aux_source_directory(./src/camera_manager SRC_LIST)
#aux_source_directory(./src/flight_controller SRC_LIST)
aux_source_directory(./src/gimbal SRC_LIST)
aux_source_directory(./src/hms_manager SRC_LIST)
aux_source_directory(./src/liveview SRC_LIST)
aux_source_directory(./src/perception SRC_LIST)
aux_source_directory(./src/osal SRC_LIST)
aux_source_directory(./src/utils SRC_LIST)
aux_source_directory(./src/flight_control SRC_LIST)
aux_source_directory(./src/gimbal_manager SRC_LIST)
aux_source_directory(./src/gimbal SRC_LIST)
aux_source_directory(./src/hms SRC_LIST)
aux_source_directory(./src/hms_manager SRC_LIST)
aux_source_directory(./src/waypoint_v2 SRC_LIST)
aux_source_directory(./src/waypoint_v3 SRC_LIST)
aux_source_directory(./src/widget SRC_LIST)
aux_source_directory(./src/widget_interaction_test SRC_LIST)
aux_source_directory(./src/interest_point SRC_LIST)
aux_source_directory(./src/power_management SRC_LIST)

include_directories(
  ${catkin_INCLUDE_DIRS}
  ${CMAKE_CURRENT_SOURCE_DIR}/src
)

add_executable(dji_nav ${SRC_LIST})

target_link_libraries(dji_nav
  ${catkin_LIBRARIES}
  m
  payloadsdk
  pthread
)

catkin_package(
  INCLUDE_DIRS include
  LIBRARIES dji_ros
  CATKIN_DEPENDS roscpp sensor_msgs std_msgs
)

aux_source_directory是Cmake中用来递归添加文件的命令，通过这个命令，我们可以省去一些繁琐的步骤，只需要把先前展示的include中所有文件夹的内容添加上去即可。

4 结尾

大疆不是没有给出过类似的ROS包，在OSDK中就有官方自己编写的ROS代码——Onboard-SDK-ROS，本人在PSDK中的工作，只能算对OSDK-ROS的拙劣模仿吧，哈哈哈哈。

好了，本文到这里也就讲完了所有文件的编辑方法，后续经过catkin_make编译之后，只需要运行rosrun dji_ros dji_nav命令就可以启动GPS信息的话题发布节点，发布的话题消息在dji_ros/rtk_info和dji_ros/gps_info中。本文在实现ROS包的过程中还是留有遗憾，因为时间较为紧张，项目急需要交付，所以flight_controller部分的代码没有编译（catkin_make编译是屡次报出"undefined reference XXX"的错误），这也是后续需要改进的地方吧，本文只是简简单单实现一个GPS ROS话题消息发布的功能，并记录一下自己的心路历程。

最后还是插一句话：果然，大疆客服在我屡次发邮件问问题之后，解答问题的速度加快了，哈哈哈哈

参考文档

【0】Payload SDK

PSDK详细介绍

【1】E-Port 开发者套件

飞行器硬件连接

PSDK各机型连接

【2】PSDK数据的订阅

遥测数据订阅

PSDK中如何订阅数据？

【3】拓展阅读

时间同步

NatSatFix数据类型