flowzl/flyLink
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

v4l2 version

Browse Source
This commit is contained in:
flower
2026-02-02 13:35:52 +08:00
parent 47016c00a6
commit 977a99f9d1
25 changed files with 309 additions and 2008 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
gitignore
View File

@@ -2,6 +2,8 @@
# Gradle files # Gradle files
.gradle/ .gradle/
build/ build/
install/
log/
*/build/ */build/
log/ log/
# Local configuration file (sdk path, etc) # Local configuration file (sdk path, etc)

6
skylink_ros2_ws/build.sh
View File

@@ -3,6 +3,10 @@
# SkyLink ROS 2 机载端 - 快速编译脚本 # SkyLink ROS 2 机载端 - 快速编译脚本
# 用法: ./build.sh [release|debug] # 用法: ./build.sh [release|debug]
# 加载git分支
# 如果是python分支，则加载python相关依赖
# 如果是cpp分支，则加载cpp相关依赖
set -e set -e
SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
@@ -65,4 +69,6 @@ echo " source $SCRIPT_DIR/install/setup.bash"
echo "" echo ""
echo "2. 启动节点:" echo "2. 启动节点:"
echo " ros2 launch skylink_bridge bridge.launch.py" echo " ros2 launch skylink_bridge bridge.launch.py"
echo "python 版本节点启动命令:"
echo " ros2 launch skylink_bridge_python bridge.launch.py"
echo "" echo ""

156
skylink_ros2_ws/src/skylink_bridge/CMakeLists.txt
View File

@@ -1,156 +0,0 @@
cmake_minimum_required(VERSION 3.16)
project(skylink_bridge LANGUAGES CXX)
# ============================================================================
# 编译标准和优化选项
# ============================================================================
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra")
# 如果未指定编译类型，默认为 Release
if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE Release CACHE STRING "Build type" FORCE)
endif()
# ============================================================================
# 查找依赖
# ============================================================================
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(image_transport REQUIRED)
find_package(mavros_msgs REQUIRED)
# OpenCV
find_package(OpenCV REQUIRED COMPONENTS core imgproc)
# JPEG 库
find_package(JPEG REQUIRED)
# 线程库
find_package(Threads REQUIRED)
# ============================================================================
# 包含目录
# ============================================================================
include_directories(
include
)
# ============================================================================
# 源文件
# ============================================================================
set(SOURCES
src/udp_sender_node.cpp
src/camera_driver_node.cpp
src/device_info_sender_node.cpp
)
set(HEADERS
include/skylink_bridge/udp_sender_node.h
include/skylink_bridge/device_info_sender_node.h
include/protocol.h
)
# ============================================================================
# 创建节点可执行文件
# ============================================================================
add_executable(udp_sender_node src/udp_sender_node.cpp ${HEADERS})
add_executable(camera_driver_node src/camera_driver_node.cpp)
add_executable(device_info_sender_node src/device_info_sender_node.cpp)
# 链接库 - UDP Sender
target_link_libraries(udp_sender_node
${rclcpp_LIBRARIES}
${sensor_msgs_LIBRARIES}
${geometry_msgs_LIBRARIES}
${cv_bridge_LIBRARIES}
${image_transport_LIBRARIES}
${mavros_msgs_LIBRARIES}
${OpenCV_LIBRARIES}
${JPEG_LIBRARIES}
Threads::Threads
)
# 链接库 - Camera Driver
target_link_libraries(camera_driver_node
${rclcpp_LIBRARIES}
${sensor_msgs_LIBRARIES}
Threads::Threads
)
target_link_libraries(device_info_sender_node
${rclcpp_LIBRARIES}
Threads::Threads
)
# 目标级别的包含目录
ament_target_dependencies(udp_sender_node
rclcpp
sensor_msgs
geometry_msgs
cv_bridge
image_transport
mavros_msgs
)
ament_target_dependencies(camera_driver_node
rclcpp
sensor_msgs
)
ament_target_dependencies(device_info_sender_node
rclcpp
)
# ============================================================================
# 安装规则
# ============================================================================
# 安装可执行文件
install(TARGETS udp_sender_node camera_driver_node
DESTINATION lib/${PROJECT_NAME}
)
install(TARGETS device_info_sender_node
DESTINATION lib/${PROJECT_NAME}
)
# 安装启动文件
install(DIRECTORY launch/
DESTINATION share/${PROJECT_NAME}/launch
)
# 安装配置文件
install(DIRECTORY config/
DESTINATION share/${PROJECT_NAME}/config
)
# 安装头文件
install(DIRECTORY include/
DESTINATION include
)
# ============================================================================
# Ament 导出
# ============================================================================
ament_export_dependencies(
rclcpp
sensor_msgs
geometry_msgs
cv_bridge
image_transport
mavros_msgs
OpenCV
)
ament_package()

171
skylink_ros2_ws/src/skylink_bridge/include/protocol.h
View File

@@ -1,171 +0,0 @@
/**
* @file protocol.h
* @brief SkyLink 多平台实时视频传输 - 公共协议定义
*
* 这个文件定义了机载端（ROS 2）、PC 端（Qt）和移动端（Android）之间的通信协议。
* 必须保持三端的字节对齐和字段顺序一致。
*
* @author flowzl
* @date 2026-01-19
*/
#pragma once
#include <cstdint>
#include <cstring>
/**
* @brief 强制1字节对齐，避免编译器补齐导致的大小差异
*/
#pragma pack(push, 1)
/**
* @struct PacketHeader
* @brief UDP 数据包头部结构
*
* 总大小: 50 字节
* - 报头识别: 2 字节 (Magic)
* - 帧标识: 4 字节 (FrameID)
* - 分片信息: 4 字节 (TotalChunks + ChunkIndex)
* - 数据长度: 2 字节 (DataLen)
* - 时间戳: 8 字节 (Timestamp)
* - GPS 位置: 24 字节 (Lat, Lon, Alt)
* - 预留: 6 字节 (Reserve 用于将来扩展)
*/
struct PacketHeader {
// ============ 包头识别 ============
uint16_t magic; ///< 魔数 0xAA55 用于识别合法数据包
// ============ 帧标识 ============
uint32_t frame_id; ///< 唯一的帧编号，从 0 开始递增，用于帧同步
// ============ 分片信息 ============
uint16_t total_chunks; ///< 该帧总共被分成多少个 UDP 包
uint16_t chunk_index; ///< 当前数据包是第几个分片 (0-based)
// ============ 载荷信息 ============
uint16_t data_len; ///< 当前包的有效载荷长度（单位：字节）
// ============ 时间戳 ============
double timestamp; ///< GPS 时间戳 (秒数，Unix 时间 或 ROS 时间)
// ============ GPS 位置数据 ============
double lat; ///< 纬度 (WGS84 坐标系)
double lon; ///< 经度 (WGS84 坐标系)
double alt; ///< 海拔高度 (米)
// ============ 校验与预留 ============
uint16_t crc16; ///< CRC16 校验码 (可选，当前未使用)
uint32_t reserve; ///< 保留字段，用于将来的扩展
/**
* @brief 获取包头大小（字节数）
* @return 包头固定大小：50 字节
*/
static constexpr uint16_t size() {
return sizeof(PacketHeader);
}
};
/**
* @brief 设备信息包头结构（UDP 10000）
*/
struct DeviceInfoPacket {
uint16_t magic; ///< 魔数 0xAA56
uint32_t sequence; ///< 序列号
uint16_t width; ///< 图像宽度
uint16_t height; ///< 图像高度
float framerate; ///< 帧率
uint32_t pixel_format; ///< 像素格式（FourCC）
char device_path[32]; ///< 设备路径（/dev/videoX）
char device_name[64]; ///< 设备名称
float current_fps; ///< 当前实际 FPS
uint32_t total_frames; ///< 总帧数
uint64_t timestamp; ///< 时间戳（毫秒）
uint16_t crc16; ///< CRC 校验（可选）
static constexpr uint16_t size() {
return sizeof(DeviceInfoPacket);
}
};
/**
* @brief 恢复默认的对齐方式
*/
#pragma pack(pop)
// ============================================================================
// 协议常量定义
// ============================================================================
/** @brief 数据包魔数，用于识别合法的 SkyLink 数据包 */
constexpr uint16_t SKYLINK_MAGIC = 0xAA55;
/** @brief 设备信息魔数，用于识别设备信息数据包 */
constexpr uint16_t DEVICE_INFO_MAGIC = 0xAA56;
/** @brief 设备信息请求魔数，用于请求立即发送设备信息 */
constexpr uint16_t DEVICE_INFO_REQUEST = 0xAA57;
/** @brief 最大的单个 UDP 包大小 (包含报头) - MTU 1500 字节 */
constexpr uint16_t MAX_UDP_PACKET_SIZE = 1472; // 1500 - 20 (IP) - 8 (UDP)
/** @brief 最大的载荷大小 */
constexpr uint16_t MAX_PAYLOAD_SIZE = MAX_UDP_PACKET_SIZE - PacketHeader::size();
/** @brief UDP 接收缓冲区大小 */
constexpr uint32_t RX_BUFFER_SIZE = 65536;
/** @brief 最大支持的分片数 */
constexpr uint16_t MAX_CHUNKS = 1000;
/** @brief 帧超时时间（毫秒），超过此时间的分片将被丢弃 */
constexpr uint32_t FRAME_TIMEOUT_MS = 5000;
// ============================================================================
// 图像编码相关常量
// ============================================================================
/** @brief JPEG 图像格式 ID */
constexpr uint8_t IMAGE_FORMAT_JPEG = 0x01;
/** @brief JPEG 最大压缩质量 */
constexpr uint8_t JPEG_MAX_QUALITY = 100;
/** @brief JPEG 最小压缩质量 */
constexpr uint8_t JPEG_MIN_QUALITY = 10;
// ============================================================================
// 辅助函数
// ============================================================================
/**
* @brief 验证包头魔数是否合法
* @param header 待验证的包头结构
* @return 如果魔数为 0xAA55 返回 true，否则返回 false
*/
inline bool isValidPacket(const PacketHeader& header) {
return header.magic == SKYLINK_MAGIC;
}
/**
* @brief 计算 CRC16 校验码 (可选)
* @param data 数据指针
* @param len 数据长度
* @return 16 位 CRC 校验结果
*/
inline uint16_t crc16_ccitt(const uint8_t* data, uint16_t len) {
uint16_t crc = 0xFFFF;
for (uint16_t i = 0; i < len; i++) {
crc ^= (uint16_t)data[i] << 8;
for (int j = 0; j < 8; j++) {
if (crc & 0x8000) {
crc = (crc << 1) ^ 0x1021;
} else {
crc = crc << 1;
}
crc &= 0xFFFF;
}
}
return crc;
}

58
skylink_ros2_ws/src/skylink_bridge/include/skylink_bridge/device_info_sender_node.h
View File

@@ -1,58 +0,0 @@
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <atomic>
#include <netinet/in.h>
#include <string>
#include <thread>
#include "protocol.h"
/**
* @brief 设备信息发送节点
*
* 通过 UDP 端口 10000 定期广播设备信息，并响应请求包（DEVICE_INFO_REQUEST）。
*/
class DeviceInfoSenderNode : public rclcpp::Node {
public:
DeviceInfoSenderNode();
~DeviceInfoSenderNode();
private:
void load_parameters();
void start_listener();
void stop_listener();
void broadcast_timer_callback();
void handle_request_loop();
void send_device_info(const sockaddr_in& addr);
DeviceInfoPacket build_packet();
static uint64_t htonll(uint64_t value);
static uint32_t fourcc_from_string(const std::string& fmt);
private:
// 套接字
int udp_socket_{-1};
std::atomic<bool> running_{false};
std::thread listener_thread_;
// 参数
uint16_t info_port_{10000};
std::string target_ip_{"255.255.255.255"};
bool enable_broadcast_{true};
bool enable_request_response_{true};
int broadcast_interval_ms_{1000};
// 设备信息
std::string device_path_{"/dev/video0"};
std::string device_name_{"Unknown"};
uint16_t width_{0};
uint16_t height_{0};
float framerate_{0.0f};
uint32_t pixel_format_{0};
float current_fps_{0.0f};
uint32_t total_frames_{0};
uint32_t sequence_{0};
rclcpp::TimerBase::SharedPtr broadcast_timer_;
};

237
skylink_ros2_ws/src/skylink_bridge/include/skylink_bridge/udp_sender_node.h
View File

@@ -1,237 +0,0 @@
/**
* @file udp_sender_node.hpp
* @brief SkyLink ROS 2 UDP 发送节点 - 头文件
*
* 这个节点订阅相机和 GPS Topic，将图像编码为 JPEG 并通过 UDP 广播发送。
*
* @author flowzl
* @date 2026-01-19
*/
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/compressed_image.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <mavros_msgs/msg/global_position_target.hpp>
#include <sensor_msgs/msg/nav_sat_fix.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <vector>
#include <queue>
#include <mutex>
#include <condition_variable>
#include <thread>
#include <atomic>
#include <cstring>
// 协议定义
#include "protocol.h"
/**
* @class UdpSenderNode
* @brief ROS 2 UDP 发送节点
*
* 负责：
* 1. 订阅相机和 GPS 数据
* 2. 编码图像为 JPEG
* 3. 分片打包为 UDP 数据包
* 4. 通过网络发送数据包
*/
class UdpSenderNode : public rclcpp::Node {
public:
/**
* @brief 构造函数
*/
UdpSenderNode();
/**
* @brief 析构函数
*/
~UdpSenderNode();
private:
// ========================================================================
// ROS 2 订阅和发布
// ========================================================================
/** 相机图像订阅 */
rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr camera_subscriber_;
/** GPS 全球位置订阅 */
rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr gps_subscriber_;
// ========================================================================
// 参数
// ========================================================================
std::string target_ip_; ///< 目标 IP 地址
uint16_t target_port_; ///< 目标 UDP 端口
std::string camera_topic_; ///< 相机 Topic 名称
std::string gps_topic_; ///< GPS Topic 名称
int jpeg_quality_; ///< JPEG 压缩质量 (1-100)
bool adaptive_quality_; ///< 自适应质量标志
int quality_min_, quality_max_; ///< 质量范围
bool enable_gps_; ///< 是否启用 GPS
uint16_t max_packet_size_; ///< 最大 UDP 包大小
uint32_t frame_timeout_ms_; ///< 帧超时时间
bool enable_fps_stats_; ///< FPS 统计
bool enable_drop_stats_; ///< 丢弃帧统计
// ========================================================================
// 套接字和网络
// ========================================================================
int udp_socket_; ///< UDP 套接字文件描述符
uint32_t frame_counter_; ///< 帧计数器
std::atomic<uint32_t> bytes_sent_{0}; ///< 已发送字节数
std::atomic<uint32_t> packets_sent_{0}; ///< 已发送包数
std::atomic<uint32_t> frames_dropped_{0}; ///< 丢弃的帧数
// ========================================================================
// 异步发送队列和线程
// ========================================================================
/** UDP 数据包队列项 */
struct PacketQueueItem {
std::vector<uint8_t> data;
std::chrono::system_clock::time_point enqueue_time;
};
std::queue<PacketQueueItem> send_queue_; ///< 发送队列
std::mutex send_queue_mutex_; ///< 队列互斥锁
std::condition_variable send_queue_cv_; ///< 队列条件变量
std::thread sender_thread_; ///< 后台发送线程
std::atomic<bool> sender_running_{true}; ///< 线程运行标志
static const size_t MAX_QUEUE_SIZE = 5000; ///< 队列最大大小
static const size_t BATCH_SEND_SIZE = 128; ///< 批量发送大小 (增加以减少锁竞争)
/**
* @brief 后台发送工作函数
*/
void sender_worker();
/**
* @brief 将数据包加入发送队列
* @param data 数据指针
* @param len 数据长度
*/
void enqueue_packet(const uint8_t* data, uint16_t len);
/**
* @brief 批量将数据包加入发送队列
* @param packets 数据包列表 (将被移动)
*/
void enqueue_packets(std::vector<std::vector<uint8_t>>& packets);
// ========================================================================
// GPS 数据缓存
// ========================================================================
struct {
double latitude = 39.9042; ///< 纬度
double longitude = 116.4074; ///< 经度
double altitude = 100.0; ///< 海拔
std::mutex mutex;
} current_gps_data_;
// ========================================================================
// 统计信息
// ========================================================================
struct {
uint32_t total_frames = 0;
uint32_t total_bytes = 0;
uint32_t total_packets = 0;
std::chrono::system_clock::time_point last_report_time;
} stats_;
std::mutex stats_mutex_;
// ========================================================================
// 回调函数
// ========================================================================
/**
* @brief 相机图像回调
*/
void camera_callback(const sensor_msgs::msg::CompressedImage::SharedPtr msg);
/**
* @brief GPS 回调
* @param msg GPS 消息
*/
void gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg);
// ========================================================================
// 网络函数
// ========================================================================
/**
* @brief 初始化 UDP 套接字
* @return 成功返回 true
*/
bool init_udp_socket();
/**
* @brief 关闭 UDP 套接字
*/
void close_udp_socket();
/**
* @brief 发送 UDP 数据包
* @param data 数据指针
* @param len 数据长度
* @return 发送的字节数，失败返回 -1
*/
int send_udp_packet(const uint8_t* data, uint16_t len);
// ========================================================================
// 图像处理函数
// ========================================================================
/**
* @brief 将 OpenCV Mat 编码为 JPEG
* @param frame OpenCV 图像
* @param quality 压缩质量
* @param output 输出 JPEG 数据
* @return 成功返回 true
*/
bool encode_to_jpeg(const cv::Mat& frame, int quality, std::vector<uint8_t>& output);
/**
* @brief 将 JPEG 数据分片为 UDP 包并发送
* @param jpeg_data JPEG 数据指针
* @param jpeg_len JPEG 数据长度
* @param timestamp 时间戳
* @param lat 纬度
* @param lon 经度
* @param alt 海拔
* @return 发送的包数
*/
uint16_t fragment_and_send(
const uint8_t* jpeg_data,
uint32_t jpeg_len,
double timestamp,
double lat,
double lon,
double alt
);
// ========================================================================
// 统计和监控
// ========================================================================
/**
* @brief 更新统计信息
* @param bytes_count 新增字节数
* @param packet_count 新增包数
*/
void update_stats(uint32_t bytes_count, uint32_t packet_count);
/**
* @brief 定期报告统计信息
*/
void report_stats();
};

41
skylink_ros2_ws/src/skylink_bridge/package.xml
View File

@@ -1,41 +0,0 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>skylink_bridge</name>
<version>1.0.0</version>
<description>
SkyLink 实时视频传输系统 - ROS 2 机载端
负责采集相机和 GPS 数据，通过 UDP 广播发送到地面站和移动端
</description>
<!-- 维护者信息 -->
<maintainer email="support@skylink.dev">flowzl</maintainer>
<license>MIT</license>
<!-- 作者 -->
<author email="dev@skylink.dev">SkyLink Development</author>
<!-- 编译工具 -->
<buildtool_depend>ament_cmake</buildtool_depend>
<!-- ROS 2 依赖 -->
<depend>rclcpp</depend>
<depend>sensor_msgs</depend>
<depend>geometry_msgs</depend>
<depend>cv_bridge</depend>
<depend>image_transport</depend>
<!-- 系统库依赖 -->
<exec_depend>opencv-dev</exec_depend>
<exec_depend>libjpeg-turbo-dev</exec_depend>
<!-- 可选：MAVROS GPS 支持 -->
<depend>mavros_msgs</depend>
<!-- 可选：USB 相机支持 -->
<exec_depend>usb_cam</exec_depend>
<!-- 导出信息 -->
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

494
skylink_ros2_ws/src/skylink_bridge/src/camera_driver_node.cpp
View File

@@ -1,494 +0,0 @@
/**
* @file camera_driver_node.cpp
* @brief V4L2 相机驱动节点 - 发布 ROS 2 CompressedImage 消息
*
* 使用 V4L2 直接读取 USB 摄像头 MJPEG 流，发布为 ROS 2 sensor_msgs/CompressedImage
*
* @author flowzl
* @date 2026-01-19
*/
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/compressed_image.hpp>
#include <thread>
#include <atomic>
#include <chrono>
#include <vector>
// V4L2 includes
#include <linux/videodev2.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <unistd.h>
#include <sys/stat.h>
/**
* @class CameraDriverNode
* @brief V4L2 相机驱动 ROS 2 节点 (Raw MJPEG)
*/
class CameraDriverNode : public rclcpp::Node {
public:
/**
* @brief 构造函数
*/
CameraDriverNode() : rclcpp::Node("camera_driver") {
// ====================================================================
// 声明参数
// ====================================================================
this->declare_parameter<std::string>("video_device", "/dev/video4");
this->declare_parameter<double>("framerate", 30.0);
this->declare_parameter<int>("image_width", 640);
this->declare_parameter<int>("image_height", 480);
this->declare_parameter<std::string>("camera_topic", "/camera/image_raw/compressed");
this->declare_parameter<std::string>("camera_frame", "camera");
this->declare_parameter<bool>("enable_fps_stats", true);
// 读取参数
video_device_ = this->get_parameter("video_device").as_string();
target_framerate_ = this->get_parameter("framerate").as_double();
image_width_ = this->get_parameter("image_width").as_int();
image_height_ = this->get_parameter("image_height").as_int();
camera_topic_ = this->get_parameter("camera_topic").as_string();
camera_frame_ = this->get_parameter("camera_frame").as_string();
enable_fps_stats_ = this->get_parameter("enable_fps_stats").as_bool();
// ====================================================================
// 初始化摄像头
// ====================================================================
RCLCPP_INFO(this->get_logger(), "======================================");
RCLCPP_INFO(this->get_logger(), "V4L2 Native Camera Driver Node 启动");
RCLCPP_INFO(this->get_logger(), "======================================");
RCLCPP_INFO(this->get_logger(), "设备: %s", video_device_.c_str());
RCLCPP_INFO(this->get_logger(), "分辨率: %dx%d", image_width_, image_height_);
RCLCPP_INFO(this->get_logger(), "帧率: %.1f", target_framerate_);
if (!init_camera()) {
RCLCPP_ERROR(this->get_logger(), "❌ 摄像头初始化失败");
rclcpp::shutdown();
return;
}
RCLCPP_INFO(this->get_logger(), "✓ 摄像头初始化成功");
// ====================================================================
// 创建发布者
// ====================================================================
// 修改为默认 QoS (Reliable, KeepLast=10) 以匹配 Python 行为和 ros2 topic hz 工具
// 之前使用 SensorDataQoS (BestEffort) 导致 ros2 topic hz (默认 Reliable) 无法正常接收
rclcpp::QoS qos_profile(10);
// 使用 CompressedImage 发布
image_publisher_ = this->create_publisher<sensor_msgs::msg::CompressedImage>(
camera_topic_,
qos_profile
);
RCLCPP_INFO(this->get_logger(), "✓ CompressedImage 发布器已创建: %s (Reliable)", camera_topic_.c_str());
// ====================================================================
// 启动捕获线程
// ====================================================================
capturing_ = true;
capture_thread_ = std::thread(&CameraDriverNode::capture_worker, this);
RCLCPP_INFO(this->get_logger(), "✓ 捕获线程已启动");
RCLCPP_INFO(this->get_logger(), "✓ 节点初始化完成");
RCLCPP_INFO(this->get_logger(), "======================================");
}
/**
* @brief 析构函数
*/
~CameraDriverNode() {
capturing_ = false;
if (capture_thread_.joinable()) {
capture_thread_.join();
}
stop_capturing();
uninit_device();
close_device();
RCLCPP_INFO(this->get_logger(), "Camera Driver Node 已关闭");
}
// 设备信息查询接口
const std::string& device_path() const { return video_device_; }
const std::string& device_name() const { return device_name_; }
uint16_t image_width() const { return static_cast<uint16_t>(image_width_); }
uint16_t image_height() const { return static_cast<uint16_t>(image_height_); }
float target_fps() const { return static_cast<float>(target_framerate_); }
uint32_t pixel_format_fourcc() const { return pixel_format_fourcc_; }
float current_fps() const { return current_fps_; }
uint32_t total_frames() const { return frame_counter_; }
private:
// ========================================================================
// V4L2 结构
// ========================================================================
struct Buffer {
void *start;
size_t length;
};
// ========================================================================
// 成员变量
// ========================================================================
// V4L2 members
int fd_ = -1;
std::vector<Buffer> buffers_;
/** 摄像头参数 */
std::string video_device_;
std::string device_name_{"Unknown"};
double target_framerate_;
int image_width_;
int image_height_;
std::string camera_topic_;
std::string camera_frame_;
bool enable_fps_stats_;
/** 发布者 */
rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr image_publisher_;
/** 捕获线程 */
std::thread capture_thread_;
std::atomic<bool> capturing_{false};
/** 统计信息 */
uint32_t frame_counter_ = 0;
std::chrono::system_clock::time_point last_stats_time_;
float current_fps_ = 0.0f;
uint32_t pixel_format_fourcc_ = V4L2_PIX_FMT_MJPEG;
// ========================================================================
// 私有方法
// ========================================================================
// xioctl helper
static int xioctl(int fh, int request, void *arg) {
int r;
do {
r = ioctl(fh, request, arg);
} while (-1 == r && EINTR == errno);
return r;
}
/**
* @brief 关闭设备
*/
void close_device() {
if (fd_ != -1) {
close(fd_);
fd_ = -1;
}
}
/**
* @brief 释放内存映射
*/
void uninit_device() {
for (const auto& buffer : buffers_) {
if (-1 == munmap(buffer.start, buffer.length)) {
perror("munmap");
}
}
buffers_.clear();
}
/**
* @brief 停止流
*/
void stop_capturing() {
if (fd_ != -1) {
enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
xioctl(fd_, VIDIOC_STREAMOFF, &type);
}
}
/**
* @brief 初始化摄像头
*/
bool init_camera() {
struct stat st;
if (-1 == stat(video_device_.c_str(), &st)) {
RCLCPP_ERROR(this->get_logger(), "无法识别 '%s': %d, %s", video_device_.c_str(), errno, strerror(errno));
return false;
}
if (!S_ISCHR(st.st_mode)) {
RCLCPP_ERROR(this->get_logger(), "%s 不是字符设备", video_device_.c_str());
return false;
}
fd_ = open(video_device_.c_str(), O_RDWR | O_NONBLOCK, 0);
if (-1 == fd_) {
RCLCPP_ERROR(this->get_logger(), "无法打开 '%s': %d, %s", video_device_.c_str(), errno, strerror(errno));
return false;
}
struct v4l2_capability cap;
if (-1 == xioctl(fd_, VIDIOC_QUERYCAP, &cap)) {
RCLCPP_ERROR(this->get_logger(), "不是 V4L2 设备");
return false;
}
if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
RCLCPP_ERROR(this->get_logger(), "不是视频捕获设备");
return false;
}
if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
RCLCPP_ERROR(this->get_logger(), "不支持流 I/O");
return false;
}
device_name_ = reinterpret_cast<const char*>(cap.card);
// 设置格式
struct v4l2_format fmt;
memset(&fmt, 0, sizeof(fmt));
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
fmt.fmt.pix.width = image_width_;
fmt.fmt.pix.height = image_height_;
fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG; // 强制 MJPEG
fmt.fmt.pix.field = V4L2_FIELD_ANY; // V4L2_FIELD_NONE? ANY common for USB cams
if (-1 == xioctl(fd_, VIDIOC_S_FMT, &fmt)) {
RCLCPP_ERROR(this->get_logger(), "设置像素格式失败");
return false;
}
// 检查实际设置的格式
if (-1 == xioctl(fd_, VIDIOC_G_FMT, &fmt)) {
RCLCPP_WARN(this->get_logger(), "无法获取实际格式");
} else {
char fourcc[5] = {0};
fourcc[0] = (fmt.fmt.pix.pixelformat) & 0xFF;
fourcc[1] = (fmt.fmt.pix.pixelformat >> 8) & 0xFF;
fourcc[2] = (fmt.fmt.pix.pixelformat >> 16) & 0xFF;
fourcc[3] = (fmt.fmt.pix.pixelformat >> 24) & 0xFF;
RCLCPP_INFO(this->get_logger(), "实际设置的格式: %s (0x%08X), 分辨率: %dx%d",
fourcc, fmt.fmt.pix.pixelformat,
fmt.fmt.pix.width, fmt.fmt.pix.height);
if (fmt.fmt.pix.pixelformat != V4L2_PIX_FMT_MJPEG) {
RCLCPP_WARN(this->get_logger(),
"⚠️ 设备不支持MJPEG格式！实际格式: %s，可能需要使用其他格式或软件编码",
fourcc);
}
pixel_format_fourcc_ = fmt.fmt.pix.pixelformat;
}
// 设置帧率
struct v4l2_streamparm streamparm;
memset(&streamparm, 0, sizeof(streamparm));
streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
streamparm.parm.capture.timeperframe.numerator = 1;
streamparm.parm.capture.timeperframe.denominator = (unsigned int)target_framerate_;
if (xioctl(fd_, VIDIOC_S_PARM, &streamparm) != -1) {
double actual_fps = (double)streamparm.parm.capture.timeperframe.denominator /
streamparm.parm.capture.timeperframe.numerator;
RCLCPP_INFO(this->get_logger(), "尝试设置FPS: %.1f, 实际FPS: %.1f", target_framerate_, actual_fps);
}
// 请求 buffers
struct v4l2_requestbuffers req;
memset(&req, 0, sizeof(req));
req.count = 4;
req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
req.memory = V4L2_MEMORY_MMAP;
if (-1 == xioctl(fd_, VIDIOC_REQBUFS, &req)) {
RCLCPP_ERROR(this->get_logger(), "VIDIOC_REQBUFS 失败");
return false;
}
if (req.count < 2) {
RCLCPP_WARN(this->get_logger(), "Buffer 数量过少: %d", req.count);
}
buffers_.resize(req.count);
for (size_t i = 0; i < req.count; ++i) {
struct v4l2_buffer buf;
memset(&buf, 0, sizeof(buf));
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
buf.index = i;
if (-1 == xioctl(fd_, VIDIOC_QUERYBUF, &buf)) {
RCLCPP_ERROR(this->get_logger(), "VIDIOC_QUERYBUF 失败");
return false;
}
buffers_[i].length = buf.length;
buffers_[i].start = mmap(NULL, buf.length,
PROT_READ | PROT_WRITE, MAP_SHARED,
fd_, buf.m.offset);
if (MAP_FAILED == buffers_[i].start) {
RCLCPP_ERROR(this->get_logger(), "mmap 失败");
return false;
}
}
RCLCPP_INFO(this->get_logger(), "实际设置 - 分辨率: %dx%d, MJPEG",
fmt.fmt.pix.width, fmt.fmt.pix.height);
// 开始流
for (size_t i = 0; i < buffers_.size(); ++i) {
struct v4l2_buffer buf;
memset(&buf, 0, sizeof(buf));
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
buf.index = i;
if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf)) {
RCLCPP_ERROR(this->get_logger(), "VIDIOC_QBUF 失败");
return false;
}
}
enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (-1 == xioctl(fd_, VIDIOC_STREAMON, &type)) {
RCLCPP_ERROR(this->get_logger(), "VIDIOC_STREAMON 失败");
return false;
}
return true;
}
/**
* @brief 捕获工作线程
*/
void capture_worker() {
RCLCPP_INFO(this->get_logger(), "📷 捕获线程开始工作 (V4L2)");
last_stats_time_ = std::chrono::system_clock::now();
uint32_t last_frame_counter = 0;
while (capturing_ && rclcpp::ok()) {
fd_set fds;
struct timeval tv;
int r;
FD_ZERO(&fds);
FD_SET(fd_, &fds);
tv.tv_sec = 2; // Timeout
tv.tv_usec = 0;
r = select(fd_ + 1, &fds, NULL, NULL, &tv);
if (-1 == r) {
if (EINTR == errno) continue;
RCLCPP_ERROR(this->get_logger(), "select error");
break;
}
if (0 == r) {
RCLCPP_WARN(this->get_logger(), "select timeout");
continue;
}
struct v4l2_buffer buf;
memset(&buf, 0, sizeof(buf));
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
if (-1 == xioctl(fd_, VIDIOC_DQBUF, &buf)) {
if (errno == EAGAIN) continue;
RCLCPP_ERROR(this->get_logger(), "VIDIOC_DQBUF error: %d, %s", errno, strerror(errno));
continue; // try next
}
// 数据在 buffers_[buf.index].start，长度 buf.bytesused
try {
// 验证MJPEG数据格式（每300帧检查一次）
if (frame_counter_ % 300 == 0 && buf.bytesused >= 2) {
const uint8_t* data_ptr = (const uint8_t*)buffers_[buf.index].start;
RCLCPP_INFO(this->get_logger(),
"V4L2 MJPEG数据检查 - 大小: %u bytes, 前2字节: 0x%02X 0x%02X",
buf.bytesused, data_ptr[0], data_ptr[1]);
}
auto msg = std::make_unique<sensor_msgs::msg::CompressedImage>();
msg->header.stamp = this->now();
msg->header.frame_id = camera_frame_;
msg->format = "jpeg"; // MJPEG stream
// 直接使用V4L2原始数据，不进行二次解码
msg->data.resize(buf.bytesused);
memcpy(msg->data.data(), buffers_[buf.index].start, buf.bytesused);
// 发布消息
image_publisher_->publish(std::move(msg));
frame_counter_++;
// 定期报告统计 (每 300 帧，约 10 秒)
if (enable_fps_stats_ && (frame_counter_ % 300 == 0)) {
auto now = std::chrono::system_clock::now();
auto elapsed_seconds = std::chrono::duration_cast<std::chrono::seconds>(
now - last_stats_time_
).count();
if (elapsed_seconds > 0) {
uint32_t current_frames = frame_counter_ - last_frame_counter;
uint32_t fps = current_frames / elapsed_seconds;
current_fps_ = static_cast<float>(fps);
RCLCPP_INFO(this->get_logger(),
"相机统计 - 实时 FPS: %u, 总帧: %u, 每帧: %u KB",
fps, frame_counter_, buf.bytesused/1024
);
last_stats_time_ = now;
last_frame_counter = frame_counter_;
}
}
} catch (const std::exception& e) {
RCLCPP_ERROR(this->get_logger(), "发布失败: %s", e.what());
}
// 放回队列
if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf)) {
RCLCPP_ERROR(this->get_logger(), "VIDIOC_QBUF error");
}
}
RCLCPP_INFO(this->get_logger(), "📷 捕获线程已停止");
}
/**
* @brief 报告统计信息 (Legacy)
*/
void report_stats() {
auto now = std::chrono::system_clock::now();
auto elapsed_seconds = std::chrono::duration_cast<std::chrono::seconds>(
now - last_stats_time_
).count();
if (elapsed_seconds > 0) {
uint32_t fps = frame_counter_ / elapsed_seconds;
RCLCPP_INFO(this->get_logger(),
"相机统计 - FPS: %u, 总帧: %u",
fps, frame_counter_
);
}
}
};
/**
* @brief 主函数
*/
int main(int argc, char* argv[]) {
rclcpp::init(argc, argv);
auto node = std::make_shared<CameraDriverNode>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}

251
skylink_ros2_ws/src/skylink_bridge/src/device_info_sender_node.cpp
View File

@@ -1,251 +0,0 @@
#include "skylink_bridge/device_info_sender_node.h"
#include <arpa/inet.h>
#include <chrono>
#include <cstring>
#include <fcntl.h>
#include <sys/socket.h>
#include <unistd.h>
using namespace std::chrono_literals;
namespace {
uint16_t read_uint16(const std::string& s) {
try {
return static_cast<uint16_t>(std::stoi(s));
} catch (...) {
return 0;
}
}
} // namespace
DeviceInfoSenderNode::DeviceInfoSenderNode()
: rclcpp::Node("device_info_sender_node") {
load_parameters();
// 创建 UDP 套接字
udp_socket_ = socket(AF_INET, SOCK_DGRAM, 0);
if (udp_socket_ < 0) {
RCLCPP_FATAL(this->get_logger(), "创建 UDP 套接字失败: %s", strerror(errno));
throw std::runtime_error("socket create failed");
}
// 允许广播
int opt = 1;
setsockopt(udp_socket_, SOL_SOCKET, SO_BROADCAST, &opt, sizeof(opt));
// 绑定监听端口
sockaddr_in addr{};
addr.sin_family = AF_INET;
addr.sin_port = htons(info_port_);
addr.sin_addr.s_addr = INADDR_ANY;
if (bind(udp_socket_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0) {
RCLCPP_FATAL(this->get_logger(), "绑定端口 %u 失败: %s", info_port_, strerror(errno));
throw std::runtime_error("bind failed");
}
// 定期广播
if (enable_broadcast_) {
broadcast_timer_ = this->create_wall_timer(
std::chrono::milliseconds(broadcast_interval_ms_),
std::bind(&DeviceInfoSenderNode::broadcast_timer_callback, this));
}
// 请求响应线程
if (enable_request_response_) {
running_.store(true);
listener_thread_ = std::thread(&DeviceInfoSenderNode::handle_request_loop, this);
}
RCLCPP_INFO(this->get_logger(),
"设备信息发送节点启动，端口: %u, 广播: %s, 请求响应: %s",
info_port_, enable_broadcast_ ? "开启" : "关闭",
enable_request_response_ ? "开启" : "关闭");
}
DeviceInfoSenderNode::~DeviceInfoSenderNode() {
running_.store(false);
if (listener_thread_.joinable()) {
listener_thread_.join();
}
if (udp_socket_ >= 0) {
close(udp_socket_);
}
}
void DeviceInfoSenderNode::load_parameters() {
this->declare_parameter<int>("info_port", 10000);
this->declare_parameter<std::string>("target_ip", "255.255.255.255");
this->declare_parameter<bool>("enable_broadcast", true);
this->declare_parameter<bool>("enable_request_response", true);
this->declare_parameter<int>("broadcast_interval_ms", 1000);
this->declare_parameter<std::string>("device_path", "/dev/video0");
this->declare_parameter<std::string>("device_name", "Unknown");
this->declare_parameter<int>("width", 640);
this->declare_parameter<int>("height", 480);
this->declare_parameter<double>("framerate", 30.0);
this->declare_parameter<std::string>("pixel_format", "YUYV");
// 读取
info_port_ = static_cast<uint16_t>(this->get_parameter("info_port").as_int());
target_ip_ = this->get_parameter("target_ip").as_string();
enable_broadcast_ = this->get_parameter("enable_broadcast").as_bool();
enable_request_response_ = this->get_parameter("enable_request_response").as_bool();
broadcast_interval_ms_ = this->get_parameter("broadcast_interval_ms").as_int();
device_path_ = this->get_parameter("device_path").as_string();
device_name_ = this->get_parameter("device_name").as_string();
width_ = static_cast<uint16_t>(this->get_parameter("width").as_int());
height_ = static_cast<uint16_t>(this->get_parameter("height").as_int());
framerate_ = static_cast<float>(this->get_parameter("framerate").as_double());
pixel_format_ = fourcc_from_string(this->get_parameter("pixel_format").as_string());
current_fps_ = 0.0f;
total_frames_ = 0;
}
void DeviceInfoSenderNode::broadcast_timer_callback() {
sockaddr_in addr{};
addr.sin_family = AF_INET;
addr.sin_port = htons(info_port_);
addr.sin_addr.s_addr = inet_addr(target_ip_.c_str());
send_device_info(addr);
}
void DeviceInfoSenderNode::send_device_info(const sockaddr_in& addr) {
DeviceInfoPacket packet = build_packet();
// 手动序列化为网络字节序
std::vector<uint8_t> buffer(sizeof(DeviceInfoPacket), 0);
uint8_t* p = buffer.data();
auto write_u16 = [](uint8_t*& ptr, uint16_t v) {
uint16_t n = htons(v);
std::memcpy(ptr, &n, sizeof(n));
ptr += sizeof(n);
};
auto write_u32 = [](uint8_t*& ptr, uint32_t v) {
uint32_t n = htonl(v);
std::memcpy(ptr, &n, sizeof(n));
ptr += sizeof(n);
};
auto write_float = [](uint8_t*& ptr, float f) {
uint32_t bits;
std::memcpy(&bits, &f, sizeof(bits));
uint32_t n = htonl(bits);
std::memcpy(ptr, &n, sizeof(n));
ptr += sizeof(n);
};
auto write_u64 = [](uint8_t*& ptr, uint64_t v) {
uint64_t n = DeviceInfoSenderNode::htonll(v);
std::memcpy(ptr, &n, sizeof(n));
ptr += sizeof(n);
};
write_u16(p, packet.magic);
write_u32(p, packet.sequence);
write_u16(p, packet.width);
write_u16(p, packet.height);
write_float(p, packet.framerate);
write_u32(p, packet.pixel_format);
std::memcpy(p, packet.device_path, sizeof(packet.device_path));
p += sizeof(packet.device_path);
std::memcpy(p, packet.device_name, sizeof(packet.device_name));
p += sizeof(packet.device_name);
write_float(p, packet.current_fps);
write_u32(p, packet.total_frames);
write_u64(p, packet.timestamp);
write_u16(p, packet.crc16);
ssize_t sent = sendto(udp_socket_, buffer.data(), buffer.size(), 0,
reinterpret_cast<const sockaddr*>(&addr), sizeof(addr));
if (sent < 0) {
RCLCPP_WARN(this->get_logger(), "发送设备信息失败: %s", strerror(errno));
}
}
void DeviceInfoSenderNode::handle_request_loop() {
// 设置非阻塞
int flags = fcntl(udp_socket_, F_GETFL, 0);
fcntl(udp_socket_, F_SETFL, flags | O_NONBLOCK);
std::vector<uint8_t> buf(512);
while (running_.load()) {
sockaddr_in sender{};
socklen_t sender_len = sizeof(sender);
ssize_t n = recvfrom(udp_socket_, buf.data(), buf.size(), 0,
reinterpret_cast<sockaddr*>(&sender), &sender_len);
if (n <= 0) {
std::this_thread::sleep_for(50ms);
continue;
}
if (n < 2) {
continue;
}
uint16_t magic = ntohs(*reinterpret_cast<uint16_t*>(buf.data()));
if (magic == DEVICE_INFO_REQUEST) {
send_device_info(sender);
}
}
}
DeviceInfoPacket DeviceInfoSenderNode::build_packet() {
DeviceInfoPacket packet{};
packet.magic = DEVICE_INFO_MAGIC;
packet.sequence = sequence_++;
packet.width = width_;
packet.height = height_;
packet.framerate = framerate_;
packet.pixel_format = pixel_format_;
std::memset(packet.device_path, 0, sizeof(packet.device_path));
std::strncpy(packet.device_path, device_path_.c_str(), sizeof(packet.device_path) - 1);
std::memset(packet.device_name, 0, sizeof(packet.device_name));
std::strncpy(packet.device_name, device_name_.c_str(), sizeof(packet.device_name) - 1);
packet.current_fps = current_fps_;
packet.total_frames = total_frames_;
auto now = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::system_clock::now().time_since_epoch());
packet.timestamp = static_cast<uint64_t>(now.count());
packet.crc16 = 0;
return packet;
}
uint64_t DeviceInfoSenderNode::htonll(uint64_t value) {
static const int num = 42;
if (*reinterpret_cast<const char*>(&num) == num) {
// little-endian
uint32_t high_part = htonl(static_cast<uint32_t>(value >> 32));
uint32_t low_part = htonl(static_cast<uint32_t>(value & 0xFFFFFFFFULL));
return (static_cast<uint64_t>(low_part) << 32) | high_part;
} else {
return value;
}
}
uint32_t DeviceInfoSenderNode::fourcc_from_string(const std::string& fmt) {
if (fmt.size() >= 4) {
return (static_cast<uint32_t>(fmt[0]) & 0xFF) |
((static_cast<uint32_t>(fmt[1]) & 0xFF) << 8) |
((static_cast<uint32_t>(fmt[2]) & 0xFF) << 16) |
((static_cast<uint32_t>(fmt[3]) & 0xFF) << 24);
}
// 默认 YUYV
return ('Y') | ('U' << 8) | ('Y' << 16) | ('V' << 24);
}
int main(int argc, char* argv[]) {
rclcpp::init(argc, argv);
auto node = std::make_shared<DeviceInfoSenderNode>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}

555
skylink_ros2_ws/src/skylink_bridge/src/udp_sender_node.cpp
View File

@@ -1,555 +0,0 @@
/**
* @file udp_sender_node.cpp
* @brief SkyLink ROS 2 UDP 发送节点 - 实现文件
*
* 完整实现相机和 GPS 数据采集、JPEG 编码、UDP 分片发送的功能。
*
* @author flowzl
* @date 2026-01-19
*/
#include "skylink_bridge/udp_sender_node.h"
#include <sys/socket.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <unistd.h>
#include <fcntl.h>
#include <cerrno>
#include <chrono>
#include <iomanip>
#include <sstream>
#include <sensor_msgs/msg/nav_sat_fix.hpp>
#include <thread>
/**
* @brief 构造函数
*/
UdpSenderNode::UdpSenderNode() : rclcpp::Node("udp_sender_node"), udp_socket_(-1), frame_counter_(0) {
// ========================================================================
// 声明和读取参数
// ========================================================================
this->declare_parameter<std::string>("target_ip", "255.255.255.255");
this->declare_parameter<int>("target_port", 9999);
this->declare_parameter<std::string>("camera_topic", "/camera/image_raw");
this->declare_parameter<std::string>("gps_topic", "/mavros/global_position/global");
this->declare_parameter<int>("jpeg_quality", 70);
this->declare_parameter<bool>("adaptive_quality", true);
this->declare_parameter<int>("quality_min", 30);
this->declare_parameter<int>("quality_max", 95);
this->declare_parameter<bool>("enable_gps", true);
this->declare_parameter<int>("max_packet_size", 500);
this->declare_parameter<int>("frame_timeout_ms", 5000);
this->declare_parameter<bool>("enable_fps_stats", true);
this->declare_parameter<bool>("enable_drop_stats", true);
// 读取参数
target_ip_ = this->get_parameter("target_ip").as_string();
target_port_ = static_cast<uint16_t>(this->get_parameter("target_port").as_int());
camera_topic_ = this->get_parameter("camera_topic").as_string();
gps_topic_ = this->get_parameter("gps_topic").as_string();
jpeg_quality_ = this->get_parameter("jpeg_quality").as_int();
adaptive_quality_ = this->get_parameter("adaptive_quality").as_bool();
quality_min_ = this->get_parameter("quality_min").as_int();
quality_max_ = this->get_parameter("quality_max").as_int();
enable_gps_ = this->get_parameter("enable_gps").as_bool();
max_packet_size_ = static_cast<uint16_t>(this->get_parameter("max_packet_size").as_int());
frame_timeout_ms_ = static_cast<uint32_t>(this->get_parameter("frame_timeout_ms").as_int());
enable_fps_stats_ = this->get_parameter("enable_fps_stats").as_bool();
enable_drop_stats_ = this->get_parameter("enable_drop_stats").as_bool();
// ========================================================================
// 打印初始化信息
// ========================================================================
RCLCPP_INFO(this->get_logger(), "======================================");
RCLCPP_INFO(this->get_logger(), "SkyLink UDP Sender Node 启动");
RCLCPP_INFO(this->get_logger(), "======================================");
RCLCPP_INFO(this->get_logger(), "目标 IP: %s:%d", target_ip_.c_str(), target_port_);
RCLCPP_INFO(this->get_logger(), "相机 Topic: %s", camera_topic_.c_str());
RCLCPP_INFO(this->get_logger(), "GPS Topic: %s", gps_topic_.c_str());
RCLCPP_INFO(this->get_logger(), "JPEG 质量: %d (自适应: %s)", jpeg_quality_, adaptive_quality_ ? "true" : "false");
RCLCPP_INFO(this->get_logger(), "最大包大小: %d 字节", max_packet_size_);
// ========================================================================
// 初始化 UDP 套接字
// ========================================================================
if (!init_udp_socket()) {
RCLCPP_ERROR(this->get_logger(), "❌ UDP 套接字初始化失败，节点退出");
rclcpp::shutdown();
return;
}
RCLCPP_INFO(this->get_logger(), "✓ UDP 套接字初始化成功");
// ========================================================================
// 创建订阅
// ========================================================================
// 使用更激进的 QoS 设置，保留更多消息
rclcpp::QoS qos_profile = rclcpp::SensorDataQoS();
qos_profile.keep_last(10); // 保留最后 10 条消息
camera_subscriber_ = this->create_subscription<sensor_msgs::msg::CompressedImage>(
camera_topic_,
qos_profile,
[this](const sensor_msgs::msg::CompressedImage::SharedPtr msg) {
this->camera_callback(msg);
}
);
RCLCPP_INFO(this->get_logger(), "✓ 订阅相机 Topic: %s (Compressed)", camera_topic_.c_str());
if (enable_gps_) {
gps_subscriber_ = this->create_subscription<sensor_msgs::msg::NavSatFix>(
gps_topic_,
rclcpp::SensorDataQoS(),
[this](const sensor_msgs::msg::NavSatFix::SharedPtr msg) {
this->gps_callback(msg);
}
);
RCLCPP_INFO(this->get_logger(), "✓ 订阅 GPS Topic: %s", gps_topic_.c_str());
}
// ========================================================================
// 启动后台发送线程
// ========================================================================
sender_running_ = true;
sender_thread_ = std::thread(&UdpSenderNode::sender_worker, this);
RCLCPP_INFO(this->get_logger(), "✓ 后台发送线程已启动");
// ========================================================================
// 初始化统计
// ========================================================================
stats_.last_report_time = std::chrono::system_clock::now();
RCLCPP_INFO(this->get_logger(), "✓ 节点初始化完成，等待数据...");
RCLCPP_INFO(this->get_logger(), "======================================");
}
/**
* @brief 析构函数
*/
UdpSenderNode::~UdpSenderNode() {
// 停止后台发送线程
sender_running_ = false;
send_queue_cv_.notify_one(); // 唤醒等待的线程
if (sender_thread_.joinable()) {
sender_thread_.join();
RCLCPP_INFO(this->get_logger(), "✓ 后台发送线程已停止");
}
close_udp_socket();
RCLCPP_INFO(this->get_logger(), "UDP Sender Node 已关闭");
}
/**
* @brief 初始化 UDP 套接字
*/
bool UdpSenderNode::init_udp_socket() {
// 创建 UDP 套接字
udp_socket_ = socket(AF_INET, SOCK_DGRAM, 0);
if (udp_socket_ < 0) {
RCLCPP_ERROR(this->get_logger(), "套接字创建失败: %s", strerror(errno));
return false;
}
// 开启广播权限
int broadcast_permission = 1;
if (setsockopt(udp_socket_, SOL_SOCKET, SO_BROADCAST, &broadcast_permission, sizeof(broadcast_permission)) < 0) {
RCLCPP_WARN(this->get_logger(), "设置 SO_BROADCAST 失败");
}
// 设置套接字可重用
int opt = 1;
if (setsockopt(udp_socket_, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)) < 0) {
RCLCPP_WARN(this->get_logger(), "设置 SO_REUSEADDR 失败");
}
// 设置发送缓冲区
// 增加缓冲区大小到 4MB (4 * 1024 * 1024) 以应对高码率 MJPEG 流
int buffer_size = 4 * 1024 * 1024;
if (setsockopt(udp_socket_, SOL_SOCKET, SO_SNDBUF, &buffer_size, sizeof(buffer_size)) < 0) {
RCLCPP_WARN(this->get_logger(), "设置发送缓冲区失败 (尝试 4MB): %s", strerror(errno));
// Fallback to smaller buffer if failed
buffer_size = 512 * 1024;
setsockopt(udp_socket_, SOL_SOCKET, SO_SNDBUF, &buffer_size, sizeof(buffer_size));
} else {
// 验证实际设置的大小 (Linux 内核通常会将其翻倍)
int actual_buf_size = 0;
socklen_t opt_len = sizeof(actual_buf_size);
if (getsockopt(udp_socket_, SOL_SOCKET, SO_SNDBUF, &actual_buf_size, &opt_len) == 0) {
RCLCPP_INFO(this->get_logger(), "UDP 发送缓冲区已设置为: %d bytes", actual_buf_size);
}
}
// 设置非阻塞模式
int flags = fcntl(udp_socket_, F_GETFL, 0);
fcntl(udp_socket_, F_SETFL, flags | O_NONBLOCK);
return true;
}
/**
* @brief 关闭 UDP 套接字
*/
void UdpSenderNode::close_udp_socket() {
if (udp_socket_ >= 0) {
close(udp_socket_);
udp_socket_ = -1;
}
}
/**
* @brief 发送 UDP 数据包
*/
int UdpSenderNode::send_udp_packet(const uint8_t* data, uint16_t len) {
if (udp_socket_ < 0 || !data || len == 0) {
return -1;
}
struct sockaddr_in addr;
std::memset(&addr, 0, sizeof(addr));
addr.sin_family = AF_INET;
addr.sin_port = htons(target_port_);
addr.sin_addr.s_addr = inet_addr(target_ip_.c_str());
int retries = 0;
const int MAX_RETRIES = 5;
ssize_t sent = -1;
while (retries < MAX_RETRIES) {
sent = sendto(udp_socket_, data, len, 0, (struct sockaddr*)&addr, sizeof(addr));
if (sent > 0) {
bytes_sent_ += sent;
packets_sent_++;
return sent;
}
if (errno == EAGAIN || errno == EWOULDBLOCK) {
// 资源暂时不可用，等待并重试
retries++;
// 短暂休眠 1ms，给内核一点时间清空缓冲区
std::this_thread::sleep_for(std::chrono::microseconds(1000));
continue;
} else {
// 其他错误，直接退出
break;
}
}
// 如果重试后仍然失败
if (sent <= 0) {
// 简单的错误限流日志
static auto last_error_time = std::chrono::steady_clock::now();
auto now = std::chrono::steady_clock::now();
if (std::chrono::duration_cast<std::chrono::seconds>(now - last_error_time).count() > 5) {
RCLCPP_WARN(this->get_logger(), "UDP 发送失败 (即使重试后): %s (errno=%d)", strerror(errno), errno);
last_error_time = now;
}
}
return sent;
}
/**
* @brief 批量将数据包加入发送队列
*/
void UdpSenderNode::enqueue_packets(std::vector<std::vector<uint8_t>>& packets) {
{
std::lock_guard<std::mutex> lock(send_queue_mutex_);
if (send_queue_.size() + packets.size() > MAX_QUEUE_SIZE) {
frames_dropped_++;
// 队列满时，丢弃整个 batch，避免部分帧发送
return;
}
for (auto& packet_data : packets) {
PacketQueueItem item;
// 使用 move 避免拷贝
item.data = std::move(packet_data);
item.enqueue_time = std::chrono::system_clock::now();
send_queue_.push(std::move(item));
}
}
// 通知发送线程有新数据
send_queue_cv_.notify_one();
}
/**
* @brief 将数据包加入发送队列
*/
void UdpSenderNode::enqueue_packet(const uint8_t* data, uint16_t len) {
{
std::lock_guard<std::mutex> lock(send_queue_mutex_);
if (send_queue_.size() >= MAX_QUEUE_SIZE) {
frames_dropped_++;
return;
}
PacketQueueItem item;
item.data.assign(data, data + len);
item.enqueue_time = std::chrono::system_clock::now();
send_queue_.push(item);
}
// 通知发送线程有新数据
send_queue_cv_.notify_one();
}
/**
* @brief 后台发送工作线程
*/
void UdpSenderNode::sender_worker() {
RCLCPP_INFO(this->get_logger(), "发送线程开始运行 PID: %d", (int)getpid());
// 预分配 batch 内存以减少循环中的分配
std::vector<PacketQueueItem> batch;
batch.reserve(BATCH_SEND_SIZE);
while (sender_running_) {
try {
batch.clear();
{
std::unique_lock<std::mutex> lock(send_queue_mutex_);
// 如果队列为空，等待条件变量唤醒（等待最多 100ms）
if (send_queue_.empty()) {
send_queue_cv_.wait_for(lock, std::chrono::milliseconds(100),
[this] { return !send_queue_.empty() || !sender_running_; });
}
// 批量获取数据包（一次最多 BATCH_SEND_SIZE 个）
while (!send_queue_.empty() && batch.size() < BATCH_SEND_SIZE) {
// 使用 move 避免通过拷贝构造函数复制数据，大幅提高性能
batch.push_back(std::move(send_queue_.front()));
send_queue_.pop();
}
}
// 发送批量数据包（不在锁内进行）
for (const auto& item : batch) {
send_udp_packet(item.data.data(), item.data.size());
}
} catch (const std::exception& e) {
RCLCPP_ERROR(this->get_logger(), "发送线程发生异常: %s", e.what());
// 发生异常后稍微休眠一下避免死循环疯狂报错
std::this_thread::sleep_for(std::chrono::milliseconds(100));
} catch (...) {
RCLCPP_ERROR(this->get_logger(), "发送线程发生未知异常");
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
}
RCLCPP_INFO(this->get_logger(), "发送线程已退出");
}
/**
* @brief 相机图像回调
*/
void UdpSenderNode::camera_callback(const sensor_msgs::msg::CompressedImage::SharedPtr msg) {
try {
if (msg->data.empty()) {
RCLCPP_WARN(this->get_logger(), "收到空帧");
return;
}
// 直接使用压缩数据，不再解码/重编码
const std::vector<uint8_t>& jpeg_data = msg->data;
// 获取当前 GPS 数据
double lat, lon, alt;
{
std::lock_guard<std::mutex> lock(current_gps_data_.mutex);
lat = current_gps_data_.latitude;
lon = current_gps_data_.longitude;
alt = current_gps_data_.altitude;
}
// 分片并发送
double timestamp = rclcpp::Clock().now().seconds();
uint16_t chunks_sent_local = fragment_and_send(
jpeg_data.data(),
jpeg_data.size(),
timestamp,
lat,
lon,
alt
);
(void)chunks_sent_local;
// 记录本帧发送统计，便于调试（每帧输出可能较多，建议仅在调试时启用）
// RCLCPP_INFO(this->get_logger(), "帧信息: jpeg_size=%zu bytes, chunks_sent=%u", jpeg_data.size(), chunks_sent_local);
frame_counter_++;
stats_.total_frames++;
stats_.total_bytes += jpeg_data.size();
// 定期打印统计
if (enable_fps_stats_ && (frame_counter_ % 300 == 0)) {
report_stats();
}
} catch (const std::exception& e) {
RCLCPP_ERROR(this->get_logger(), "相机回调异常: %s", e.what());
frames_dropped_++;
}
}
/**
* @brief GPS 回调
*/
void UdpSenderNode::gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) {
std::lock_guard<std::mutex> lock(current_gps_data_.mutex);
current_gps_data_.latitude = msg->latitude;
current_gps_data_.longitude = msg->longitude;
current_gps_data_.altitude = msg->altitude;
}
/**
* @brief 将 Mat 编码为 JPEG
*/
bool UdpSenderNode::encode_to_jpeg(const cv::Mat& frame, int quality, std::vector<uint8_t>& output) {
try {
std::vector<int> params{cv::IMWRITE_JPEG_QUALITY, quality};
cv::imencode(".jpg", frame, output, params);
return !output.empty();
} catch (const cv::Exception& e) {
RCLCPP_ERROR(this->get_logger(), "OpenCV 编码异常: %s", e.what());
return false;
}
}
/**
* @brief 分片并发送
*/
uint16_t UdpSenderNode::fragment_and_send(
const uint8_t* jpeg_data,
uint32_t jpeg_len,
double timestamp,
double lat,
double lon,
double alt) {
// 计算需要的分片数
uint16_t payload_size = max_packet_size_ - sizeof(PacketHeader);
uint16_t total_chunks = (jpeg_len + payload_size - 1) / payload_size;
if (total_chunks > MAX_CHUNKS) {
RCLCPP_WARN(this->get_logger(), "帧太大，超过最大分片数");
return 0;
}
// 预分配所有分片，进行批量入队
std::vector<std::vector<uint8_t>> frame_packets;
frame_packets.reserve(total_chunks);
uint16_t chunks_sent = 0;
// 创建每个分片
for (uint16_t i = 0; i < total_chunks; i++) {
uint16_t current_payload_size = std::min(
(uint32_t)payload_size,
jpeg_len - (uint32_t)i * payload_size
);
// 构造包头
PacketHeader header;
header.magic = SKYLINK_MAGIC;
header.frame_id = frame_counter_;
header.total_chunks = total_chunks;
header.chunk_index = i;
header.data_len = current_payload_size;
header.timestamp = timestamp;
header.lat = lat;
header.lon = lon;
header.alt = alt;
header.crc16 = 0; // TODO: 计算 CRC
header.reserve = 0;
// 创建完整数据包
std::vector<uint8_t> packet(sizeof(PacketHeader) + current_payload_size);
std::memcpy(packet.data(), &header, sizeof(PacketHeader));
std::memcpy(
packet.data() + sizeof(PacketHeader),
jpeg_data + i * payload_size,
current_payload_size
);
frame_packets.push_back(std::move(packet)); // Move 到 vector 中
chunks_sent++;
}
// 批量入队，减少锁的竞争
enqueue_packets(frame_packets);
return chunks_sent;
}
/**
* @brief 报告统计信息
*/
void UdpSenderNode::report_stats() {
auto now = std::chrono::system_clock::now();
auto elapsed_seconds = std::chrono::duration_cast<std::chrono::seconds>(
now - stats_.last_report_time
).count();
// 获取当前队列深度
size_t current_queue_depth = 0;
{
std::lock_guard<std::mutex> lock(send_queue_mutex_);
current_queue_depth = send_queue_.size();
}
if (elapsed_seconds > 0) {
// 计算本周期的帧数和字节数（不是累计值）
uint32_t frames_this_period = stats_.total_frames;
uint32_t bytes_this_period = stats_.total_bytes;
// 计算实时 FPS 和吞吐量
uint32_t fps = frames_this_period / elapsed_seconds;
uint32_t kbps = (bytes_this_period / elapsed_seconds) / 1024;
// 计算队列使用率（百分比）
float queue_utilization = (current_queue_depth * 100.0f) / MAX_QUEUE_SIZE;
RCLCPP_INFO(this->get_logger(),
"发送统计 - FPS: %u, 速率: %u KB/s, 总帧: %u, 总包: %u, 丢弃: %u, 队列: %zu/%zu (%.1f%%)",
fps, kbps, frame_counter_, packets_sent_.load(), frames_dropped_.load(),
current_queue_depth, MAX_QUEUE_SIZE, queue_utilization
);
// 如果队列接近满，发出警告
if (queue_utilization > 80.0f) {
RCLCPP_WARN(this->get_logger(),
"发送队列接近饱和 (%.1f%%)，建议检查网络或增加发送线程",
queue_utilization
);
}
// 重置周期统计
stats_.total_frames = 0;
stats_.total_bytes = 0;
stats_.last_report_time = now;
}
}
/**
* @brief 主函数
*/
int main(int argc, char* argv[]) {
rclcpp::init(argc, argv);
auto node = std::make_shared<UdpSenderNode>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}

32
skylink_ros2_ws/src/skylink_bridge/config/params.yaml → skylink_ros2_ws/src/skylink_bridge_python/config/params.yaml
View File

@@ -9,13 +9,15 @@
/camera_driver: /camera_driver:
ros__parameters: ros__parameters:
# 相机驱动参数（对应 launch 中的 node name: camera_driver） # 相机驱动参数（对应 launch 中的 node name: camera_driver）
video_device: "/dev/video6" video_device: "/dev/video2"
framerate: 30.0
image_width: 640 image_width: 3840
image_height: 480 image_height: 2160
camera_topic: "/camera/image_raw/compressed" # 拍照功能配置
camera_frame: "camera" save_dir: "./camera_data"
enable_fps_stats: true warmup_frames: 20 # 拍照前预热帧数，防止刚启动时过曝
interval: 5.0 # 自动拍照间隔（秒），设为 0 则仅通过服务触发拍照
gps_topic: "/gps_info"
/udp_sender: /udp_sender:
ros__parameters: ros__parameters:
@@ -26,7 +28,7 @@
sender_thread_name: "skylink_sender" sender_thread_name: "skylink_sender"
# 相机相关（udp_sender 使用的 topic / 图像格式） # 相机相关（udp_sender 使用的 topic / 图像格式）
camera_topic: "/camera/image_raw/compressed" camera_topic: "/camera/image_raw"
camera_frame: "bgr8" camera_frame: "bgr8"
# JPEG 压缩配置 # JPEG 压缩配置
@@ -50,17 +52,3 @@
qos_keep_last: 10 # 增加消息队列深度 qos_keep_last: 10 # 增加消息队列深度
enable_batch_send: true # 启用批量发送优化 enable_batch_send: true # 启用批量发送优化
send_interval_ms: 0 # 发送间隔（0 表示尽快发送） send_interval_ms: 0 # 发送间隔（0 表示尽快发送）
/device_info_sender:
ros__parameters:
info_port: 10000
target_ip: "255.255.255.255"
broadcast_interval_ms: 1000
enable_broadcast: true
enable_request_response: true
device_path: "/dev/video6"
device_name: "Unknown"
width: 1280
height: 720
framerate: 30.0
pixel_format: "YUYV"

29
skylink_ros2_ws/src/skylink_bridge/launch/bridge.launch.py → skylink_ros2_ws/src/skylink_bridge_python/launch/bridge.launch.py
View File

@@ -10,12 +10,12 @@ def generate_launch_description():
SkyLink Bridge 启动配置 SkyLink Bridge 启动配置
用法: 用法:
ros2 launch skylink_bridge bridge.launch.py ros2 launch skylink_bridge_python bridge.launch.py
ros2 launch skylink_bridge bridge.launch.py target_ip:=192.168.1.100 jpeg_quality:=80 ros2 launch skylink_bridge_python bridge.launch.py target_ip:=192.168.1.100 jpeg_quality:=80
""" """
# 获取包共享目录 # 获取包共享目录
pkg_share_dir = get_package_share_directory('skylink_bridge') pkg_share_dir = get_package_share_directory('skylink_bridge_python')
config_file = os.path.join(pkg_share_dir, 'config', 'params.yaml') config_file = os.path.join(pkg_share_dir, 'config', 'params.yaml')
# ======================================================================== # ========================================================================
@@ -34,18 +34,6 @@ def generate_launch_description():
description='UDP 目标端口' description='UDP 目标端口'
) )
jpeg_quality_arg = DeclareLaunchArgument(
'jpeg_quality',
default_value='70',
description='JPEG 压缩质量 (1-100)'
)
camera_topic_arg = DeclareLaunchArgument(
'camera_topic',
default_value='/camera/image_raw',
description='相机 Topic 名称'
)
gps_topic_arg = DeclareLaunchArgument( gps_topic_arg = DeclareLaunchArgument(
'gps_topic', 'gps_topic',
default_value='/mavros/global_position/global', default_value='/mavros/global_position/global',
@@ -56,15 +44,12 @@ def generate_launch_description():
# ======================================================================== # ========================================================================
camera_driver_node = Node( camera_driver_node = Node(
package='skylink_bridge', package='skylink_bridge_python',
executable='camera_driver_node', executable='camera_driver_node',
name='camera_driver', name='camera_driver',
output='screen', output='screen',
parameters=[ parameters=[
config_file, # 从 YAML 配置文件加载参数 config_file, # 从 YAML 配置文件加载参数
{
'camera_topic': LaunchConfiguration('camera_topic'),
}
], ],
) )
@@ -73,7 +58,7 @@ def generate_launch_description():
# ======================================================================== # ========================================================================
udp_sender_node = Node( udp_sender_node = Node(
package='skylink_bridge', package='skylink_bridge_python',
executable='udp_sender_node', executable='udp_sender_node',
name='udp_sender', name='udp_sender',
output='screen', output='screen',
@@ -100,9 +85,7 @@ def generate_launch_description():
return LaunchDescription([ return LaunchDescription([
target_ip_arg, target_ip_arg,
target_port_arg, target_port_arg,
jpeg_quality_arg,
camera_topic_arg,
gps_topic_arg, gps_topic_arg,
camera_driver_node, camera_driver_node,
udp_sender_node, # udp_sender_node,
]) ])

23
skylink_ros2_ws/src/skylink_bridge_python/package.xml Normal file
View File

@@ -0,0 +1,23 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>skylink_bridge_python</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="737666956@qq.com">flower</maintainer>
<license>TODO: License declaration</license>
<test_depend>python3-pytest</test_depend>
<!-- ros2 依赖 -->
<exec_depend>rclpy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>std_srvs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>ffmpeg</exec_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>

0
skylink_ros2_ws/src/skylink_bridge/test/cam_list.py → skylink_ros2_ws/src/skylink_bridge_python/pkg_test/cam_list.py
View File

0
skylink_ros2_ws/src/skylink_bridge/test/test_cam.py → skylink_ros2_ws/src/skylink_bridge_python/pkg_test/cam_test.py
View File

0
skylink_ros2_ws/src/skylink_bridge_python/resource/skylink_bridge_python Normal file
View File

4
skylink_ros2_ws/src/skylink_bridge_python/setup.cfg Normal file
View File

@@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/skylink_bridge_python
[install]
install_scripts=$base/lib/skylink_bridge_python

32
skylink_ros2_ws/src/skylink_bridge_python/setup.py Normal file
View File

@@ -0,0 +1,32 @@
from setuptools import find_packages, setup
package_name = 'skylink_bridge_python'
setup(
name=package_name,
version='0.0.0',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
('share/' + package_name + '/launch', ['launch/bridge.launch.py']),
('share/' + package_name + '/config', ['config/params.yaml']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='flower',
maintainer_email='737666956@qq.com',
description='TODO: Package description',
license='TODO: License declaration',
extras_require={
'test': [
'pytest',
],
},
entry_points={
'console_scripts': [
'camera_driver_node = skylink_bridge_python.camera_driver_node:main'
],
},
)

0
skylink_ros2_ws/src/skylink_bridge_python/skylink_bridge_python/__init__.py Normal file
View File

BIN
skylink_ros2_ws/src/skylink_bridge_python/skylink_bridge_python/__pycache__/__init__.cpython-310.pyc Normal file
View File

Binary file not shown.

BIN
skylink_ros2_ws/src/skylink_bridge_python/skylink_bridge_python/__pycache__/camera_driver_node.cpython-310.pyc Normal file
View File

Binary file not shown.

153
skylink_ros2_ws/src/skylink_bridge_python/skylink_bridge_python/camera_driver_node.py Normal file
View File

@@ -0,0 +1,153 @@
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import NavSatFix
from std_srvs.srv import Trigger
import subprocess
import os
import datetime
import threading
class CameraNode(Node):
def __init__(self):
super().__init__('camera_driver_node')
# 声明参数
self.declare_parameter('video_device', '/dev/video0')
self.declare_parameter('save_dir', './camera_data')
self.declare_parameter('gps_topic', '/gps_info')
self.declare_parameter('interval', 5.0)
self.declare_parameter('image_width', 640)
self.declare_parameter('image_height', 480)
self.declare_parameter('warmup_frames', 20) # 默认预热20帧
# 获取参数值
self.camera_device = self.get_parameter('video_device').value
self.save_dir = self.get_parameter('save_dir').value
self.gps_topic = self.get_parameter('gps_topic').value
self.interval = self.get_parameter('interval').value
self.image_width = self.get_parameter('image_width').value
self.image_height = self.get_parameter('image_height').value
self.warmup_frames = self.get_parameter('warmup_frames').value
# 创建保存目录
if not os.path.exists(self.save_dir):
os.makedirs(self.save_dir)
self.get_logger().info(f'Created directory: {self.save_dir}')
# 初始化 GPS 数据
self.latest_gps = None
# 订阅 GPS
self.gps_sub = self.create_subscription(
NavSatFix,
self.gps_topic,
self.gps_callback,
10
)
# 创建拍照服务
self.srv = self.create_service(Trigger, 'trigger_photo', self.trigger_callback)
# 定时拍照
self.timer = self.create_timer(self.interval, self.timer_callback)
def gps_callback(self, msg):
self.latest_gps = msg
def trigger_callback(self, request, response):
"""
响应服务调用进行拍照
"""
self.get_logger().info('Received capture trigger request')
success, message = self.take_photo()
response.success = success
response.message = message
return response
def take_photo(self):
"""
执行拍照逻辑，使用 ffmpeg 直接调用 v4l2，返回 (success, message)
"""
# 获取当前时间
now = datetime.datetime.now()
time_str = now.strftime('%Y%m%d-%H%M%S')
# 格式化 GPS 信息
if self.latest_gps:
# 假设 GPS 数据有效
lat = self.latest_gps.latitude
lon = self.latest_gps.longitude
gps_str = f"lat{lat:.6f}_lon{lon:.6f}"
else:
gps_str = "no_gps"
# 拼接文件名: 时间-gps信息
filename = f"{time_str}-{gps_str}.jpg"
full_path = os.path.join(self.save_dir, filename)
# 构建 ffmpeg 命令
# 为了防止过曝，需要让摄像头运行几帧让自动曝光收敛
# 策略：抓取 warmup 帧数，使用 -y 和 -update 1 不断覆盖输出文件，最终保留的是最后一帧
# 注意：warmup_frames 至少为 1
frames_to_capture = max(1, self.warmup_frames)
cmd = [
'ffmpeg',
'-y', # 覆盖输出文件
'-f', 'video4linux2', # 强制使用 v4l2 格式
'-video_size', f'{self.image_width}x{self.image_height}', # 设置分辨率
'-i', self.camera_device, # 输入设备
'-vframes', str(frames_to_capture), # 抓取 N 帧
'-update', '1', # 允许输出文件被覆盖（配合 vframes 使用）
full_path # 输出路径
]
try:
# 运行命令，捕获输出以便出错时打印
self.get_logger().info(f"Executing: {' '.join(cmd)}")
result = subprocess.run(
cmd,
stdout=subprocess.PIPE,
stderr=subprocess.PIPE,
timeout=10 # 设置超时防止卡死
)
if result.returncode == 0:
msg = f'Saved photo: {filename}'
self.get_logger().info(msg)
return True, msg
else:
err_msg = f"FFmpeg failed with return code {result.returncode}: {result.stderr.decode()}"
self.get_logger().error(err_msg)
return False, err_msg
except subprocess.TimeoutExpired:
err_msg = "FFmpeg command timed out"
self.get_logger().error(err_msg)
return False, err_msg
except Exception as e:
err_msg = f"Error saving photo: {str(e)}"
self.get_logger().error(err_msg)
return False, err_msg
def timer_callback(self):
# 简单的定时拍照，若需要可保留，逻辑委托给 take_photo
if self.interval > 0:
self.take_photo()
def destroy_node(self):
super().destroy_node()
def main(args=None):
rclpy.init(args=args)
node = CameraNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

25
skylink_ros2_ws/src/skylink_bridge_python/test/test_copyright.py Normal file
View File

@@ -0,0 +1,25 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_copyright.main import main
import pytest
# Remove the `skip` decorator once the source file(s) have a copyright header
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found errors'

25
skylink_ros2_ws/src/skylink_bridge_python/test/test_flake8.py Normal file
View File

@@ -0,0 +1,25 @@
# Copyright 2017 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_flake8.main import main_with_errors
import pytest
@pytest.mark.flake8
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
assert rc == 0, \
'Found %d code style errors / warnings:\n' % len(errors) + \
'\n'.join(errors)

23
skylink_ros2_ws/src/skylink_bridge_python/test/test_pep257.py Normal file
View File

@@ -0,0 +1,23 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_pep257.main import main
import pytest
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found code style errors / warnings'