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

分支开始

Browse Source
This commit is contained in:
flower
2026-02-02 10:41:39 +08:00
parent 9b5abe0877
commit 47016c00a6
463 changed files with 18 additions and 67629 deletions
Download Patch File Download Diff File Expand all files Collapse all files

68
skylink_ros2_ws/build.sh Executable file
View File

@@ -0,0 +1,68 @@
#!/bin/bash
# SkyLink ROS 2 机载端 - 快速编译脚本
# 用法: ./build.sh [release|debug]
set -e
SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
BUILD_TYPE="${1:-release}"
echo "========================================"
echo "SkyLink ROS 2 编译脚本"
echo "========================================"
echo "工作目录: $SCRIPT_DIR"
echo "编译类型: $BUILD_TYPE"
echo ""
# 检查 ROS 2 环境
if [ -z "$ROS_DISTRO" ]; then
echo "❌ 错误：未检测到 ROS 2 环境"
echo "请先运行: source /opt/ros/humble/setup.bash"
exit 1
fi
echo "✓ ROS 2 发行版: $ROS_DISTRO"
# 检查依赖
echo ""
echo "正在检查系统依赖..."
if ! command -v colcon &> /dev/null; then
echo "❌ 错误：未找到 colcon，请安装："
echo " sudo apt install -y python3-colcon-common"
exit 1
fi
echo "✓ colcon 已安装"
# 清理旧编译（可选）
if [ -d "$SCRIPT_DIR/build" ] && [ "$BUILD_TYPE" = "clean" ]; then
echo ""
echo "清理旧编译产物..."
rm -rf "$SCRIPT_DIR/build" "$SCRIPT_DIR/install" "$SCRIPT_DIR/log"
echo "✓ 清理完成"
fi
# 编译
echo ""
echo "正在编译 skylink_bridge 包..."
echo ""
if [ "$BUILD_TYPE" = "debug" ]; then
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Debug
else
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
fi
echo ""
echo "========================================"
echo "✓ 编译成功！"
echo "========================================"
echo ""
echo "下一步："
echo "1. 加载 ROS 环境:"
echo " source $SCRIPT_DIR/install/setup.bash"
echo ""
echo "2. 启动节点:"
echo " ros2 launch skylink_bridge bridge.launch.py"
echo ""

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

@@ -0,0 +1,156 @@
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()

66
skylink_ros2_ws/src/skylink_bridge/config/params.yaml Normal file
View File

@@ -0,0 +1,66 @@
/**:
ros__parameters:
# 全局默认参数（可被各节点覆盖）
log_level: "INFO"
enable_fps_stats: true
fps_stats_interval: 5
enable_drop_stats: true
/camera_driver:
ros__parameters:
# 相机驱动参数（对应 launch 中的 node name: camera_driver）
video_device: "/dev/video6"
framerate: 30.0
image_width: 640
image_height: 480
camera_topic: "/camera/image_raw/compressed"
camera_frame: "camera"
enable_fps_stats: true
/udp_sender:
ros__parameters:
# 网络与发送配置（对应 udp_sender 节点）
target_ip: "255.255.255.255"
target_port: 9999
udp_buffer_size: 65536
sender_thread_name: "skylink_sender"
# 相机相关（udp_sender 使用的 topic / 图像格式）
camera_topic: "/camera/image_raw/compressed"
camera_frame: "bgr8"
# JPEG 压缩配置
jpeg_quality: 80
adaptive_quality: false # 禁用自适应，使用固定质量以获得稳定帧率
quality_min: 30
quality_max: 95
# GPS / 定位
gps_topic: "/mavros/global_position/global"
enable_gps: true
default_latitude: 39.9042
default_longitude: 116.4074
default_altitude: 100.0
# 分片和重组
max_packet_size: 1472
frame_timeout_ms: 5000
# 性能优化参数
qos_keep_last: 10 # 增加消息队列深度
enable_batch_send: true # 启用批量发送优化
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"

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

@@ -0,0 +1,171 @@
/**
* @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 Normal file
View File

@@ -0,0 +1,58 @@
#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 Normal file
View File

@@ -0,0 +1,237 @@
/**
* @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();
};

108
skylink_ros2_ws/src/skylink_bridge/launch/bridge.launch.py Normal file
View File

@@ -0,0 +1,108 @@
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
import os
from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
"""
SkyLink Bridge 启动配置
用法:
ros2 launch skylink_bridge bridge.launch.py
ros2 launch skylink_bridge bridge.launch.py target_ip:=192.168.1.100 jpeg_quality:=80
"""
# 获取包共享目录
pkg_share_dir = get_package_share_directory('skylink_bridge')
config_file = os.path.join(pkg_share_dir, 'config', 'params.yaml')
# ========================================================================
# 启动参数声明 (可通过命令行覆盖)
# ========================================================================
target_ip_arg = DeclareLaunchArgument(
'target_ip',
default_value='255.255.255.255',
description='目标 IP 地址 (广播)'
)
target_port_arg = DeclareLaunchArgument(
'target_port',
default_value='9999',
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',
default_value='/mavros/global_position/global',
description='GPS Topic 名称'
)
# ========================================================================
# 相机 节点 (OpenCV 驱动)
# ========================================================================
camera_driver_node = Node(
package='skylink_bridge',
executable='camera_driver_node',
name='camera_driver',
output='screen',
parameters=[
config_file, # 从 YAML 配置文件加载参数
{
'camera_topic': LaunchConfiguration('camera_topic'),
}
],
)
# ========================================================================
# UDP Sender 节点
# ========================================================================
udp_sender_node = Node(
package='skylink_bridge',
executable='udp_sender_node',
name='udp_sender',
output='screen',
parameters=[
config_file, # 从 YAML 配置文件加载参数
{
'target_ip': LaunchConfiguration('target_ip'),
'target_port': LaunchConfiguration('target_port'),
'jpeg_quality': LaunchConfiguration('jpeg_quality'),
'camera_topic': LaunchConfiguration('camera_topic'),
'gps_topic': LaunchConfiguration('gps_topic'),
}
],
remappings=[
# 如果需要重映射 Topic，在此添加
# ('/camera/image_raw', '/my_camera/image'),
]
)
# ========================================================================
# 构造启动描述
# ========================================================================
return LaunchDescription([
target_ip_arg,
target_port_arg,
jpeg_quality_arg,
camera_topic_arg,
gps_topic_arg,
camera_driver_node,
udp_sender_node,
])

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

@@ -0,0 +1,41 @@
<?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 Normal file
View File

@@ -0,0 +1,494 @@
/**
* @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 Normal file
View File

@@ -0,0 +1,251 @@
#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 Normal file
View File

@@ -0,0 +1,555 @@
/**
* @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;
}

234
skylink_ros2_ws/src/skylink_bridge/test/cam_list.py Normal file
View File

@@ -0,0 +1,234 @@
import cv2
import sys
import platform
def list_available_cameras(max_test=10):
"""
检测所有可用的相机设备
"""
available_cameras = []
print("=== 开始检测可用相机设备 ===")
for i in range(max_test):
# 根据操作系统选择不同的后端
if platform.system() == "Windows":
cap = cv2.VideoCapture(i, cv2.CAP_DSHOW)
elif platform.system() == "Linux":
cap = cv2.VideoCapture(i, cv2.CAP_V4L2)
else: # macOS
cap = cv2.VideoCapture(i, cv2.CAP_AVFOUNDATION)
if cap.isOpened():
# 获取相机基本信息
width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
fps = cap.get(cv2.CAP_PROP_FPS)
print(f"✅ 相机 {i}: 可用")
available_cameras.append(i)
cap.release()
else:
cap.release()
print(f"检测完成，找到 {len(available_cameras)} 个可用相机: {available_cameras}")
return available_cameras
def get_camera_properties(camera_index):
"""
获取相机的基本属性和当前设置
"""
if platform.system() == "Windows":
cap = cv2.VideoCapture(camera_index, cv2.CAP_DSHOW)
elif platform.system() == "Linux":
cap = cv2.VideoCapture(camera_index, cv2.CAP_V4L2)
else:
cap = cv2.VideoCapture(camera_index, cv2.CAP_AVFOUNDATION)
if not cap.isOpened():
print(f"❌ 无法打开相机 {camera_index}")
return None
print(f"\n{'='*60}")
print(f"相机 {camera_index} 详细信息")
print(f"{'='*60}")
# 相机基本属性列表 [5,8](@ref)
properties = [
(cv2.CAP_PROP_FRAME_WIDTH, "帧宽度"),
(cv2.CAP_PROP_FRAME_HEIGHT, "帧高度"),
(cv2.CAP_PROP_FPS, "帧率(FPS)"),
(cv2.CAP_PROP_BRIGHTNESS, "亮度"),
(cv2.CAP_PROP_CONTRAST, "对比度"),
(cv2.CAP_PROP_SATURATION, "饱和度"),
(cv2.CAP_PROP_HUE, "色调"),
(cv2.CAP_PROP_GAIN, "增益"),
(cv2.CAP_PROP_EXPOSURE, "曝光度"),
(cv2.CAP_PROP_AUTO_EXPOSURE, "自动曝光"),
(cv2.CAP_PROP_AUTOFOCUS, "自动对焦"),
(cv2.CAP_PROP_FOCUS, "焦距"),
(cv2.CAP_PROP_ZOOM, "变焦"),
(cv2.CAP_PROP_WHITE_BALANCE_BLUE_U, "白平衡蓝色"),
(cv2.CAP_PROP_WHITE_BALANCE_RED_V, "白平衡红色"),
(cv2.CAP_PROP_BACKLIGHT, "背光补偿"),
(cv2.CAP_PROP_BUFFERSIZE, "缓冲区大小"),
]
# 打印各属性当前值
print("📊 当前参数设置:")
for prop_id, prop_name in properties:
value = cap.get(prop_id)
# 特殊处理曝光属性（可能为负值）
if prop_id == cv2.CAP_PROP_EXPOSURE:
if value < 0:
print(f" {prop_name}: 自动曝光")
else:
print(f" {prop_name}: {value}")
else:
print(f" {prop_name}: {value}")
return cap
def test_supported_resolutions(cap, camera_index):
"""
测试相机支持的分辨率 [5,6](@ref)
"""
print(f"\n🔍 测试相机 {camera_index} 支持的分辨率:")
# 常见分辨率列表
common_resolutions = [
(160, 120), # QQVGA
(320, 240), # QVGA
(640, 480), # VGA
(800, 600), # SVGA
(1024, 768), # XGA
(1280, 720), # 720p HD
(1280, 800), # WXGA
(1366, 768), # HD
(1920, 1080), # 1080p Full HD
(2560, 1440), # 2K
(3840, 2160), # 4K UHD
]
supported_resolutions = []
for width, height in common_resolutions:
# 尝试设置分辨率
cap.set(cv2.CAP_PROP_FRAME_WIDTH, width)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
# 读取实际设置的分辨率
actual_width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
actual_height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
# 检查是否设置成功
if actual_width == width and actual_height == height:
supported_resolutions.append((width, height))
print(f" ✅ {width}x{height}")
else:
print(f" ❌ {width}x{height} (实际: {actual_width}x{actual_height})")
return supported_resolutions
def test_supported_fps(cap, camera_index, test_width=640, test_height=480):
"""
测试相机支持的帧率范围 [10](@ref)
"""
print(f"\n🎬 测试相机 {camera_index} 支持的帧率 (分辨率: {test_width}x{test_height}):")
# 设置测试分辨率
cap.set(cv2.CAP_PROP_FRAME_WIDTH, test_width)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, test_height)
test_fps_values = [15, 24, 25, 30, 48, 50, 60, 90, 120]
supported_fps = []
for fps in test_fps_values:
# 尝试设置帧率
if cap.set(cv2.CAP_PROP_FPS, fps):
actual_fps = cap.get(cv2.CAP_PROP_FPS)
# 允许一定的误差
if abs(actual_fps - fps) <= 1.0:
supported_fps.append(fps)
print(f" ✅ {fps} FPS")
else:
print(f" ⚠️ {fps} FPS (实际: {actual_fps:.1f} FPS)")
else:
print(f" ❌ {fps} FPS")
return supported_fps
def get_camera_backend_info(cap, camera_index):
"""
获取相机后端信息 [1,8](@ref)
"""
try:
# 尝试获取后端名称（某些OpenCV版本支持）
backend_name = str(cap.getBackendName()) if hasattr(cap, 'getBackendName') else "未知"
# 根据后端判断相机类型
if "rtsp" in backend_name.lower() or "ffmpeg" in backend_name.lower():
camera_type = "网络相机"
else:
camera_type = "物理相机"
print(f"\n🔧 相机 {camera_index} 后端信息:")
print(f" 后端类型: {backend_name}")
print(f" 相机类型: {camera_type}")
return backend_name, camera_type
except:
print(f"\n🔧 相机 {camera_index} 后端信息: 无法获取")
return "未知", "未知"
def main():
"""
主函数
"""
print("📷 OpenCV相机信息检测工具")
print("=" * 50)
# 检查OpenCV版本
print(f"OpenCV版本: {cv2.__version__}")
print(f"Python版本: {sys.version}")
print(f"操作系统: {platform.system()} {platform.release()}")
# 1. 检测可用相机
available_cameras = list_available_cameras()
if not available_cameras:
print("❌ 未找到任何可用的相机设备")
return
# 2. 为每个相机获取详细信息
for camera_index in available_cameras:
cap = get_camera_properties(camera_index)
if cap is None:
continue
# 获取后端信息
backend_name, camera_type = get_camera_backend_info(cap, camera_index)
# 测试支持的分辨率
supported_res = test_supported_resolutions(cap, camera_index)
# 测试支持的帧率（使用VGA分辨率进行测试）
supported_fps = test_supported_fps(cap, camera_index, 640, 480)
# 恢复默认设置
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
cap.set(cv2.CAP_PROP_FPS, 30)
# 释放相机资源
cap.release()
print(f"\n📋 相机 {camera_index} 支持汇总:")
print(f" 分辨率: {len(supported_res)} 种")
for res in supported_res:
print(f" - {res[0]}x{res[1]}")
print(f" 帧率: {supported_fps}")
print(f" 类型: {camera_type}")
print(f"{'='*60}\n")
if __name__ == "__main__":
main()

48
skylink_ros2_ws/src/skylink_bridge/test/test_cam.py Normal file
View File

@@ -0,0 +1,48 @@
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
class OpenCVCam(Node):
def __init__(self):
super().__init__('opencv_cam_node')
# 发布话题，名字和 params.yaml 里的一致
self.publisher_ = self.create_publisher(Image, '/camera/image_raw', 10)
self.timer = self.create_timer(0.033, self.timer_callback) # 30 FPS
self.bridge = CvBridge()
# 打开 0 号摄像头
self.cap = cv2.VideoCapture(6)
# ====================================================
# 核心：强制使用 MJPEG，解决卡顿的关键！
# ====================================================
self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc('M', 'J', 'P', 'G'))
self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
self.cap.set(cv2.CAP_PROP_FPS, 60)
if not self.cap.isOpened():
print("❌ 无法打开摄像头！")
exit()
print(f"✅ 摄像头已打开 (MJPEG模式): 640x480 @ 60FPS")
def timer_callback(self):
ret, frame = self.cap.read()
if ret:
# 转成 ROS 消息并发布
msg = self.bridge.cv2_to_imgmsg(frame, encoding="bgr8")
self.publisher_.publish(msg)
# print("已发布一帧...") # 可以在这里看有没有在跑
def main(args=None):
rclpy.init(args=args)
node = OpenCVCam()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()