前阵子折腾了一个项目要在妙算3这类机载计算机上把PSDK拿到的GPS数据接进ROS系统再用CMake把节点完整编译运行起来。这活儿听起来不复杂无非就是读写数据、发个话题但真做起来牵扯到PSDK接口、ROS消息类型、CMake链接关系哪一个环节没理顺都会卡住半天。好在这个项目最后跑通了我把中间的设计思路、踩坑过程、完整的CMake配置方式都整理出来给正在做无人机机载开发、ROS机器人定位或者准备从PSDK/OSDK过渡到ROS的朋友一个可以直接参考的案例。这个项目解决的核心问题其实可以拆成三块第一怎么从PSDK链路里把GPS原始数据取出来第二怎么把拿到手的经纬度、海拔、速度等信息转换成ROS标准的sensor_msgs/NavSatFix话题第三怎么用CMake把依赖了PSDK库和ROS库的节点编译利索。文中用到的方案、代码片段和编译配置都是实际验证过能跑的版本照着抄基本不会翻车。1. 项目概述与整体设计思路1.1 这个项目到底在做什么先明确一下项目边界。整个应用跑在机载计算机上机载计算机通过SkyPort或者E-Port这类接口和飞控通信PSDK就是负责这个通信链路的开发包。我们需要定期从飞控获取GPS定位数据然后把数据填进ROS的NavSatFix消息里以固定的频率发布到ROS话题上供上层模块订阅。如果你在做一个无人机的自主飞行、航点规划或者视觉定位融合的项目这一步通常是整个软件链路的第一环。GPS数据不到位后面所有跟位置相关的模块都没有依据。这个节点本身不复杂但它要和两个不同的生态打交道一边是大疆的PSDK另一边是ROS的消息通信体系中间要做数据格式转换、单位换算、坐标转换还要保证时序和频率稳定。1.2 为什么用“PSDK ROS CMake”这套组合先说PSDK。大疆提供OSDK和PSDK两套开发体系OSDK偏向机载计算机对接飞控PSDK偏向负载设备开发但PSDK同样能拿到飞控的GPS数据。实际项目中如果你的负载挂载在一个需要双向交互的硬件上或者需要复用PSDK的负载链路选择PSDK是合理的。当然如果你的项目纯粹只做飞控数据采集用OSDK可能更直接但本文这套节点的架构思路完全通用接口换个名字而已。再说ROS。机载端跑Ubuntu系统用ROS来管理数据流几乎已经是行业默认做法。GPS数据给导航模块、障碍物感知模块、地面站通信模块都用得上而ROS的话题机制可以把GPS数据发布者和订阅者解耦各模块独立开发、独立运行后续加功能也方便。最后说CMake。ROS的catkin编译环境本质上是基于CMake的一层封装但一旦工程里同时要链接第三方库比如PSDK的静态库、动态库CMakeLists的写法和普通ROS工程就不一样了。这个项目中没有正确配置PSDK头文件路径、库文件路径编译就会报一堆fatal error: dji_xxx.h: No such file or directory或者链接时提示undefined reference。这一块也是很多人卡住的地方后面我会专门拆解。2. 环境准备与工程结构设计2.1 ROS环境与PSDK环境的准备项目环境是Ubuntu 20.04 ROS Noetic。如果你还在为装ROS发愁可以用鱼香ROS一键安装脚本我实际用过比手动敲命令省心很多一条命令就能把ROS环境、编译工具链和常用工具装好适合快速建开发环境。但生产环境我建议还是自己手动按官方步骤装一遍对系统依赖更可控出问题也知道去哪里排查。# 鱼香ROS一键安装适合快速体验和开发调试 wget http://fishros.com/install -O fishros . fishros装完ROS之后检查一下roscore能不能正常启动再确认echo $ROS_DISTRO输出的是noetic环境就算就绪了。PSDK这边需要准备的是PSDK的库文件和头文件。官方SDK解压后目录结构一般是psdk_lib/存放库文件包括libpsdk_lib.a或者.so文件psdk_inc/存放头文件比如psdk_aircraft_info.h、psdk_error.h这些sample/官方示例代码注意PSDK库通常要求C11及以上编译如果你的代码里用了C17特性也要保持整个工程编译标准统一不然新旧标准混用容易出隐藏的内存布局问题。2.2 工作空间与功能包结构设计在ROS里开发第一步要建工作空间。这个项目我建议把GPS节点单独做一个功能包不要和PSDK的示例代码混在同一个包里不然编译和依赖管理都会乱。mkdir -p ~/catkin_ws/src cd ~/catkin_ws/src catkin_init_workspace然后创建GPS发布功能包catkin_create_pkg gps_publisher roscpp sensor_msgs std_msgs工程目录结构如下gps_publisher/ ├── CMakeLists.txt ├── package.xml ├── include/ │ └── gps_publisher/ │ └── gps_converter.h ├── src/ │ ├── gps_publisher_node.cpp │ └── gps_converter.cpp ├── launch/ │ └── gps_publisher.launch └── config/ └── psdk_path.cmake把PSDK相关路径单独放在config/psdk_path.cmake里这样即使换一台机器、换一套PSDK库也不需要改动主CMakeLists只改这个配置文件就行。这是我在实际项目中养成的习惯多机联调、同事之间互相拉代码编译时这条路路径管理会省很多事。2.3 package.xml中的依赖声明package.xml是ROS包管理器的入口后续find_package、依赖安装都靠它。我需要把用到的roscpp、sensor_msgs、std_msgs都声明进去package format2 namegps_publisher/name version0.1.0/version descriptionGPS data publisher from PSDK to ROS/description maintainer emailyouexample.comdeveloper/maintainer licenseMIT/license buildtool_dependcatkin/buildtool_depend dependroscpp/depend dependsensor_msgs/depend dependstd_msgs/depend /package注意format2要写对否则新版ROS系统Noetic会提示兼容问题。3. GPS数据节点的实现细节3.1 从PSDK取GPS数据的正确姿势GPS数据在PSDK的接口体系里一般可以通过获取飞控信息相关的接口拿到。数据量包括经纬度、海拔、速度、卫星颗数、定位质量等。接口回调返回的数据结构通常是一个结构体类似psdk_gps_info_t。我的实现思路是启动一个独立线程定期调用PSDK的获取接口比如每秒10次也就是10Hz。为什么要独立线程因为PSDK的某些接口有阻塞耗时如果在ROS主线程里直接调用发布时间戳会抖动耦合度高后面导致整个节点不及时。实际测试中发现PSDK接口获取一次GPS数据最坏情况可能耗时几十毫秒如果直接放在回调函数里会导致话题发布频率不稳定用独立线程则可以很好地解决这个问题。关于单位需要特别留意PSDK返回的经纬度可能是度degree也可能是弧度radian不同版本、不同接口返回单位不一样。我在项目中第一次接数据的时候直接用原始值填充NavSatFix结果在Rviz里显示的经纬度完全偏到非洲去了查了半天才发现是单位换算问题。这里给出一段稳定的处理逻辑// 从PSDK获取GPS原始数据 psdk_gps_info_t gps_raw {}; psdk_get_gps_data(gps_raw); // 转换为ROS NavSatFix消息 sensor_msgs::NavSatFix gps_msg; gps_msg.header.stamp ros::Time::now(); gps_msg.header.frame_id gps_link; // 检查单位如果接口返回的是弧度需要转成度 if (gps_raw.coordinate_unit PSDK_GPS_COORD_UNIT_RADIAN) { gps_msg.latitude gps_raw.latitude * 180.0 / M_PI; gps_msg.longitude gps_raw.longitude * 180.0 / M_PI; } else { gps_msg.latitude gps_raw.latitude; gps_msg.longitude gps_raw.longitude; } gps_msg.altitude gps_raw.altitude; // GPS状态填充 if (gps_raw.fix_status 1) { gps_msg.status.status sensor_msgs::NavSatStatus::STATUS_FIX; gps_msg.status.service sensor_msgs::NavSatStatus::SERVICE_GPS; } else { gps_msg.status.status sensor_msgs::NavSatStatus::STATUS_NO_FIX; } gps_msg.position_covariance_type sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN; gps_pub.publish(gps_msg);这里coordinate_unit字段是我项目里的自定义字段实际使用PSDK时你可以用宏定义或者ifdef来处理不同版本的单位差异。重点是这个换算逻辑必须有不要嫌啰嗦。3.2 话题设计与发布时间戳处理话题名我用的是psdk/gps消息类型是sensor_msgs/NavSatFix。选择这个类型的原因很简单它是ROS标准消息里专门用来表示全球定位系统GNSS数据的类型下游的robot_localization、move_base、rtabmap等模块直接就能订阅使用不需要自己再写转换层。如果自己定义一个消息类型后续每次对接新模块都要再写一个bridge节点维护成本成倍增加。另一个关键细节是时间戳。GPS设备的数据本身有时间延迟有些型号的GPS芯片输出的时间戳和系统时间不同步我这里的做法是直接用ros::Time::now()作为消息时间戳同时开启use_sim_time时要注意如果当前是零仿真时间时间戳会出现跳变。如果你对接的是仿真环境比如Gazebo要确认话题发布器启动在/use_sim_time为true之前否则容易出现时间戳为0的问题。3.3 帧ID与坐标系的约定header.frame_id建议统一叫gps_link或者gps然后在TF树里建立一个从gps_link到base_link的静态变换。原因在于GPS数据本身是ENU东-北-天坐标系下的位置如果你直接把GPS话题挂到base_link上下游做坐标变换时会忽略GPS天线相对机体中心的位置偏移导致融合定位结果出现几厘米到几十厘米的系统性误差。添加静态TF变换rosrun tf2_ros static_transform_publisher 0 0 0.3 0 0 0 base_link gps_link这里假设GPS天线在机体中心正上方0.3米处。如果你实际装在机臂或者云台附近把这三个偏移量换成实际测量值即可。3.4 经纬度转高德坐标的拓展处理这个项目后面遇到一个实际需求无人机的GPS坐标要在自己写的地面站页面上显示页面用的地图是高德地图需要把WGS-84坐标转成GCJ-02也就是国内使用的火星坐标。这个转换必须经过偏移算法不能简单加减一个常数。实际项目中我写了一个轻量的转换函数直接放在gps_converter.cpp里#include cmath const double PI 3.14159265358979324; void wgs84_to_gcj02(double wgs_lat, double wgs_lng, double gcj_lat, double gcj_lng) { double a 6378245.0; double ee 0.00669342162296594323; double dLat transformLat(wgs_lng - 105.0, wgs_lat - 35.0); double dLon transformLng(wgs_lng - 105.0, wgs_lat - 35.0); double radLat wgs_lat / 180.0 * PI; double magic sin(radLat); magic 1 - ee * magic * magic; double sqrtMagic sqrt(magic); dLat (dLat * 180.0) / ((a * (1 - ee)) / (magic * sqrtMagic) * PI); dLon (dLon * 180.0) / (a / sqrtMagic * cos(radLat) * PI); gcj_lat wgs_lat dLat; gcj_lng wgs_lng dLon; }两个子函数transformLat和transformLng是官方坐标系转换算法的拆分网上有大量现成实现细节这里不展开。这段代码的实际意义在于ROS内部统一用WGS-84坐标只在展示层或者上报层做转换不要污染内部的定位计算流程。4. CMake配置的实战拆解4.1 CMakeLists的核心写法CMakeLists是整个编译过程的灵魂。一个同时依赖ROS和PSDK的节点CMake配置要兼顾两种情况一是通过find_package(catkin)找到ROS的包二是通过自定义变量找到PSDK的头文件和库文件。我的最终版CMakeLists核心结构如下cmake_minimum_required(VERSION 3.0.2) project(gps_publisher) # 引入PSDK路径配置路径集中管理 include(${CMAKE_CURRENT_SOURCE_DIR}/config/psdk_path.cmake) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs std_msgs ) # PSDK库目录 set(PSDK_LIB_DIR ${PSDK_PATH}/psdk_lib) # PSDK头文件目录 set(PSDK_INC_DIR ${PSDK_PATH}/psdk_inc) catkin_package( INCLUDE_DIRS include LIBRARIES gps_publisher CATKIN_DEPENDS roscpp sensor_msgs std_msgs DEPENDS PSDK ) include_directories( include ${catkin_INCLUDE_DIRS} ${PSDK_INC_DIR} ) add_executable(gps_publisher_node src/gps_publisher_node.cpp src/gps_converter.cpp ) add_dependencies(gps_publisher_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ) target_link_libraries(gps_publisher_node ${catkin_LIBRARIES} ${PSDK_LIB_DIR}/libpsdk_lib.a )psdk_path.cmake内容很简单set(PSDK_PATH /home/user/psdk_development/psdk_3.0)这样换机器只需要改这个文件里的一行路径。4.2 常见编译错误与原因分析在实际编译过程中最容易遇到的问题有三大类。第一类是找不到头文件报错信息通常是fatal error: psdk_aircraft_info.h: No such file or directory原因很直接include_directories里没有加PSDK头文件目录或者路径写错了。检查的办法是在CMakeLists.txt里临时加一行message(STATUS PSDK_INC_DIR${PSDK_INC_DIR})重新编译输出路径看是否真实存在。第二类是链接失败报错信息类似undefined reference to psdk_get_gps_data这是最折磨人的问题。原因一般是target_link_libraries里没有包含PSDK库或者库的依赖没有找到。PSDK的静态库常常依赖其他系统库比如libpthread.so、libdl.so所以链接时还要额外加target_link_libraries(gps_publisher_node ${catkin_LIBRARIES} ${PSDK_LIB_DIR}/libpsdk_lib.a pthread dl )如果缺少pthread链接阶段会报很多undefined reference to pthread_create之类的错误。第三类是编译通过但运行时找不到库报错error while loading shared libraries: libpsdk_lib.so: cannot open shared object file如果你链接的是动态库需要把psdk_lib目录加入LD_LIBRARY_PATHexport LD_LIBRARY_PATH$LD_LIBRARY_PATH:/home/user/psdk_development/psdk_3.0/psdk_lib建议把这句话写到~/.bashrc里避免每次开终端都要手工执行。这个问题在机载电脑上特别坑因为重启后环境变量丢失节点启动就是报错找不到库。4.3 catkin_make与catkin build的选择ROS早期习惯用catkin_make但Noetic已经完全支持catkin build后者在编译多个功能包时增量编译更快依赖关系处理也更清晰。实际使用中我并不建议在同一个工作空间混用两者因为它们生成的devel目录结构不同混用时source完了容易出现package not found的诡异问题。我一般这样编译cd ~/catkin_ws catkin build gps_publisher -j4 source devel/setup.bash-j4限制编译并行度避免机载计算机内存不够导致编译时卡死。妙算3这类设备内存有限编译大型依赖时小心一点。5. 常见问题与排查技巧实录5.1 GPS数据异常类问题GPS数据的异常挑几个真实遇到的说一说。第一个是GPS周数翻转问题。2020年前后旧款GPS模块因为周数翻转GPS周数计数器只有10位1024周归零一次导致日期跳变定位数据一会儿正常一会儿异常。这类问题表现非常诡异真实坐标明明没动GPS输出却会有几百米的跳变或者时间戳突然回到1999年。排查的手段是看GPS输出里的UTC时间字段如果日期明显不对基本可以断定是周数翻转。解决办法是把GPS模块固件升级到支持16位周数的版本或者给SDK打翻转补丁。第二个是GPS误差偏大定位漂移严重。民用GPS本身就有几米到十几米的随机误差在城市峡谷、厂房附近多径效应会把误差进一步放大。仿真和真机不同静态悬停时GPS点飘来飘去是正常的别慌。如果你需要更高的精度可以考虑板载RTK模块或者在算法层做滤波融合。我曾经在一个项目里把GPS数据和IMU做EKF融合静态漂移从5米减小到1米以内。第三个是经纬度单位异常。这个前面提过接口给的是弧度直接当度使用会导致定位偏到离谱的位置。排查技巧是把话题值打印出来和手机GPS对比。如果你在北京打印的纬度是0.65左右那是弧度没转成度北京的纬度约39.9度转弧度大约是0.696一眼就能看出来。5.2 编译与运行问题速查表我把实际项目里遇到的编译和运行问题做了一个速查表方便你对照排查。现象可能原因解决办法找不到psdk_*.h头文件路径未配置检查include_directories确认路径存在undefined reference未链接PSDK库或系统库添加${PSDK_LIB_DIR}/libpsdk_lib.a、pthread、dl运行时找不到.so动态库路径不在LD_LIBRARY_PATHexport LD_LIBRARY_PATH...并写入~/.bashrcpackage not foundsource了错误的devel目录统一使用catkin_make或catkin build不要混用运行rosrun找不到包未source devel/setup.bash编译后先source再运行时间戳一直为0/use_sim_time开启但无仿真时间发布器启动前关闭use_sim_time或设置仿真时间源GPS输出出现明显跳变GPS周数翻转或固件版本过旧升级GPS模块固件打翻转补丁5.3 真实场景的调试记录我记得有一次调试地面站显示的轨迹总是有一个固定的偏移大概向东偏了80米。一开始怀疑是GPS误差因为80米超出民用GPS的正常误差范围。后来排查发现是PSDK数据里的经纬度字段在SDK内部做了高斯投影转换而我没有反算回去直接把投影坐标当经纬度用了。那次的排查过程花了一个下午。最后锁定原因的方式很笨把PSDK原始数据用串口打印出来和飞控自己的调参软件里显示的经纬度对比发现原始数据的数值量级明显不对然后再翻SDK文档才发现接口的返回值是经过某种转换后的坐标。从那以后我拿到任何GPS数据第一件事永远是打印原始值和已知参考点对一下量级再做后续处理。这个项目的完整链路跑通后我实测GPS话题发布的频率稳定在10Hz左右话题延迟在可接受范围内。CMake配置理顺之后整个编译过程只需要一条命令非常干净。最后再分享一个后期扩展的小技巧。如果你想把GPS数据录下来回放调试用rosbag record /psdk/gps直接录制即可调试的时候用rosbag play回放不需要再拿着飞机跑到户外。配合Rviz里的NavSatFix显示插件可以很直观地看到轨迹回放。这些工具组合起来整个GPS数据链路的开发和调试效率会高很多。