拍照version
This commit is contained in:
flower
@@ -22,7 +22,7 @@ if [ -z "$ROS_DISTRO" ]; then
|
||||
exit 1
|
||||
fi
|
||||
|
||||
echo "✓ ROS 2 发行版: $ROS_DISTRO"
|
||||
echo " ROS 2 发行版: $ROS_DISTRO"
|
||||
|
||||
# 检查依赖
|
||||
echo ""
|
||||
@@ -33,14 +33,14 @@ if ! command -v colcon &> /dev/null; then
|
||||
exit 1
|
||||
fi
|
||||
|
||||
echo "✓ colcon 已安装"
|
||||
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 "✓ 清理完成"
|
||||
echo " 清理完成"
|
||||
fi
|
||||
|
||||
# 编译
|
||||
@@ -56,7 +56,7 @@ fi
|
||||
|
||||
echo ""
|
||||
echo "========================================"
|
||||
echo "✓ 编译成功！"
|
||||
echo " 编译成功！"
|
||||
echo "========================================"
|
||||
echo ""
|
||||
echo "下一步："
|
||||
|
||||
72
skylink_ros2_ws/sent_log.txt
Normal file
72
skylink_ros2_ws/sent_log.txt
Normal file
@@ -0,0 +1,72 @@
|
||||
--- Session Started: 2026-02-02 15:40:47 ---
|
||||
--- Session Started: 2026-02-02 16:12:35 ---
|
||||
--- Session Started: 2026-02-02 16:14:58 ---
|
||||
--- Session Started: 2026-02-02 16:16:59 ---
|
||||
[2026-02-02 16:17:00] Sent: photo_20260202_161700_000.jpg Size: 551838 Total Sent: 1
|
||||
[2026-02-02 16:17:00] Sent: photo_20260202_161700_001.jpg Size: 411948 Total Sent: 2
|
||||
[2026-02-02 16:17:01] Sent: photo_20260202_161701_002.jpg Size: 376731 Total Sent: 3
|
||||
[2026-02-02 16:17:01] Sent: photo_20260202_161701_003.jpg Size: 377754 Total Sent: 4
|
||||
[2026-02-02 16:17:02] Sent: photo_20260202_161702_004.jpg Size: 371593 Total Sent: 5
|
||||
[2026-02-02 16:17:02] Sent: photo_20260202_161702_005.jpg Size: 385420 Total Sent: 6
|
||||
--- Session Started: 2026-02-02 16:23:14 ---
|
||||
[2026-02-02 16:23:14] Sent: photo_20260202_161700_000.jpg Size: 551838 Total Sent: 1
|
||||
[2026-02-02 16:23:14] Sent: photo_20260202_161700_001.jpg Size: 411948 Total Sent: 2
|
||||
[2026-02-02 16:23:14] Sent: photo_20260202_161701_002.jpg Size: 376731 Total Sent: 3
|
||||
[2026-02-02 16:23:14] Sent: photo_20260202_161701_003.jpg Size: 377754 Total Sent: 4
|
||||
[2026-02-02 16:23:14] Sent: photo_20260202_161702_004.jpg Size: 371593 Total Sent: 5
|
||||
[2026-02-02 16:23:14] Sent: photo_20260202_161702_005.jpg Size: 385420 Total Sent: 6
|
||||
[2026-02-02 16:23:14] Sent: photo_20260202_162314_000.jpg Size: 594448 Total Sent: 7
|
||||
[2026-02-02 16:23:15] Sent: photo_20260202_162315_001.jpg Size: 416092 Total Sent: 8
|
||||
[2026-02-02 16:23:15] Sent: photo_20260202_162315_002.jpg Size: 448451 Total Sent: 9
|
||||
[2026-02-02 16:23:16] Sent: photo_20260202_162316_003.jpg Size: 447612 Total Sent: 10
|
||||
[2026-02-02 16:23:16] Sent: photo_20260202_162316_004.jpg Size: 441193 Total Sent: 11
|
||||
[2026-02-02 16:23:17] Sent: photo_20260202_162317_005.jpg Size: 465881 Total Sent: 12
|
||||
[2026-02-02 16:23:17] Sent: photo_20260202_162317_006.jpg Size: 446361 Total Sent: 13
|
||||
[2026-02-02 16:23:18] Sent: photo_20260202_162318_007.jpg Size: 415256 Total Sent: 14
|
||||
[2026-02-02 16:23:18] Sent: photo_20260202_162318_008.jpg Size: 415862 Total Sent: 15
|
||||
[2026-02-02 16:23:19] Sent: photo_20260202_162319_009.jpg Size: 415374 Total Sent: 16
|
||||
--- Session Started: 2026-02-02 16:28:39 ---
|
||||
[2026-02-02 16:28:39] Sent: photo_000_20260202_162839_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 584359 Total Sent: 1
|
||||
[2026-02-02 16:28:40] Sent: photo_001_20260202_162840_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 583744 Total Sent: 2
|
||||
[2026-02-02 16:28:40] Sent: photo_002_20260202_162840_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 583338 Total Sent: 3
|
||||
[2026-02-02 16:28:41] Sent: photo_003_20260202_162841_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 571135 Total Sent: 4
|
||||
[2026-02-02 16:28:41] Sent: photo_004_20260202_162841_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 570012 Total Sent: 5
|
||||
[2026-02-02 16:28:42] Sent: photo_005_20260202_162842_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 570419 Total Sent: 6
|
||||
[2026-02-02 16:28:42] Sent: photo_006_20260202_162842_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 582702 Total Sent: 7
|
||||
[2026-02-02 16:28:43] Sent: photo_007_20260202_162843_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 582108 Total Sent: 8
|
||||
--- Session Started: 2026-02-02 16:32:10 ---
|
||||
[2026-02-02 16:32:13] Sent: photo_000_20260202_163213_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 559873 Total Sent: 1
|
||||
[2026-02-02 16:32:15] Sent: photo_001_20260202_163215_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 559766 Total Sent: 2
|
||||
--- Session Started: 2026-02-02 16:32:54 ---
|
||||
[2026-02-02 16:32:55] Sent: photo_000_20260202_163213_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 559873 Total Sent: 1
|
||||
[2026-02-02 16:32:55] Sent: photo_001_20260202_163215_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 559766 Total Sent: 2
|
||||
[2026-02-02 16:32:56] Sent: photo_000_20260202_163256_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 558412 Total Sent: 3
|
||||
[2026-02-02 16:32:57] Sent: photo_001_20260202_163257_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 601159 Total Sent: 4
|
||||
[2026-02-02 16:32:58] Sent: photo_002_20260202_163258_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 558166 Total Sent: 5
|
||||
[2026-02-02 16:32:59] Sent: photo_003_20260202_163259_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 600601 Total Sent: 6
|
||||
[2026-02-02 16:33:00] Sent: photo_004_20260202_163300_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 600806 Total Sent: 7
|
||||
--- Session Started: 2026-02-02 16:34:57 ---
|
||||
--- Session Started: 2026-02-02 16:36:29 ---
|
||||
[2026-02-02 16:39:13] Sent: photo_000_20260202_163913_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 500250 Total Sent: 1
|
||||
[2026-02-02 16:39:14] Sent: photo_001_20260202_163914_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 499410 Total Sent: 2
|
||||
[2026-02-02 16:39:15] Sent: photo_002_20260202_163915_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 499874 Total Sent: 3
|
||||
[2026-02-02 16:39:16] Sent: photo_003_20260202_163916_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 499487 Total Sent: 4
|
||||
[2026-02-02 16:39:17] Sent: photo_004_20260202_163917_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 498681 Total Sent: 5
|
||||
[2026-02-02 16:39:52] Sent: photo_005_20260202_163952_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 603995 Total Sent: 6
|
||||
[2026-02-02 16:39:54] Sent: photo_006_20260202_163954_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 595479 Total Sent: 7
|
||||
[2026-02-02 16:39:56] Sent: photo_007_20260202_163956_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 643671 Total Sent: 8
|
||||
[2026-02-02 16:39:58] Sent: photo_008_20260202_163958_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 623669 Total Sent: 9
|
||||
[2026-02-02 16:40:00] Sent: photo_009_20260202_164000_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 601373 Total Sent: 10
|
||||
[2026-02-02 16:40:17] Sent: photo_010_20260202_164017_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 630691 Total Sent: 11
|
||||
[2026-02-02 16:40:19] Sent: photo_011_20260202_164019_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 687730 Total Sent: 12
|
||||
[2026-02-02 16:40:21] Sent: photo_012_20260202_164021_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 653137 Total Sent: 13
|
||||
[2026-02-02 16:40:23] Sent: photo_013_20260202_164023_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 628483 Total Sent: 14
|
||||
--- Session Started: 2026-02-02 16:53:03 ---
|
||||
[2026-02-02 16:53:30] Sent: photo_000_20260202_165330_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 473529 Total Sent: 1
|
||||
[2026-02-02 16:53:32] Sent: photo_001_20260202_165332_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 473852 Total Sent: 2
|
||||
[2026-02-02 16:53:34] Sent: photo_002_20260202_165334_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 473945 Total Sent: 3
|
||||
[2026-02-02 16:53:36] Sent: photo_003_20260202_165336_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 606768 Total Sent: 4
|
||||
[2026-02-02 16:54:24] Sent: photo_004_20260202_165424_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 583563 Total Sent: 5
|
||||
[2026-02-02 16:54:26] Sent: photo_005_20260202_165426_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 602259 Total Sent: 6
|
||||
[2026-02-02 16:54:28] Sent: photo_006_20260202_165428_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 597611 Total Sent: 7
|
||||
[2026-02-02 16:54:30] Sent: photo_007_20260202_165430_GPS_lat0.000000_lon0.000000_alt0.0.jpg Size: 583412 Total Sent: 8
|
||||
@@ -1,5 +1,5 @@
|
||||
cmake_minimum_required(VERSION 3.16)
|
||||
project(skylink_bridge LANGUAGES CXX)
|
||||
project(skylink_bridge LANGUAGES C CXX)
|
||||
|
||||
# ============================================================================
|
||||
# 编译标准和优化选项
|
||||
@@ -25,6 +25,7 @@ find_package(geometry_msgs REQUIRED)
|
||||
find_package(cv_bridge REQUIRED)
|
||||
find_package(image_transport REQUIRED)
|
||||
find_package(mavros_msgs REQUIRED)
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
|
||||
# OpenCV
|
||||
find_package(OpenCV REQUIRED COMPONENTS core imgproc)
|
||||
@@ -43,6 +44,15 @@ include_directories(
|
||||
include
|
||||
)
|
||||
|
||||
# ============================================================================
|
||||
# 生成服务接口
|
||||
# ============================================================================
|
||||
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"srv/TriggerCapture.srv"
|
||||
DEPENDENCIES
|
||||
)
|
||||
|
||||
# ============================================================================
|
||||
# 源文件
|
||||
# ============================================================================
|
||||
@@ -56,6 +66,8 @@ set(SOURCES
|
||||
set(HEADERS
|
||||
include/skylink_bridge/udp_sender_node.h
|
||||
include/skylink_bridge/device_info_sender_node.h
|
||||
include/skylink_bridge/camera_driver_node.h
|
||||
include/skylink_bridge/httplib.h
|
||||
include/protocol.h
|
||||
)
|
||||
|
||||
@@ -64,7 +76,7 @@ set(HEADERS
|
||||
# ============================================================================
|
||||
|
||||
add_executable(udp_sender_node src/udp_sender_node.cpp ${HEADERS})
|
||||
add_executable(camera_driver_node src/camera_driver_node.cpp)
|
||||
add_executable(camera_driver_node src/camera_driver_node.cpp ${HEADERS})
|
||||
add_executable(device_info_sender_node src/device_info_sender_node.cpp)
|
||||
|
||||
# 链接库 - UDP Sender
|
||||
@@ -84,9 +96,14 @@ target_link_libraries(udp_sender_node
|
||||
target_link_libraries(camera_driver_node
|
||||
${rclcpp_LIBRARIES}
|
||||
${sensor_msgs_LIBRARIES}
|
||||
${OpenCV_LIBRARIES}
|
||||
Threads::Threads
|
||||
)
|
||||
|
||||
# 链接服务接口库
|
||||
rosidl_target_interfaces(camera_driver_node
|
||||
${PROJECT_NAME} "rosidl_typesupport_cpp")
|
||||
|
||||
target_link_libraries(device_info_sender_node
|
||||
${rclcpp_LIBRARIES}
|
||||
Threads::Threads
|
||||
@@ -107,6 +124,11 @@ ament_target_dependencies(camera_driver_node
|
||||
sensor_msgs
|
||||
)
|
||||
|
||||
# 链接服务接口库
|
||||
rosidl_get_typesupport_target(cpp_typesupport_target
|
||||
${PROJECT_NAME} "rosidl_typesupport_cpp")
|
||||
target_link_libraries(camera_driver_node "${cpp_typesupport_target}")
|
||||
|
||||
ament_target_dependencies(device_info_sender_node
|
||||
rclcpp
|
||||
)
|
||||
@@ -139,6 +161,11 @@ install(DIRECTORY include/
|
||||
DESTINATION include
|
||||
)
|
||||
|
||||
# 安装服务定义文件
|
||||
install(DIRECTORY srv/
|
||||
DESTINATION share/${PROJECT_NAME}/srv
|
||||
)
|
||||
|
||||
# ============================================================================
|
||||
# Ament 导出
|
||||
# ============================================================================
|
||||
@@ -151,6 +178,7 @@ ament_export_dependencies(
|
||||
image_transport
|
||||
mavros_msgs
|
||||
OpenCV
|
||||
rosidl_default_runtime
|
||||
)
|
||||
|
||||
ament_package()
|
||||
|
||||
@@ -9,14 +9,28 @@
|
||||
/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" # OpenCV 摄像头索引，例如 0 代表 /dev/video0，2 代表 /dev/video2
|
||||
|
||||
image_width: 3840
|
||||
image_height: 2160
|
||||
|
||||
# 调试参数
|
||||
frame_rate: 30.0 # 帧率
|
||||
fourcc: "MJPG" # 视频流格式: MJPG, YUYV 等
|
||||
auto_exposure: true # 自动曝光开关: true=自动, false=手动
|
||||
exposure_value: -5.0 # 曝光值 (仅在 auto_exposure=false 时生效, 具体范围取决于设备，通常是负数或很小的正数)
|
||||
|
||||
# 拍照功能配置
|
||||
save_dir: "./camera_data"
|
||||
interval: 5.0 # 周期
|
||||
photo_count: 5 # 周期内拍照数量
|
||||
gps_topic: "/gps_info"
|
||||
|
||||
capture_enabled: false # 是否启用周期拍照功能
|
||||
|
||||
# HTTP REST API 配置
|
||||
http_port: 8080 # HTTP API 服务器端口
|
||||
http_enabled: true # 是否启用 HTTP 服务器
|
||||
/udp_sender:
|
||||
ros__parameters:
|
||||
# 网络与发送配置（对应 udp_sender 节点）
|
||||
@@ -26,10 +40,11 @@
|
||||
sender_thread_name: "skylink_sender"
|
||||
|
||||
# 相机相关（udp_sender 使用的 topic / 图像格式）
|
||||
camera_topic: "/camera/image_raw/compressed"
|
||||
camera_frame: "bgr8"
|
||||
# camera_topic: "/camera/image_raw" # 已移除，改为文件监控
|
||||
save_dir: "./camera_data" # 监控目录，与 camera_driver 的 save_dir 一致
|
||||
log_file: "./sent_log.txt" # 发送日志文件
|
||||
|
||||
# JPEG 压缩配置
|
||||
# JPEG 压缩配置 (对于文件模式，这些如果不压缩可能忽略，但保持原样)
|
||||
jpeg_quality: 80
|
||||
adaptive_quality: false # 禁用自适应，使用固定质量以获得稳定帧率
|
||||
quality_min: 30
|
||||
@@ -50,17 +65,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"
|
||||
|
||||
@@ -0,0 +1,260 @@
|
||||
/**
|
||||
* @file camera_driver_node.h
|
||||
* @brief V4L2 相机驱动节点 - 头文件
|
||||
*
|
||||
* 使用 V4L2 直接读取 USB 摄像头 MJPEG 流，支持定时拍照和服务触发拍照
|
||||
*
|
||||
* @author flowzl
|
||||
* @date 2026-01-19
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <sensor_msgs/msg/nav_sat_fix.hpp>
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <vector>
|
||||
#include <fstream>
|
||||
#include <filesystem>
|
||||
#include <iomanip>
|
||||
#include <sstream>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
|
||||
// V4L2 includes
|
||||
#include <linux/videodev2.h>
|
||||
#include <fcntl.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/mman.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/stat.h>
|
||||
|
||||
// 服务定义
|
||||
#include "skylink_bridge/srv/trigger_capture.hpp"
|
||||
|
||||
// OpenCV for thumbnail generation
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <opencv2/imgproc.hpp>
|
||||
|
||||
// HTTP server
|
||||
#include "skylink_bridge/httplib.h"
|
||||
|
||||
/**
|
||||
* @class CameraDriverNode
|
||||
* @brief V4L2 相机驱动 ROS 2 节点
|
||||
*/
|
||||
class CameraDriverNode : public rclcpp::Node {
|
||||
public:
|
||||
/**
|
||||
* @brief 构造函数
|
||||
*/
|
||||
CameraDriverNode();
|
||||
|
||||
/**
|
||||
* @brief 析构函数
|
||||
*/
|
||||
~CameraDriverNode();
|
||||
|
||||
// 设备信息查询接口
|
||||
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_; }
|
||||
uint32_t total_photos() const { return total_photos_saved_.load(); }
|
||||
|
||||
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_;
|
||||
bool enable_fps_stats_;
|
||||
|
||||
/** GPS 订阅 */
|
||||
std::string gps_topic_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr gps_subscription_;
|
||||
sensor_msgs::msg::NavSatFix latest_gps_fix_;
|
||||
std::mutex gps_mutex_;
|
||||
bool has_gps_fix_ = false;
|
||||
|
||||
/** 服务接口 */
|
||||
rclcpp::Service<skylink_bridge::srv::TriggerCapture>::SharedPtr trigger_service_;
|
||||
|
||||
/** HTTP 服务器相关 */
|
||||
std::thread http_server_thread_;
|
||||
std::atomic<bool> http_server_running_{false};
|
||||
int http_port_ = 8080;
|
||||
bool http_enabled_ = true;
|
||||
|
||||
/** 捕获线程 */
|
||||
std::thread capture_thread_;
|
||||
std::atomic<bool> capturing_{false};
|
||||
|
||||
/** 定时拍照相关 */
|
||||
rclcpp::TimerBase::SharedPtr capture_timer_;
|
||||
bool capture_enabled_; // 是否启用定时拍照
|
||||
double capture_interval_; // 拍照间隔（秒）
|
||||
int photo_count_per_batch_; // 每批拍照数量
|
||||
int current_photo_index_; // 当前批次中的照片索引
|
||||
std::atomic<bool> is_capturing_{false}; // 是否正在拍照
|
||||
std::mutex capture_mutex_; // 拍照互斥锁
|
||||
|
||||
/** 照片保存 */
|
||||
std::string save_dir_;
|
||||
std::atomic<uint32_t> total_photos_saved_{0}; // 总照片数
|
||||
|
||||
/** 传输模式 */
|
||||
enum class TransmissionMode {
|
||||
ORIGINAL, // 原图
|
||||
THUMBNAIL // 缩略图
|
||||
};
|
||||
TransmissionMode transmission_mode_ = TransmissionMode::ORIGINAL;
|
||||
int thumbnail_width_ = 640; // 缩略图宽度
|
||||
int thumbnail_height_ = 480; // 缩略图高度
|
||||
|
||||
/** 统计信息 */
|
||||
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;
|
||||
|
||||
/** 最新帧数据缓存（用于拍照） */
|
||||
std::vector<uint8_t> latest_frame_data_;
|
||||
std::mutex frame_data_mutex_;
|
||||
std::atomic<bool> has_frame_data_{false};
|
||||
|
||||
// ========================================================================
|
||||
// 私有方法
|
||||
// ========================================================================
|
||||
|
||||
// xioctl helper
|
||||
static int xioctl(int fh, int request, void *arg);
|
||||
|
||||
/**
|
||||
* @brief 关闭设备
|
||||
*/
|
||||
void close_device();
|
||||
|
||||
/**
|
||||
* @brief 释放内存映射
|
||||
*/
|
||||
void uninit_device();
|
||||
|
||||
/**
|
||||
* @brief 停止流
|
||||
*/
|
||||
void stop_capturing();
|
||||
|
||||
/**
|
||||
* @brief 初始化摄像头
|
||||
*/
|
||||
bool init_camera();
|
||||
|
||||
/**
|
||||
* @brief 捕获工作线程（持续获取帧数据并缓存）
|
||||
*/
|
||||
void capture_worker();
|
||||
|
||||
/**
|
||||
* @brief 拍照并保存
|
||||
* @param mode 传输模式（原图或缩略图）
|
||||
*/
|
||||
void capture_photo(TransmissionMode mode = TransmissionMode::ORIGINAL);
|
||||
|
||||
/**
|
||||
* @brief 生成缩略图
|
||||
* @param original_data 原始JPEG数据
|
||||
* @return 缩略图JPEG数据
|
||||
*/
|
||||
std::vector<uint8_t> generate_thumbnail(const std::vector<uint8_t>& original_data);
|
||||
|
||||
/**
|
||||
* @brief 启动批量拍照
|
||||
* @param duration 总时间（秒）
|
||||
* @param count 拍照数量
|
||||
* @param interval 时间间隔（秒）
|
||||
* @param mode 传输模式
|
||||
*/
|
||||
void start_batch_capture(double duration, int count, double interval, TransmissionMode mode);
|
||||
|
||||
/**
|
||||
* @brief 服务回调函数
|
||||
*/
|
||||
void service_callback(
|
||||
const std::shared_ptr<skylink_bridge::srv::TriggerCapture::Request> request,
|
||||
std::shared_ptr<skylink_bridge::srv::TriggerCapture::Response> response);
|
||||
|
||||
/**
|
||||
* @brief 报告统计信息 (Legacy)
|
||||
*/
|
||||
void report_stats();
|
||||
|
||||
// ========================================================================
|
||||
// HTTP 服务器方法
|
||||
// ========================================================================
|
||||
|
||||
/**
|
||||
* @brief 启动 HTTP 服务器
|
||||
*/
|
||||
void start_http_server();
|
||||
|
||||
/**
|
||||
* @brief 停止 HTTP 服务器
|
||||
*/
|
||||
void stop_http_server();
|
||||
|
||||
/**
|
||||
* @brief HTTP 服务器工作线程
|
||||
*/
|
||||
void http_server_worker();
|
||||
|
||||
/**
|
||||
* @brief 处理触发拍照 API 请求
|
||||
* @param req HTTP 请求
|
||||
* @param res HTTP 响应
|
||||
*/
|
||||
void handle_trigger_capture_api(const httplib::Request& req, httplib::Response& res);
|
||||
|
||||
/**
|
||||
* @brief 处理状态查询 API 请求
|
||||
* @param req HTTP 请求
|
||||
* @param res HTTP 响应
|
||||
*/
|
||||
void handle_status_api(const httplib::Request& req, httplib::Response& res);
|
||||
|
||||
/**
|
||||
* @brief 简单的 JSON 解析辅助函数
|
||||
* @param json_str JSON 字符串
|
||||
* @param key 键名
|
||||
* @param default_value 默认值
|
||||
* @return 解析的值或默认值
|
||||
*/
|
||||
template<typename T>
|
||||
T parse_json_value(const std::string& json_str, const std::string& key, T default_value) {
|
||||
// 默认实现返回默认值，特化版本在cpp中实现
|
||||
return default_value;
|
||||
}
|
||||
};
|
||||
16993
skylink_ros2_ws/src/skylink_bridge/include/skylink_bridge/httplib.h
Normal file
16993
skylink_ros2_ws/src/skylink_bridge/include/skylink_bridge/httplib.h
Normal file
File diff suppressed because it is too large
Load Diff
@@ -24,6 +24,9 @@
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
#include <cstring>
|
||||
#include <fstream>
|
||||
#include <filesystem>
|
||||
#include <set>
|
||||
|
||||
// 协议定义
|
||||
#include "protocol.h"
|
||||
@@ -33,10 +36,10 @@
|
||||
* @brief ROS 2 UDP 发送节点
|
||||
*
|
||||
* 负责：
|
||||
* 1. 订阅相机和 GPS 数据
|
||||
* 2. 编码图像为 JPEG
|
||||
* 3. 分片打包为 UDP 数据包
|
||||
* 4. 通过网络发送数据包
|
||||
* 1. 监控指定目录下的 JPEG 文件
|
||||
* 2. 读取文件并分片打包为 UDP 数据包
|
||||
* 3. 通过网络发送数据包
|
||||
* 4. 记录发送日志
|
||||
*/
|
||||
class UdpSenderNode : public rclcpp::Node {
|
||||
public:
|
||||
@@ -55,35 +58,42 @@ private:
|
||||
// ROS 2 订阅和发布
|
||||
// ========================================================================
|
||||
|
||||
/** 相机图像订阅 */
|
||||
rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr camera_subscriber_;
|
||||
|
||||
/** GPS 全球位置订阅 */
|
||||
/** GPS 订阅 */
|
||||
rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr gps_subscriber_;
|
||||
|
||||
/** 文件扫描定时器 */
|
||||
rclcpp::TimerBase::SharedPtr file_scan_timer_;
|
||||
|
||||
// ========================================================================
|
||||
// 参数
|
||||
// 成员变量
|
||||
// ========================================================================
|
||||
|
||||
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_; ///< 丢弃帧统计
|
||||
// 网络相关
|
||||
std::string target_ip_;
|
||||
uint16_t target_port_;
|
||||
int udp_socket_;
|
||||
|
||||
// ========================================================================
|
||||
// 套接字和网络
|
||||
// ========================================================================
|
||||
// GPS 数据
|
||||
std::string gps_topic_;
|
||||
bool enable_gps_;
|
||||
|
||||
int udp_socket_; ///< UDP 套接字文件描述符
|
||||
uint32_t frame_counter_; ///< 帧计数器
|
||||
// 配置参数
|
||||
uint16_t max_packet_size_;
|
||||
uint32_t frame_timeout_ms_;
|
||||
bool enable_fps_stats_;
|
||||
bool enable_drop_stats_;
|
||||
|
||||
// 图像源配置
|
||||
std::string save_dir_; // 图片保存目录
|
||||
std::string log_file_path_; // 日志文件路径
|
||||
std::set<std::string> sent_files_; // 已发送文件记录
|
||||
std::filesystem::file_time_type last_check_time_; // 上次检查时间
|
||||
|
||||
// 日志流
|
||||
std::ofstream log_file_stream_;
|
||||
|
||||
// 统计相关
|
||||
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}; ///< 丢弃的帧数
|
||||
@@ -104,26 +114,8 @@ private:
|
||||
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; ///< 批量发送大小 (增加以减少锁竞争)
|
||||
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 数据缓存
|
||||
// ========================================================================
|
||||
@@ -149,89 +141,30 @@ private:
|
||||
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
|
||||
*/
|
||||
// ---- 核心工作流程 ----
|
||||
void sender_worker();
|
||||
void scan_and_send_files();
|
||||
void process_file(const std::string& filepath, const std::string& filename);
|
||||
|
||||
// ---- 队列管理 ----
|
||||
void enqueue_packets(std::vector<std::vector<uint8_t>>& packets);
|
||||
int send_udp_packet(const uint8_t* data, uint16_t len);
|
||||
|
||||
// ========================================================================
|
||||
// 图像处理函数
|
||||
// ========================================================================
|
||||
// ---- 辅助功能 ----
|
||||
void log_sent_file(const std::string& filename, size_t size);
|
||||
|
||||
/**
|
||||
* @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);
|
||||
// ---- 回调函数 ----
|
||||
void gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg);
|
||||
|
||||
/**
|
||||
* @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();
|
||||
|
||||
};
|
||||
|
||||
|
||||
@@ -31,6 +31,11 @@
|
||||
|
||||
<!-- 可选：MAVROS GPS 支持 -->
|
||||
<depend>mavros_msgs</depend>
|
||||
|
||||
<!-- 服务接口生成 -->
|
||||
<build_depend>rosidl_default_generators</build_depend>
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||
|
||||
<!-- 可选：USB 相机支持 -->
|
||||
<exec_depend>usb_cam</exec_depend>
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -20,6 +20,8 @@
|
||||
#include <sstream>
|
||||
#include <sensor_msgs/msg/nav_sat_fix.hpp>
|
||||
#include <thread>
|
||||
#include <regex>
|
||||
#include <sstream>
|
||||
|
||||
/**
|
||||
* @brief 构造函数
|
||||
@@ -31,32 +33,27 @@ UdpSenderNode::UdpSenderNode() : rclcpp::Node("udp_sender_node"), udp_socket_(-1
|
||||
|
||||
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>("max_packet_size", 1400); // Default to safer MTU
|
||||
this->declare_parameter<int>("frame_timeout_ms", 5000);
|
||||
this->declare_parameter<bool>("enable_fps_stats", true);
|
||||
this->declare_parameter<bool>("enable_drop_stats", true);
|
||||
this->declare_parameter<std::string>("save_dir", "./camera_data"); // 监控目录
|
||||
this->declare_parameter<std::string>("log_file", "./sent_log.txt"); // 日志文件
|
||||
|
||||
|
||||
// 读取参数
|
||||
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();
|
||||
save_dir_ = this->get_parameter("save_dir").as_string();
|
||||
log_file_path_ = this->get_parameter("log_file").as_string();
|
||||
|
||||
// ========================================================================
|
||||
// 打印初始化信息
|
||||
@@ -66,10 +63,24 @@ UdpSenderNode::UdpSenderNode() : rclcpp::Node("udp_sender_node"), udp_socket_(-1
|
||||
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(), "监控目录: %s", save_dir_.c_str());
|
||||
RCLCPP_INFO(this->get_logger(), "日志文件: %s", log_file_path_.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_);
|
||||
|
||||
// 创建目录
|
||||
std::filesystem::create_directories(save_dir_);
|
||||
|
||||
// 打开日志文件
|
||||
log_file_stream_.open(log_file_path_, std::ios::out | std::ios::app);
|
||||
if (!log_file_stream_.is_open()) {
|
||||
RCLCPP_ERROR(this->get_logger(), "无法打开日志文件: %s", log_file_path_.c_str());
|
||||
} else {
|
||||
// 写入头部
|
||||
auto now = std::chrono::system_clock::now();
|
||||
std::time_t now_c = std::chrono::system_clock::to_time_t(now);
|
||||
log_file_stream_ << "--- Session Started: " << std::put_time(std::localtime(&now_c), "%F %T") << " ---" << std::endl;
|
||||
}
|
||||
|
||||
// ========================================================================
|
||||
// 初始化 UDP 套接字
|
||||
@@ -81,26 +92,19 @@ UdpSenderNode::UdpSenderNode() : rclcpp::Node("udp_sender_node"), udp_socket_(-1
|
||||
return;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "✓ UDP 套接字初始化成功");
|
||||
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());
|
||||
// 创建文件扫描定时器 (1Hz, 即每秒检查一次)
|
||||
file_scan_timer_ = this->create_wall_timer(
|
||||
std::chrono::milliseconds(200), // 200ms
|
||||
std::bind(&UdpSenderNode::scan_and_send_files, this));
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), " 文件监控定时器已启动 (200ms)");
|
||||
|
||||
if (enable_gps_) {
|
||||
gps_subscriber_ = this->create_subscription<sensor_msgs::msg::NavSatFix>(
|
||||
gps_topic_,
|
||||
@@ -110,7 +114,7 @@ UdpSenderNode::UdpSenderNode() : rclcpp::Node("udp_sender_node"), udp_socket_(-1
|
||||
}
|
||||
);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "✓ 订阅 GPS Topic: %s", gps_topic_.c_str());
|
||||
RCLCPP_INFO(this->get_logger(), " 订阅 GPS Topic: %s", gps_topic_.c_str());
|
||||
}
|
||||
|
||||
// ========================================================================
|
||||
@@ -119,7 +123,7 @@ UdpSenderNode::UdpSenderNode() : rclcpp::Node("udp_sender_node"), udp_socket_(-1
|
||||
|
||||
sender_running_ = true;
|
||||
sender_thread_ = std::thread(&UdpSenderNode::sender_worker, this);
|
||||
RCLCPP_INFO(this->get_logger(), "✓ 后台发送线程已启动");
|
||||
RCLCPP_INFO(this->get_logger(), " 后台发送线程已启动");
|
||||
|
||||
// ========================================================================
|
||||
// 初始化统计
|
||||
@@ -127,7 +131,7 @@ UdpSenderNode::UdpSenderNode() : rclcpp::Node("udp_sender_node"), udp_socket_(-1
|
||||
|
||||
stats_.last_report_time = std::chrono::system_clock::now();
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "✓ 节点初始化完成，等待数据...");
|
||||
RCLCPP_INFO(this->get_logger(), " 节点初始化完成，等待数据...");
|
||||
RCLCPP_INFO(this->get_logger(), "======================================");
|
||||
}
|
||||
|
||||
@@ -140,7 +144,7 @@ UdpSenderNode::~UdpSenderNode() {
|
||||
send_queue_cv_.notify_one(); // 唤醒等待的线程
|
||||
if (sender_thread_.joinable()) {
|
||||
sender_thread_.join();
|
||||
RCLCPP_INFO(this->get_logger(), "✓ 后台发送线程已停止");
|
||||
RCLCPP_INFO(this->get_logger(), " 后台发送线程已停止");
|
||||
}
|
||||
|
||||
close_udp_socket();
|
||||
@@ -282,26 +286,156 @@ void UdpSenderNode::enqueue_packets(std::vector<std::vector<uint8_t>>& packets)
|
||||
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;
|
||||
void UdpSenderNode::scan_and_send_files() {
|
||||
if (!std::filesystem::exists(save_dir_)) return;
|
||||
|
||||
std::vector<std::filesystem::directory_entry> new_files;
|
||||
|
||||
try {
|
||||
for (const auto& entry : std::filesystem::directory_iterator(save_dir_)) {
|
||||
if (entry.is_regular_file() && entry.path().extension() == ".jpg") {
|
||||
if (sent_files_.find(entry.path().string()) == sent_files_.end()) {
|
||||
new_files.push_back(entry);
|
||||
}
|
||||
}
|
||||
}
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(this->get_logger(), "扫描目录失败: %s", e.what());
|
||||
return;
|
||||
}
|
||||
|
||||
// 按修改时间排序（旧的先发）
|
||||
std::sort(new_files.begin(), new_files.end(),
|
||||
[](const std::filesystem::directory_entry& a, const std::filesystem::directory_entry& b) {
|
||||
return std::filesystem::last_write_time(a) < std::filesystem::last_write_time(b);
|
||||
});
|
||||
|
||||
for (const auto& entry : new_files) {
|
||||
process_file(entry.path().string(), entry.path().filename().string());
|
||||
|
||||
PacketQueueItem item;
|
||||
item.data.assign(data, data + len);
|
||||
item.enqueue_time = std::chrono::system_clock::now();
|
||||
send_queue_.push(item);
|
||||
// 标记为已发送
|
||||
sent_files_.insert(entry.path().string());
|
||||
|
||||
// 为了防止内存无限增长，如果太大了可以清理旧的 (可选)
|
||||
if (sent_files_.size() > 10000) {
|
||||
// 简单清理策略：实际生产中可能需要更复杂的逻辑
|
||||
// 这里暂时不做，假设磁盘空间有限，文件会被外部删除
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void UdpSenderNode::process_file(const std::string& filepath, const std::string& filename) {
|
||||
// 读取文件内容
|
||||
std::ifstream file(filepath, std::ios::binary);
|
||||
if (!file) return;
|
||||
|
||||
// 获取文件大小
|
||||
file.seekg(0, std::ios::end);
|
||||
size_t size = file.tellg();
|
||||
file.seekg(0, std::ios::beg);
|
||||
|
||||
if (size == 0) return;
|
||||
|
||||
std::vector<uint8_t> buffer(size);
|
||||
if (!file.read(reinterpret_cast<char*>(buffer.data()), size)) return;
|
||||
|
||||
frame_counter_++;
|
||||
|
||||
// 从文件名提取照片编号
|
||||
// 文件名格式: photo_YYYYMMDD_HHMMSS_XXX.jpg
|
||||
uint32_t photo_number = frame_counter_; // 默认使用frame_counter_
|
||||
try {
|
||||
// 尝试从文件名中提取编号（最后一个下划线后的数字）
|
||||
std::regex pattern(R"(photo_\d{8}_\d{6}_(\d+)\.jpg)");
|
||||
std::smatch match;
|
||||
if (std::regex_match(filename, match, pattern) && match.size() > 1) {
|
||||
photo_number = std::stoul(match[1].str());
|
||||
} else {
|
||||
// 如果格式不匹配，尝试提取任何数字作为编号
|
||||
std::regex alt_pattern(R"(_(\d+)\.jpg$)");
|
||||
if (std::regex_search(filename, match, alt_pattern) && match.size() > 1) {
|
||||
photo_number = std::stoul(match[1].str());
|
||||
}
|
||||
}
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_WARN(this->get_logger(), "无法从文件名提取照片编号，使用frame_counter: %s", e.what());
|
||||
}
|
||||
|
||||
// 通知发送线程有新数据
|
||||
send_queue_cv_.notify_one();
|
||||
// 计算需要多少个包
|
||||
const size_t header_size = PacketHeader::size();
|
||||
const size_t payload_size = max_packet_size_ - header_size;
|
||||
const uint16_t total_packets = (size + payload_size - 1) / payload_size;
|
||||
|
||||
std::vector<std::vector<uint8_t>> packets;
|
||||
packets.reserve(total_packets);
|
||||
|
||||
// 获取 GPS 信息
|
||||
double lat = 0.0, lon = 0.0, alt = 0.0;
|
||||
{
|
||||
std::lock_guard<std::mutex> gps_lock(current_gps_data_.mutex);
|
||||
if (enable_gps_) {
|
||||
lat = current_gps_data_.latitude;
|
||||
lon = current_gps_data_.longitude;
|
||||
alt = current_gps_data_.altitude;
|
||||
}
|
||||
}
|
||||
|
||||
double timestamp = std::chrono::duration<double>(
|
||||
std::chrono::system_clock::now().time_since_epoch()
|
||||
).count();
|
||||
|
||||
for (uint16_t i = 0; i < total_packets; ++i) {
|
||||
size_t offset = i * payload_size;
|
||||
size_t current_payload_len = std::min(payload_size, size - offset);
|
||||
|
||||
// 准备包数据
|
||||
std::vector<uint8_t> packet_data(header_size + current_payload_len);
|
||||
|
||||
// 填充头部
|
||||
PacketHeader* header = reinterpret_cast<PacketHeader*>(packet_data.data());
|
||||
header->magic = SKYLINK_MAGIC;
|
||||
header->frame_id = frame_counter_;
|
||||
header->total_chunks = total_packets;
|
||||
header->chunk_index = i;
|
||||
header->data_len = static_cast<uint16_t>(current_payload_len);
|
||||
header->timestamp = timestamp;
|
||||
|
||||
// GPS
|
||||
header->lat = lat;
|
||||
header->lon = lon;
|
||||
header->alt = alt;
|
||||
|
||||
// 其他字段
|
||||
header->crc16 = 0;
|
||||
header->reserve = photo_number; // 存储照片编号
|
||||
|
||||
// 填充负载
|
||||
std::memcpy(packet_data.data() + header_size, buffer.data() + offset, current_payload_len);
|
||||
|
||||
packets.push_back(std::move(packet_data));
|
||||
}
|
||||
|
||||
// 加入发送队列
|
||||
enqueue_packets(packets);
|
||||
|
||||
// 记录日志
|
||||
log_sent_file(filename, size);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "已排队文件: %s (大小: %zu bytes, 包数: %d, 照片编号: %u)",
|
||||
filename.c_str(), size, total_packets, photo_number);
|
||||
}
|
||||
|
||||
void UdpSenderNode::log_sent_file(const std::string& filename, size_t size) {
|
||||
if (log_file_stream_.is_open()) {
|
||||
auto now = std::chrono::system_clock::now();
|
||||
std::time_t now_c = std::chrono::system_clock::to_time_t(now);
|
||||
log_file_stream_ << std::put_time(std::localtime(&now_c), "[%F %T]")
|
||||
<< " Sent: " << filename
|
||||
<< " Size: " << size
|
||||
<< " Total Sent: " << sent_files_.size() + 1
|
||||
<< std::endl;
|
||||
log_file_stream_.flush(); // 确保写入磁盘
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -352,58 +486,6 @@ void UdpSenderNode::sender_worker() {
|
||||
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 回调
|
||||
*/
|
||||
@@ -414,132 +496,22 @@ void UdpSenderNode::gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr ms
|
||||
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() {
|
||||
// 简单的统计报告实现
|
||||
if (!enable_fps_stats_) return;
|
||||
|
||||
auto now = std::chrono::system_clock::now();
|
||||
auto elapsed_seconds = std::chrono::duration_cast<std::chrono::seconds>(
|
||||
now - stats_.last_report_time
|
||||
).count();
|
||||
auto diff = 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;
|
||||
if (diff >= 5) { // 每5秒报告一次
|
||||
std::lock_guard<std::mutex> lock(stats_mutex_);
|
||||
// 这里只是为了演示，实际可以计算速率
|
||||
stats_.last_report_time = now;
|
||||
RCLCPP_INFO(this->get_logger(), "Stats: Sent %u packets, %u bytes. Dropped %u frames.",
|
||||
packets_sent_.load(), bytes_sent_.load(), frames_dropped_.load());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
11
skylink_ros2_ws/src/skylink_bridge/srv/TriggerCapture.srv
Normal file
11
skylink_ros2_ws/src/skylink_bridge/srv/TriggerCapture.srv
Normal file
@@ -0,0 +1,11 @@
|
||||
# 请求参数
|
||||
float64 duration # 总时间（秒），0表示使用默认
|
||||
int32 count # 拍照数量，0表示使用默认
|
||||
float64 interval # 时间间隔（秒），0表示自动计算
|
||||
int8 transmission_mode # 传输模式：0=原图，1=缩略图
|
||||
|
||||
---
|
||||
# 响应
|
||||
bool success # 是否成功启动
|
||||
string message # 状态消息
|
||||
int32 total_photos # 当前总照片数
|
||||
Reference in New Issue
Block a user