MAVROS GPS Listener 📸 无人机传感器数据采集与图像元数据写入

📖 概述

MAVROS GPS Listener 是一个基于ROS的无人机数据采集功能包，专为无人机航测、视觉定位、数据归档等场景设计。核心功能是订阅MAVROS发布的传感器话题和相机图像话题，当用户调用服务时，捕获当前图像帧，并将实时传感器数据（GPS坐标、姿态角、位置速度等）写入图像的元数据中（EXIF/XMP）。

✨ 核心特性

• 多话题订阅 — GPS Fix、GPS RAW、IMU、本地位置/速度
• 图像捕获服务 — 通过ROS服务触发，支持自定义图片名称和路径
• 元数据写入 — GPS数据写入EXIF标准标签，扩展数据写入UserComment(JSON)和XMP双备份
• 实时数据采集 — 每次服务调用时捕获最新传感器数据和图像帧
• 线程安全 — 使用互斥锁保护共享数据
• 灵活配置 — 支持launch文件配置图像话题和保存路径
• 跨平台查看 — 提供Python脚本和PyQt5可视化工具，支持Windows/Linux
• 完善的错误处理 — 即使XMP写入失败，图像和JSON数据仍可保存

⚡ 快速开始

# 终端1: 启动MAVROS
roslaunch mavros px4.launch fcu_url:=/dev/ttyACM0:921600

# 终端2: 启动相机驱动（根据实际相机）
roslaunch usb_cam usb_cam-test.launch

# 终端3: 启动GPS监听节点
roslaunch mavros_gps_listener gps_listener.launch

# 终端4: 调用服务捕获图像
rosservice call /capture_image "image_name: 'flight_001' image_path: ''"

# 查看结果
exiv2 -p e ~/gps_ws/images/flight_001.jpg | grep GPS

🏗️ 架构设计

功能包采用模块化设计，主要包含以下组件：

• GPSListener — 主节点类，负责订阅话题、管理共享数据、提供服务
• GPSMetadataWriter — 元数据写入类，封装Exiv2库操作，支持EXIF/XMP/UserComment
• SensorData — 传感器数据结构体，存储GPS、姿态、位置等所有数据
• CaptureImage.srv — 服务定义，包含请求/响应结构

📡 ROS话题订阅

节点默认订阅以下话题（可通过launch文件重映射）：

🛠️ 服务接口

请求参数

• string image_name — 图片名称（不含路径，可选，不提供则使用时间戳）
• string image_path — 完整保存路径（可选，优先级高于image_name）

响应参数

• bool success — 是否成功
• string message — 响应消息
• string saved_image_path — 实际保存的图片路径
• int32 capture_time — 捕获时间戳（秒）

# 调用示例
rosservice call /capture_image "image_name: 'waypoint_001' image_path: ''"
# 返回: success: True, saved_image_path: "/home/amov/gps_ws/images/waypoint_001.jpg"

📸 图像元数据结构

保存的图像包含以下元数据：

EXIF GPS标签（标准GPS数据）

• Exif.GPSInfo.GPSLatitude — 纬度（度分秒有理数）
• Exif.GPSInfo.GPSLatitudeRef — 纬度参考（N/S）
• Exif.GPSInfo.GPSLongitude — 经度（度分秒有理数）
• Exif.GPSInfo.GPSLongitudeRef — 经度参考（E/W）
• Exif.GPSInfo.GPSAltitude — 海拔（有理数）
• Exif.GPSInfo.GPSAltitudeRef — 海拔参考
• Exif.GPSInfo.GPSTimeStamp — GPS时间
• Exif.GPSInfo.GPSDateStamp — GPS日期

UserComment JSON（扩展传感器数据）

{
  "latitude": 39.9042,
  "longitude": 116.4074,
  "altitude": 50.5,
  "roll": 0.04999,
  "pitch": -0.02356,
  "yaw": 2.79048,
  "roll_rate": 0.00191,
  "pitch_rate": 0.00093,
  "yaw_rate": -0.00026,
  "position_x": 10.5,
  "position_y": 20.3,
  "position_z": 50.2,
  "velocity_x": 0.5,
  "velocity_y": 0.3,
  "velocity_z": 0.1,
  "gps_velocity_x": 0.52,
  "gps_velocity_y": 0.31,
  "satellites_visible": 12,
  "fix_type": 3,
  "hdop": 1.2,
  "vdop": 1.5,
  "timestamp": 1774250591.408
}

XMP标签（可选，双备份）

🚀 使用指南

1. 创建功能包

cd ~/catkin_ws/src
catkin_create_pkg mavros_gps_listener roscpp sensor_msgs geometry_msgs mavros_msgs cv_bridge image_transport

2. 编译

cd ~/catkin_ws
catkin_make
source devel/setup.bash

3. 配置launch文件

<launch>
    <arg name="image_topic" default="/uav1/camera/image_raw" />
    <arg name="save_path" default="/home/amov/gps_ws/images" />
    <node name="gps_listener_node" pkg="mavros_gps_listener" type="gps_listener_node" output="screen">
        <param name="image_topic" value="$(arg image_topic)" />
        <param name="save_path" value="$(arg save_path)" />
    </node>
</launch>

4. 运行

roslaunch mavros_gps_listener gps_listener.launch

5. 调用服务

rosservice call /capture_image "image_name: 'test' image_path: ''"

📌 示例程序

🐍 Python数据读取脚本

#!/usr/bin/env python3
import exiv2
import json

def read_image_metadata(image_path):
    image = exiv2.ImageFactory.open(image_path)
    image.readMetadata()
    
    # 读取GPS
    exif = image.exifData()
    if 'Exif.GPSInfo.GPSLatitude' in exif:
        print(f"GPS: {exif['Exif.GPSInfo.GPSLatitude']}")
    
    # 读取UserComment JSON
    if 'Exif.Photo.UserComment' in exif:
        comment = exif['Exif.Photo.UserComment'].toString()
        json_start = comment.find('{')
        if json_start != -1:
            data = json.loads(comment[json_start:])
            print(f"Roll: {data['roll']:.2f} rad")
            print(f"Pitch: {data['pitch']:.2f} rad")
            print(f"Yaw: {data['yaw']:.2f} rad")

if __name__ == '__main__':
    read_image_metadata('capture_20260323_152311.jpg')

📊 数据可视化工具

提供基于PyQt5 + Matplotlib的可视化工具，支持：

• 图像缩略图浏览
• 传感器数据表格展示
• 姿态仪表盘（Roll/Pitch/Yaw可视化）
• 姿态变化曲线（随时间变化）
• 位置轨迹（X-Y平面）
• GPS轨迹（经纬度地图）

python3 viewer.py
# 选择包含图片的文件夹，自动加载所有图片元数据

⚙️ 配置参数

📦 编译与依赖

依赖项

• ROS Melodic/Noetic
• OpenCV (≥3.2)
• Exiv2 (≥0.27)
• Eigen3 (≥3.3)
• cv_bridge, image_transport

# Ubuntu 安装依赖
sudo apt install ros-${ROS_DISTRO}-mavros ros-${ROS_DISTRO}-mavros-extras
sudo apt install ros-${ROS_DISTRO}-cv-bridge ros-${ROS_DISTRO}-image-transport
sudo apt install libopencv-dev libeigen3-dev libexiv2-dev

# 编译
cd ~/catkin_ws
catkin_make --only-pkg-with-deps mavros_gps_listener

🔧 常见问题

⚖️ 许可证