diff --git a/gitignore b/gitignore index 65cb7c8..f92f890 100644 --- a/gitignore +++ b/gitignore @@ -2,6 +2,8 @@ # Gradle files .gradle/ build/ +install/ +log/ */build/ log/ # Local configuration file (sdk path, etc) diff --git a/skylink_ros2_ws/build.sh b/skylink_ros2_ws/build.sh index 2c8f24c..1d8d08a 100755 --- a/skylink_ros2_ws/build.sh +++ b/skylink_ros2_ws/build.sh @@ -3,6 +3,10 @@ # SkyLink ROS 2 机载端 - 快速编译脚本 # 用法: ./build.sh [release|debug] +# 加载git分支 +# 如果是python分支，则加载python相关依赖 +# 如果是cpp分支，则加载cpp相关依赖 + set -e SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" @@ -65,4 +69,6 @@ echo " source $SCRIPT_DIR/install/setup.bash" echo "" echo "2. 启动节点:" echo " ros2 launch skylink_bridge bridge.launch.py" +echo "python 版本节点启动命令:" +echo " ros2 launch skylink_bridge_python bridge.launch.py" echo "" diff --git a/skylink_ros2_ws/src/skylink_bridge/CMakeLists.txt b/skylink_ros2_ws/src/skylink_bridge/CMakeLists.txt deleted file mode 100644 index 44a7ee6..0000000 --- a/skylink_ros2_ws/src/skylink_bridge/CMakeLists.txt +++ /dev/null @@ -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() diff --git a/skylink_ros2_ws/src/skylink_bridge/include/protocol.h b/skylink_ros2_ws/src/skylink_bridge/include/protocol.h deleted file mode 100644 index 97ee678..0000000 --- a/skylink_ros2_ws/src/skylink_bridge/include/protocol.h +++ /dev/null @@ -1,171 +0,0 @@ -/** - * @file protocol.h - * @brief SkyLink 多平台实时视频传输 - 公共协议定义 - * - * 这个文件定义了机载端（ROS 2）、PC 端（Qt）和移动端（Android）之间的通信协议。 - * 必须保持三端的字节对齐和字段顺序一致。 - * - * @author flowzl - * @date 2026-01-19 - */ - -#pragma once - -#include -#include - -/** - * @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; -} diff --git a/skylink_ros2_ws/src/skylink_bridge/include/skylink_bridge/device_info_sender_node.h b/skylink_ros2_ws/src/skylink_bridge/include/skylink_bridge/device_info_sender_node.h deleted file mode 100644 index 9dff435..0000000 --- a/skylink_ros2_ws/src/skylink_bridge/include/skylink_bridge/device_info_sender_node.h +++ /dev/null @@ -1,58 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -#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 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_; -}; diff --git a/skylink_ros2_ws/src/skylink_bridge/include/skylink_bridge/udp_sender_node.h b/skylink_ros2_ws/src/skylink_bridge/include/skylink_bridge/udp_sender_node.h deleted file mode 100644 index 0e4e4ae..0000000 --- a/skylink_ros2_ws/src/skylink_bridge/include/skylink_bridge/udp_sender_node.h +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// 协议定义 -#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::SharedPtr camera_subscriber_; - - /** GPS 全球位置订阅 */ - rclcpp::Subscription::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 bytes_sent_{0}; ///< 已发送字节数 - std::atomic packets_sent_{0}; ///< 已发送包数 - std::atomic frames_dropped_{0}; ///< 丢弃的帧数 - - // ======================================================================== - // 异步发送队列和线程 - // ======================================================================== - - /** UDP 数据包队列项 */ - struct PacketQueueItem { - std::vector data; - std::chrono::system_clock::time_point enqueue_time; - }; - - std::queue send_queue_; ///< 发送队列 - std::mutex send_queue_mutex_; ///< 队列互斥锁 - std::condition_variable send_queue_cv_; ///< 队列条件变量 - std::thread sender_thread_; ///< 后台发送线程 - std::atomic 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>& 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& 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(); -}; - diff --git a/skylink_ros2_ws/src/skylink_bridge/package.xml b/skylink_ros2_ws/src/skylink_bridge/package.xml deleted file mode 100644 index a9de59f..0000000 --- a/skylink_ros2_ws/src/skylink_bridge/package.xml +++ /dev/null @@ -1,41 +0,0 @@ - - - - skylink_bridge - 1.0.0 - - SkyLink 实时视频传输系统 - ROS 2 机载端 - 负责采集相机和 GPS 数据，通过 UDP 广播发送到地面站和移动端 - - - - flowzl - MIT - - - SkyLink Development - - - ament_cmake - - - rclcpp - sensor_msgs - geometry_msgs - cv_bridge - image_transport - - - opencv-dev - libjpeg-turbo-dev - - - mavros_msgs - - - usb_cam - - - ament_cmake - - diff --git a/skylink_ros2_ws/src/skylink_bridge/src/camera_driver_node.cpp b/skylink_ros2_ws/src/skylink_bridge/src/camera_driver_node.cpp deleted file mode 100644 index 66c8551..0000000 --- a/skylink_ros2_ws/src/skylink_bridge/src/camera_driver_node.cpp +++ /dev/null @@ -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 -#include -#include -#include -#include -#include - -// V4L2 includes -#include -#include -#include -#include -#include -#include - -/** - * @class CameraDriverNode - * @brief V4L2 相机驱动 ROS 2 节点 (Raw MJPEG) - */ -class CameraDriverNode : public rclcpp::Node { -public: - /** - * @brief 构造函数 - */ - CameraDriverNode() : rclcpp::Node("camera_driver") { - // ==================================================================== - // 声明参数 - // ==================================================================== - - this->declare_parameter("video_device", "/dev/video4"); - this->declare_parameter("framerate", 30.0); - this->declare_parameter("image_width", 640); - this->declare_parameter("image_height", 480); - this->declare_parameter("camera_topic", "/camera/image_raw/compressed"); - this->declare_parameter("camera_frame", "camera"); - this->declare_parameter("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( - 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(image_width_); } - uint16_t image_height() const { return static_cast(image_height_); } - float target_fps() const { return static_cast(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 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::SharedPtr image_publisher_; - - /** 捕获线程 */ - std::thread capture_thread_; - std::atomic 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(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(); - 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( - 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(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( - 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(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/skylink_ros2_ws/src/skylink_bridge/src/device_info_sender_node.cpp b/skylink_ros2_ws/src/skylink_bridge/src/device_info_sender_node.cpp deleted file mode 100644 index c6b2bfc..0000000 --- a/skylink_ros2_ws/src/skylink_bridge/src/device_info_sender_node.cpp +++ /dev/null @@ -1,251 +0,0 @@ -#include "skylink_bridge/device_info_sender_node.h" - -#include -#include -#include -#include -#include -#include - -using namespace std::chrono_literals; - -namespace { -uint16_t read_uint16(const std::string& s) { - try { - return static_cast(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(&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("info_port", 10000); - this->declare_parameter("target_ip", "255.255.255.255"); - this->declare_parameter("enable_broadcast", true); - this->declare_parameter("enable_request_response", true); - this->declare_parameter("broadcast_interval_ms", 1000); - - this->declare_parameter("device_path", "/dev/video0"); - this->declare_parameter("device_name", "Unknown"); - this->declare_parameter("width", 640); - this->declare_parameter("height", 480); - this->declare_parameter("framerate", 30.0); - this->declare_parameter("pixel_format", "YUYV"); - - // 读取 - info_port_ = static_cast(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(this->get_parameter("width").as_int()); - height_ = static_cast(this->get_parameter("height").as_int()); - framerate_ = static_cast(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 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(&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 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(&sender), &sender_len); - if (n <= 0) { - std::this_thread::sleep_for(50ms); - continue; - } - - if (n < 2) { - continue; - } - - uint16_t magic = ntohs(*reinterpret_cast(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::system_clock::now().time_since_epoch()); - packet.timestamp = static_cast(now.count()); - packet.crc16 = 0; - return packet; -} - -uint64_t DeviceInfoSenderNode::htonll(uint64_t value) { - static const int num = 42; - if (*reinterpret_cast(&num) == num) { - // little-endian - uint32_t high_part = htonl(static_cast(value >> 32)); - uint32_t low_part = htonl(static_cast(value & 0xFFFFFFFFULL)); - return (static_cast(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(fmt[0]) & 0xFF) | - ((static_cast(fmt[1]) & 0xFF) << 8) | - ((static_cast(fmt[2]) & 0xFF) << 16) | - ((static_cast(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(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/skylink_ros2_ws/src/skylink_bridge/src/udp_sender_node.cpp b/skylink_ros2_ws/src/skylink_bridge/src/udp_sender_node.cpp deleted file mode 100644 index a550919..0000000 --- a/skylink_ros2_ws/src/skylink_bridge/src/udp_sender_node.cpp +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/** - * @brief 构造函数 - */ -UdpSenderNode::UdpSenderNode() : rclcpp::Node("udp_sender_node"), udp_socket_(-1), frame_counter_(0) { - // ======================================================================== - // 声明和读取参数 - // ======================================================================== - - this->declare_parameter("target_ip", "255.255.255.255"); - this->declare_parameter("target_port", 9999); - this->declare_parameter("camera_topic", "/camera/image_raw"); - this->declare_parameter("gps_topic", "/mavros/global_position/global"); - this->declare_parameter("jpeg_quality", 70); - this->declare_parameter("adaptive_quality", true); - this->declare_parameter("quality_min", 30); - this->declare_parameter("quality_max", 95); - this->declare_parameter("enable_gps", true); - this->declare_parameter("max_packet_size", 500); - this->declare_parameter("frame_timeout_ms", 5000); - this->declare_parameter("enable_fps_stats", true); - this->declare_parameter("enable_drop_stats", true); - - // 读取参数 - target_ip_ = this->get_parameter("target_ip").as_string(); - target_port_ = static_cast(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(this->get_parameter("max_packet_size").as_int()); - frame_timeout_ms_ = static_cast(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( - 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( - 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(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>& packets) { - { - std::lock_guard 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 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 batch; - batch.reserve(BATCH_SEND_SIZE); - - while (sender_running_) { - try { - batch.clear(); - - { - std::unique_lock 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& jpeg_data = msg->data; - - // 获取当前 GPS 数据 - double lat, lon, alt; - { - std::lock_guard 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 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& output) { - try { - std::vector 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> 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 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( - now - stats_.last_report_time - ).count(); - - // 获取当前队列深度 - size_t current_queue_depth = 0; - { - std::lock_guard 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(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/skylink_ros2_ws/src/skylink_bridge/config/params.yaml b/skylink_ros2_ws/src/skylink_bridge_python/config/params.yaml similarity index 68% rename from skylink_ros2_ws/src/skylink_bridge/config/params.yaml rename to skylink_ros2_ws/src/skylink_bridge_python/config/params.yaml index d89c6ef..2569e87 100644 --- a/skylink_ros2_ws/src/skylink_bridge/config/params.yaml +++ b/skylink_ros2_ws/src/skylink_bridge_python/config/params.yaml @@ -9,13 +9,15 @@ /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 + video_device: "/dev/video2" + + image_width: 3840 + image_height: 2160 + # 拍照功能配置 + save_dir: "./camera_data" + warmup_frames: 20 # 拍照前预热帧数，防止刚启动时过曝 + interval: 5.0 # 自动拍照间隔（秒），设为 0 则仅通过服务触发拍照 + gps_topic: "/gps_info" /udp_sender: ros__parameters: @@ -26,7 +28,7 @@ sender_thread_name: "skylink_sender" # 相机相关（udp_sender 使用的 topic / 图像格式） - camera_topic: "/camera/image_raw/compressed" + camera_topic: "/camera/image_raw" camera_frame: "bgr8" # JPEG 压缩配置 @@ -50,17 +52,3 @@ 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" diff --git a/skylink_ros2_ws/src/skylink_bridge/launch/bridge.launch.py b/skylink_ros2_ws/src/skylink_bridge_python/launch/bridge.launch.py similarity index 78% rename from skylink_ros2_ws/src/skylink_bridge/launch/bridge.launch.py rename to skylink_ros2_ws/src/skylink_bridge_python/launch/bridge.launch.py index eb2cb74..2ca84bb 100644 --- a/skylink_ros2_ws/src/skylink_bridge/launch/bridge.launch.py +++ b/skylink_ros2_ws/src/skylink_bridge_python/launch/bridge.launch.py @@ -10,12 +10,12 @@ 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 + ros2 launch skylink_bridge_python bridge.launch.py + 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') # ======================================================================== @@ -34,18 +34,6 @@ def generate_launch_description(): 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', @@ -56,15 +44,12 @@ def generate_launch_description(): # ======================================================================== camera_driver_node = Node( - package='skylink_bridge', + package='skylink_bridge_python', executable='camera_driver_node', name='camera_driver', output='screen', parameters=[ config_file, # 从 YAML 配置文件加载参数 - { - 'camera_topic': LaunchConfiguration('camera_topic'), - } ], ) @@ -73,7 +58,7 @@ def generate_launch_description(): # ======================================================================== udp_sender_node = Node( - package='skylink_bridge', + package='skylink_bridge_python', executable='udp_sender_node', name='udp_sender', output='screen', @@ -100,9 +85,7 @@ def generate_launch_description(): return LaunchDescription([ target_ip_arg, target_port_arg, - jpeg_quality_arg, - camera_topic_arg, gps_topic_arg, camera_driver_node, - udp_sender_node, + # udp_sender_node, ]) diff --git a/skylink_ros2_ws/src/skylink_bridge_python/package.xml b/skylink_ros2_ws/src/skylink_bridge_python/package.xml new file mode 100644 index 0000000..a316136 --- /dev/null +++ b/skylink_ros2_ws/src/skylink_bridge_python/package.xml @@ -0,0 +1,23 @@ + + + + skylink_bridge_python + 0.0.0 + TODO: Package description + flower + TODO: License declaration + + + python3-pytest + + + rclpy + std_msgs + std_srvs + sensor_msgs + ffmpeg + + + ament_python + + diff --git a/skylink_ros2_ws/src/skylink_bridge/test/cam_list.py b/skylink_ros2_ws/src/skylink_bridge_python/pkg_test/cam_list.py similarity index 100% rename from skylink_ros2_ws/src/skylink_bridge/test/cam_list.py rename to skylink_ros2_ws/src/skylink_bridge_python/pkg_test/cam_list.py diff --git a/skylink_ros2_ws/src/skylink_bridge/test/test_cam.py b/skylink_ros2_ws/src/skylink_bridge_python/pkg_test/cam_test.py similarity index 100% rename from skylink_ros2_ws/src/skylink_bridge/test/test_cam.py rename to skylink_ros2_ws/src/skylink_bridge_python/pkg_test/cam_test.py diff --git a/skylink_ros2_ws/src/skylink_bridge_python/resource/skylink_bridge_python b/skylink_ros2_ws/src/skylink_bridge_python/resource/skylink_bridge_python new file mode 100644 index 0000000..e69de29 diff --git a/skylink_ros2_ws/src/skylink_bridge_python/setup.cfg b/skylink_ros2_ws/src/skylink_bridge_python/setup.cfg new file mode 100644 index 0000000..45ea361 --- /dev/null +++ b/skylink_ros2_ws/src/skylink_bridge_python/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/skylink_bridge_python +[install] +install_scripts=$base/lib/skylink_bridge_python diff --git a/skylink_ros2_ws/src/skylink_bridge_python/setup.py b/skylink_ros2_ws/src/skylink_bridge_python/setup.py new file mode 100644 index 0000000..8a96548 --- /dev/null +++ b/skylink_ros2_ws/src/skylink_bridge_python/setup.py @@ -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' + ], + }, +) diff --git a/skylink_ros2_ws/src/skylink_bridge_python/skylink_bridge_python/__init__.py b/skylink_ros2_ws/src/skylink_bridge_python/skylink_bridge_python/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/skylink_ros2_ws/src/skylink_bridge_python/skylink_bridge_python/__pycache__/__init__.cpython-310.pyc b/skylink_ros2_ws/src/skylink_bridge_python/skylink_bridge_python/__pycache__/__init__.cpython-310.pyc new file mode 100644 index 0000000..8e2faaa Binary files /dev/null and b/skylink_ros2_ws/src/skylink_bridge_python/skylink_bridge_python/__pycache__/__init__.cpython-310.pyc differ diff --git a/skylink_ros2_ws/src/skylink_bridge_python/skylink_bridge_python/__pycache__/camera_driver_node.cpython-310.pyc b/skylink_ros2_ws/src/skylink_bridge_python/skylink_bridge_python/__pycache__/camera_driver_node.cpython-310.pyc new file mode 100644 index 0000000..eb59570 Binary files /dev/null and b/skylink_ros2_ws/src/skylink_bridge_python/skylink_bridge_python/__pycache__/camera_driver_node.cpython-310.pyc differ diff --git a/skylink_ros2_ws/src/skylink_bridge_python/skylink_bridge_python/camera_driver_node.py b/skylink_ros2_ws/src/skylink_bridge_python/skylink_bridge_python/camera_driver_node.py new file mode 100644 index 0000000..05cb4bf --- /dev/null +++ b/skylink_ros2_ws/src/skylink_bridge_python/skylink_bridge_python/camera_driver_node.py @@ -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() diff --git a/skylink_ros2_ws/src/skylink_bridge_python/test/test_copyright.py b/skylink_ros2_ws/src/skylink_bridge_python/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/skylink_ros2_ws/src/skylink_bridge_python/test/test_copyright.py @@ -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' diff --git a/skylink_ros2_ws/src/skylink_bridge_python/test/test_flake8.py b/skylink_ros2_ws/src/skylink_bridge_python/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/skylink_ros2_ws/src/skylink_bridge_python/test/test_flake8.py @@ -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) diff --git a/skylink_ros2_ws/src/skylink_bridge_python/test/test_pep257.py b/skylink_ros2_ws/src/skylink_bridge_python/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/skylink_ros2_ws/src/skylink_bridge_python/test/test_pep257.py @@ -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'