From de5b5be58c101983eaaeb8507629b5e5fa0d1269 Mon Sep 17 00:00:00 2001 From: Liangwei XU Date: Mon, 2 Feb 2026 09:42:08 +0800 Subject: [PATCH 01/21] feat(filters): add depthlitez compression library as submodule --- .gitmodules | 4 ++++ middlewares/filters/depthlitez | 1 + 2 files changed, 5 insertions(+) create mode 100644 .gitmodules create mode 160000 middlewares/filters/depthlitez diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..43bf1ea --- /dev/null +++ b/.gitmodules @@ -0,0 +1,4 @@ +[submodule "middlewares/filters/depthlitez"] + path = middlewares/filters/depthlitez + url = ../depthlitez.git + branch = main diff --git a/middlewares/filters/depthlitez b/middlewares/filters/depthlitez new file mode 160000 index 0000000..834e1d0 --- /dev/null +++ b/middlewares/filters/depthlitez @@ -0,0 +1 @@ +Subproject commit 834e1d0ac61e609b4c8b1897e20e5c9492729837 From 82c2889a256b23a6361d1ba08fa24ef38e866d63 Mon Sep 17 00:00:00 2001 From: Liangwei XU Date: Mon, 2 Feb 2026 09:50:58 +0800 Subject: [PATCH 02/21] feat(filters): implement depth image compression filter --- CMakeLists.txt | 8 +- middlewares/filters/CMakeLists.txt | 118 +++++++++++++ .../filters/include/depth_compressor.hpp | 70 ++++++++ middlewares/filters/src/depth_compressor.cpp | 68 ++++++++ .../filters/test/test_depth_compressor.cpp | 160 ++++++++++++++++++ 5 files changed, 422 insertions(+), 2 deletions(-) create mode 100644 middlewares/filters/CMakeLists.txt create mode 100644 middlewares/filters/include/depth_compressor.hpp create mode 100644 middlewares/filters/src/depth_compressor.cpp create mode 100644 middlewares/filters/test/test_depth_compressor.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 66958fb..0624c3a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -112,8 +112,12 @@ if(AXON_BUILD_UPLOADER AND EXISTS "${AXON_UPLOADER_DIR}/CMakeLists.txt") add_subdirectory(${AXON_UPLOADER_DIR} ${CMAKE_CURRENT_BINARY_DIR}/axon_uploader) endif() -# Middleware plugins (ROS1, ROS2, Zenoh) -add_subdirectory(${AXON_MIDDLEWARES_DIR}) +# Filters (depth compression, no dependencies on other Axon modules) +set(AXON_FILTERS_DIR "${AXON_MIDDLEWARES_DIR}/filters") +set(AXON_FILTERS_DIR "${AXON_FILTERS_DIR}" CACHE INTERNAL "") +if(EXISTS "${AXON_FILTERS_DIR}/CMakeLists.txt") + add_subdirectory(${AXON_FILTERS_DIR} ${CMAKE_CURRENT_BINARY_DIR}/filters) +endif() # ============================================================================= # Applications diff --git a/middlewares/filters/CMakeLists.txt b/middlewares/filters/CMakeLists.txt new file mode 100644 index 0000000..766ca1f --- /dev/null +++ b/middlewares/filters/CMakeLists.txt @@ -0,0 +1,118 @@ +# Depth Filters Library +# Provides depth image compression using DepthLiteZ + +cmake_minimum_required(VERSION 3.16) + +project(axon_filters + VERSION 0.1.0 + LANGUAGES CXX +) + +# C++ standard +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +# Export compile commands +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +# Options +option(BUILD_TESTING "Build tests" ON) +option(BUILD_SHARED "Build shared library" OFF) + +# Find dependencies +find_package(PkgConfig REQUIRED) + +# Find LZ4 +pkg_check_modules(LZ4 REQUIRED liblz4) + +# Find ZSTD +pkg_check_modules(ZSTD REQUIRED libzstd) + +# DepthLiteZ submodule +set(DEPTHLITEZ_DIR ${CMAKE_CURRENT_SOURCE_DIR}/depthlitez) +if(NOT EXISTS ${DEPTHLITEZ_DIR}/include/dlz.hpp) + message(WARNING "DepthLiteZ submodule not found. Run: git submodule update --init --recursive") +endif() + +# Sources +set(SOURCES + src/depth_compressor.cpp +) + +set(HEADERS + include/depth_compressor.hpp +) + +# Create library +if(BUILD_SHARED) + add_library(axon_filters SHARED ${SOURCES}) +else() + add_library(axon_filters STATIC ${SOURCES}) +endif() + +target_include_directories(axon_filters + PUBLIC + $ + $ + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src + ${DEPTHLITEZ_DIR}/include +) + +target_link_libraries(axon_filters + PUBLIC + ${LZ4_LIBRARIES} + ${ZSTD_LIBRARIES} +) + +target_compile_options(axon_filters + PRIVATE + -Wall -Wextra -Wpedantic +) + +# Alias for consistent naming +add_library(axon::filters ALIAS axon_filters) + +# Tests +if(BUILD_TESTING) + enable_testing() + + find_package(GTest REQUIRED) + + # Auto-discover test files + file(GLOB TEST_SOURCES + ${CMAKE_CURRENT_SOURCE_DIR}/test/test_*.cpp + ) + + foreach(TEST_SOURCE ${TEST_SOURCES}) + get_filename_component(TEST_NAME ${TEST_SOURCE} NAME_WE) + add_executable(${TEST_NAME} ${TEST_SOURCE}) + target_link_libraries(${TEST_NAME} + PRIVATE + axon::filters + GTest::gtest_main + ) + target_include_directories(${TEST_NAME} PRIVATE ${DEPTHLITEZ_DIR}/include) + add_test(NAME ${TEST_NAME} COMMAND ${TEST_NAME}) + endforeach() +endif() + +# Installation +include(GNUInstallDirs) + +install(TARGETS axon_filters + EXPORT axon_filters_targets + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} +) + +install(FILES ${HEADERS} + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/axon/filters +) + +install(EXPORT axon_filters_targets + FILE axon_filters_targets.cmake + NAMESPACE axon:: + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/axon_filters +) diff --git a/middlewares/filters/include/depth_compressor.hpp b/middlewares/filters/include/depth_compressor.hpp new file mode 100644 index 0000000..1fa21c2 --- /dev/null +++ b/middlewares/filters/include/depth_compressor.hpp @@ -0,0 +1,70 @@ +#pragma once + +#include +#include +#include + +#include + +namespace axon { +namespace depth { + +/** + * @brief Depth image compressor using DepthLiteZ library + * + * Provides compression for 16-bit depth images with configurable quality levels. + * Supports RVL (Run-Length Variable Length) encoding with optional LZ4/ZSTD + * secondary compression. + */ +class DepthCompressor { +public: + /** + * @brief Compression configuration + */ + struct Config { + dlz::CompressionLevel level = dlz::CompressionLevel::kMedium; + dlz::Threading threading = dlz::Threading::kAuto; + int num_threads = 0; // 0 = auto-detect + std::string encoding = "16UC1"; // Input encoding format + }; + + /** + * @brief Compress depth image data + * + * @param depth_data 16-bit depth data (row-major) + * @param width Image width + * @param height Image height + * @param compressed_data Output compressed data + * @return true if compression succeeded + */ + bool compress( + const uint8_t* depth_data, + size_t width, + size_t height, + std::vector& compressed_data + ); + + /** + * @brief Get compression format identifier + * + * Returns the format string for CompressedImage.format field + * @return Format identifier ("dlz_fast", "dlz_medium", "dlz_max") + */ + std::string get_compression_format() const; + + /** + * @brief Set compression configuration + */ + void set_config(const Config& config); + + /** + * @brief Get current configuration + */ + const Config& get_config() const { return config_; } + +private: + Config config_; +}; + +} // namespace depth +} // namespace axon diff --git a/middlewares/filters/src/depth_compressor.cpp b/middlewares/filters/src/depth_compressor.cpp new file mode 100644 index 0000000..b7356e7 --- /dev/null +++ b/middlewares/filters/src/depth_compressor.cpp @@ -0,0 +1,68 @@ +#include "depth_compressor.hpp" + +#include + +namespace axon { +namespace depth { + +bool DepthCompressor::compress( + const uint8_t* depth_data, + size_t width, + size_t height, + std::vector& compressed_data +) { + if (!depth_data || width == 0 || height == 0) { + return false; + } + + // Cast to int16_t (16UC1 encoding) + const int16_t* depth_data_i16 = reinterpret_cast(depth_data); + int num_pixels = static_cast(width * height); + + // Compress using dlz library + std::vector compressed_buffer; + size_t compressed_size; + + bool success = dlz::CompressEx( + const_cast(depth_data_i16), + num_pixels, + compressed_buffer, + 0, + compressed_size, + config_.level, + config_.threading, + config_.num_threads + ); + + if (!success) { + return false; + } + + // Copy to output vector + compressed_data.assign( + compressed_buffer.begin(), + compressed_buffer.begin() + compressed_size + ); + + return true; +} + +std::string DepthCompressor::get_compression_format() const { + switch (config_.level) { + case dlz::CompressionLevel::kFast: + return "dlz_fast"; + case dlz::CompressionLevel::kMedium: + return "dlz_medium"; + case dlz::CompressionLevel::kMax: + return "dlz_max"; + default: + return "dlz_medium"; + } +} + +void DepthCompressor::set_config(const Config& config) { + config_ = config; +} + +} // namespace depth +} // namespace axon diff --git a/middlewares/filters/test/test_depth_compressor.cpp b/middlewares/filters/test/test_depth_compressor.cpp new file mode 100644 index 0000000..c5f4195 --- /dev/null +++ b/middlewares/filters/test/test_depth_compressor.cpp @@ -0,0 +1,160 @@ +#include + +#include + +namespace axon { +namespace depth { +namespace test { + +// Test fixture for DepthCompressor +class DepthCompressorTest : public ::testing::Test { +protected: + void SetUp() override { + compressor_.set_config(config_); + } + + // Create synthetic depth data + std::vector create_depth_data(size_t width, size_t height) { + std::vector data(width * height * 2); // 16-bit = 2 bytes per pixel + + uint16_t* depth_ptr = reinterpret_cast(data.data()); + for (size_t i = 0; i < width * height; ++i) { + // Create realistic depth pattern (0-10m in mm units) + depth_ptr[i] = static_cast(1000 + (i % 9000)); + } + + return data; + } + + DepthCompressor compressor_; + DepthCompressor::Config config_; +}; + +// Test basic compression functionality +TEST_F(DepthCompressorTest, Compress16UC1_Basic) { + const size_t width = 640; + const size_t height = 480; + + auto depth_data = create_depth_data(width, height); + std::vector compressed; + + ASSERT_TRUE(compressor_.compress(depth_data.data(), width, height, compressed)); + EXPECT_GT(compressed.size(), 0); +} + +// Test compression ratio +TEST_F(DepthCompressorTest, CompressionRatio) { + const size_t width = 640; + const size_t height = 480; + + auto depth_data = create_depth_data(width, height); + std::vector compressed; + + ASSERT_TRUE(compressor_.compress(depth_data.data(), width, height, compressed)); + + // Original size: 640 * 480 * 2 = 614400 bytes + // Compressed should be significantly smaller (at least 2x) + size_t original_size = width * height * 2; + EXPECT_LT(compressed.size(), original_size); + EXPECT_GT(compressed.size(), 0); + + double compression_ratio = static_cast(original_size) / compressed.size(); + EXPECT_GT(compression_ratio, 2.0); // At least 2x compression +} + +// Test compression format strings +TEST_F(DepthCompressorTest, CompressionFormatStrings) { + config_.level = dlz::CompressionLevel::kFast; + compressor_.set_config(config_); + EXPECT_EQ(compressor_.get_compression_format(), "dlz_fast"); + + config_.level = dlz::CompressionLevel::kMedium; + compressor_.set_config(config_); + EXPECT_EQ(compressor_.get_compression_format(), "dlz_medium"); + + config_.level = dlz::CompressionLevel::kMax; + compressor_.set_config(config_); + EXPECT_EQ(compressor_.get_compression_format(), "dlz_max"); +} + +// Test invalid inputs +TEST_F(DepthCompressorTest, InvalidInput) { + std::vector compressed; + + // Null data pointer + EXPECT_FALSE(compressor_.compress(nullptr, 640, 480, compressed)); + + // Zero width + EXPECT_FALSE(compressor_.compress(reinterpret_cast(0x1), 0, 480, compressed)); + + // Zero height + EXPECT_FALSE(compressor_.compress(reinterpret_cast(0x1), 640, 0, compressed)); +} + +// Test different compression levels +TEST_F(DepthCompressorTest, DifferentCompressionLevels) { + const size_t width = 640; + const size_t height = 480; + + auto depth_data = create_depth_data(width, height); + + std::vector compressed_sizes; + + for (auto level : {dlz::CompressionLevel::kFast, + dlz::CompressionLevel::kMedium, + dlz::CompressionLevel::kMax}) { + config_.level = level; + compressor_.set_config(config_); + + std::vector compressed; + ASSERT_TRUE(compressor_.compress(depth_data.data(), width, height, compressed)); + compressed_sizes.push_back(compressed.size()); + } + + // kFast should be largest (least compression) + // kMax should be smallest (best compression) + EXPECT_GE(compressed_sizes[0], compressed_sizes[1]); // fast >= medium + EXPECT_GE(compressed_sizes[1], compressed_sizes[2]); // medium >= max +} + +// Test different image sizes +TEST_F(DepthCompressorTest, DifferentImageSizes) { + std::vector> sizes = { + {320, 240}, + {640, 480}, + {1280, 720}, + {1920, 1080} + }; + + for (const auto& [width, height] : sizes) { + auto depth_data = create_depth_data(width, height); + std::vector compressed; + + ASSERT_TRUE(compressor_.compress(depth_data.data(), width, height, compressed)) + << "Failed for size: " << width << "x" << height; + + EXPECT_GT(compressed.size(), 0) + << "Empty output for size: " << width << "x" << height; + } +} + +// Test config getter/setter +TEST_F(DepthCompressorTest, ConfigGetterSetter) { + DepthCompressor::Config new_config; + new_config.level = dlz::CompressionLevel::kMax; + new_config.threading = dlz::Threading::kMulti; + new_config.num_threads = 4; + new_config.encoding = "16UC1"; + + compressor_.set_config(new_config); + + const auto& retrieved_config = compressor_.get_config(); + EXPECT_EQ(retrieved_config.level, dlz::CompressionLevel::kMax); + EXPECT_EQ(retrieved_config.threading, dlz::Threading::kMulti); + EXPECT_EQ(retrieved_config.num_threads, 4); + EXPECT_EQ(retrieved_config.encoding, "16UC1"); +} + +} // namespace test +} // namespace depth +} // namespace axon From 91009157cf3df3f788108ce04d7ceeeafdb2eb69 Mon Sep 17 00:00:00 2001 From: Liangwei XU Date: Mon, 2 Feb 2026 10:10:40 +0800 Subject: [PATCH 03/21] feat(ros2): implement depth compression in ROS2 plugin --- .gitignore | 3 + middlewares/ros2/CMakeLists.txt | 13 ++ .../ros2/include/depth_compression_filter.hpp | 89 ++++++++++++ middlewares/ros2/include/ros2_plugin.hpp | 8 +- .../include/ros2_subscription_wrapper.hpp | 19 ++- .../ros2/src/depth_compression_filter.cpp | 133 ++++++++++++++++++ middlewares/ros2/src/ros2_plugin.cpp | 21 ++- middlewares/ros2/src/ros2_plugin_export.cpp | 46 +++++- .../ros2/src/ros2_subscription_wrapper.cpp | 58 +++++++- 9 files changed, 371 insertions(+), 19 deletions(-) create mode 100644 middlewares/ros2/include/depth_compression_filter.hpp create mode 100644 middlewares/ros2/src/depth_compression_filter.cpp diff --git a/.gitignore b/.gitignore index 9cb08ca..ab2b1ac 100644 --- a/.gitignore +++ b/.gitignore @@ -79,6 +79,9 @@ ros *.swp *~ +# Cache +.cache/ + # Python (if any) __pycache__/ *.py[cod] diff --git a/middlewares/ros2/CMakeLists.txt b/middlewares/ros2/CMakeLists.txt index b000c72..8cfb72d 100644 --- a/middlewares/ros2/CMakeLists.txt +++ b/middlewares/ros2/CMakeLists.txt @@ -50,6 +50,7 @@ set(CMAKE_PREFIX_PATH "${CMAKE_PREFIX_PATH};${ROS2_DIR}") # Find required ROS2 packages find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) # Check for required ROS2 libraries set(ROS2_REQUIRED_LIBS @@ -69,6 +70,7 @@ add_library(axon_ros2_plugin SHARED src/ros2_plugin.cpp src/ros2_subscription_wrapper.cpp src/ros2_plugin_export.cpp + src/depth_compression_filter.cpp ) target_include_directories(axon_ros2_plugin PUBLIC @@ -82,10 +84,21 @@ target_link_libraries(axon_ros2_plugin PUBLIC nlohmann_json::nlohmann_json ) +# Link filters library for depth compression +if(TARGET axon::filters) + target_link_libraries(axon_ros2_plugin PRIVATE axon::filters) + target_include_directories(axon_ros2_plugin PRIVATE + ${AXON_MIDDLEWARES_DIR}/filters/include + ) +endif() + # For ROS2 type support macros - we need to link against message libraries if(TARGET std_msgs__rosidl_typesupport_cpp) target_link_libraries(axon_ros2_plugin PRIVATE std_msgs__rosidl_typesupport_cpp) endif() +if(TARGET sensor_msgs__rosidl_typesupport_cpp) + target_link_libraries(axon_ros2_plugin PRIVATE sensor_msgs__rosidl_typesupport_cpp) +endif() # Export all symbols for dynamic loading target_compile_options(axon_ros2_plugin PRIVATE -fvisibility=default) diff --git a/middlewares/ros2/include/depth_compression_filter.hpp b/middlewares/ros2/include/depth_compression_filter.hpp new file mode 100644 index 0000000..0fe95d4 --- /dev/null +++ b/middlewares/ros2/include/depth_compression_filter.hpp @@ -0,0 +1,89 @@ +// SPDX-FileCopyrightText: 2026 ArcheBase +// +// SPDX-License-Identifier: MulanPSL-2.0 + +#ifndef ROS2_DEPTH_COMPRESSION_FILTER_HPP +#define ROS2_DEPTH_COMPRESSION_FILTER_HPP + +#include + +#include +#include +#include + +namespace ros2_plugin { + +/** + * @brief 深度压缩配置 + */ +struct DepthCompressionConfig { + bool enabled = false; + std::string level = "medium"; // fast, medium, max +}; + +/** + * @brief 深度图像压缩过滤器 + * + * 在 ROS2 插件中使用，将 sensor_msgs::msg::Image (16UC1) 压缩为 + * sensor_msgs::msg::CompressedImage 格式。 + */ +class DepthCompressionFilter { +public: + /** + * @brief 处理后的消息回调类型 + */ + using ProcessedCallback = std::function& data, + uint64_t timestamp_ns + )>; + + /** + * @brief 构造函数 + * @param config 深度压缩配置 + */ + explicit DepthCompressionFilter(const DepthCompressionConfig& config); + + /** + * @brief 过滤并处理消息 + * + * @param topic 主题名称 + * @param message_type 消息类型 + * @param data 原始消息数据（CDR 序列化的 sensor_msgs::msg::Image） + * @param timestamp_ns 时间戳（纳秒） + * @param callback 处理后的消息回调 + */ + void filter_and_process( + const std::string& topic, + const std::string& message_type, + const std::vector& data, + uint64_t timestamp_ns, + ProcessedCallback callback + ); + +private: + DepthCompressionConfig config_; + axon::depth::DepthCompressor compressor_; + + /** + * @brief 从 ROS Image 消息中提取深度数据 + * @return 提取的深度数据指针，宽度，高度 + */ + std::tuple extract_depth_data( + const std::vector& image_msg + ); + + /** + * @brief 构建 CompressedImage 消息 + */ + std::vector build_compressed_image_msg( + const std::string& format, + const std::vector& compressed_data, + uint64_t timestamp_ns + ); +}; + +} // namespace ros2_plugin + +#endif // ROS2_DEPTH_COMPRESSION_FILTER_HPP diff --git a/middlewares/ros2/include/ros2_plugin.hpp b/middlewares/ros2/include/ros2_plugin.hpp index de282d2..c6e96a2 100644 --- a/middlewares/ros2/include/ros2_plugin.hpp +++ b/middlewares/ros2/include/ros2_plugin.hpp @@ -41,11 +41,17 @@ class Ros2Plugin { return spinning_; } - // Subscribe to a topic with a callback + // Subscribe to a topic with a callback (default QoS) bool subscribe( const std::string& topic_name, const std::string& message_type, MessageCallback callback ); + // Subscribe to a topic with options (for depth compression) + bool subscribe( + const std::string& topic_name, const std::string& message_type, const SubscribeOptions& options, + MessageCallback callback + ); + // Unsubscribe from a topic bool unsubscribe(const std::string& topic_name); diff --git a/middlewares/ros2/include/ros2_subscription_wrapper.hpp b/middlewares/ros2/include/ros2_subscription_wrapper.hpp index 0854c4e..ed34cc5 100644 --- a/middlewares/ros2/include/ros2_subscription_wrapper.hpp +++ b/middlewares/ros2/include/ros2_subscription_wrapper.hpp @@ -7,21 +7,37 @@ #include +#include #include #include #include +#include #include #include #include namespace ros2_plugin { +// DepthCompressionConfig is defined in depth_compression_filter.hpp + // Message callback type - passes raw serialized message data using MessageCallback = std::function& message_data, rclcpp::Time timestamp )>; +/** + * 订阅选项 + */ +struct SubscribeOptions { + rclcpp::QoS qos; + std::optional depth_compression; // 深度压缩配置（可选） + + // 默认构造函数 + SubscribeOptions() + : qos(10) {} +}; + class SubscriptionManager { public: explicit SubscriptionManager(rclcpp::Node::SharedPtr node); @@ -29,7 +45,7 @@ class SubscriptionManager { // Subscribe to any topic with any message type bool subscribe( - const std::string& topic_name, const std::string& message_type, const rclcpp::QoS& qos, + const std::string& topic_name, const std::string& message_type, const SubscribeOptions& options, MessageCallback callback ); @@ -44,6 +60,7 @@ class SubscriptionManager { struct SubscriptionInfo { rclcpp::GenericSubscription::SharedPtr subscription; MessageCallback callback; + std::unique_ptr compression_filter; }; std::unordered_map subscriptions_; mutable std::mutex mutex_; diff --git a/middlewares/ros2/src/depth_compression_filter.cpp b/middlewares/ros2/src/depth_compression_filter.cpp new file mode 100644 index 0000000..f77ec8a --- /dev/null +++ b/middlewares/ros2/src/depth_compression_filter.cpp @@ -0,0 +1,133 @@ +// SPDX-FileCopyrightText: 2026 ArcheBase +// +// SPDX-License-Identifier: MulanPSL-2.0 + +#include "depth_compression_filter.hpp" + +#include +#include + +#include + +namespace ros2_plugin { + +DepthCompressionFilter::DepthCompressionFilter(const DepthCompressionConfig& config) + : config_(config) { + // 配置压缩器 + axon::depth::DepthCompressor::Config compressor_config; + if (config.level == "fast") { + compressor_config.level = dlz::CompressionLevel::kFast; + } else if (config.level == "max") { + compressor_config.level = dlz::CompressionLevel::kMax; + } else { + compressor_config.level = dlz::CompressionLevel::kMedium; + } + compressor_.set_config(compressor_config); +} + +void DepthCompressionFilter::filter_and_process( + const std::string& topic, + const std::string& message_type, + const std::vector& data, + uint64_t timestamp_ns, + ProcessedCallback callback +) { + // 只处理 sensor_msgs::msg::Image + if (message_type != "sensor_msgs::msg::Image" && + message_type != "sensor_msgs/msg/Image") { + // 不是图像消息，直接传递 + callback(topic, message_type, data, timestamp_ns); + return; + } + + // 尝试提取并压缩深度图像 + try { + auto [depth_data, width, height] = extract_depth_data(data); + + if (depth_data != nullptr && width > 0 && height > 0) { + // 压缩深度图像 + std::vector compressed_data; + if (compressor_.compress(depth_data, width, height, compressed_data)) { + // 构建压缩图像消息 + std::string format = compressor_.get_compression_format(); + std::vector compressed_msg = + build_compressed_image_msg(format, compressed_data, timestamp_ns); + + // 回调处理后的消息 + callback(topic, "sensor_msgs::msg::CompressedImage", compressed_msg, timestamp_ns); + return; + } + } + } catch (const std::exception& e) { + // 压缩失败，回退到原始消息 + } + + // 压缩失败或不是深度图像，传递原始消息 + callback(topic, message_type, data, timestamp_ns); +} + +std::tuple +DepthCompressionFilter::extract_depth_data(const std::vector& image_msg) { + const uint8_t* data = nullptr; + size_t width = 0; + size_t height = 0; + + // 使用 ROS2 反序列化 + try { + rclcpp::Serialization serialization; + sensor_msgs::msg::Image image; + rclcpp::SerializedMessage serialized_msg; + serialized_msg.reserve(image_msg.size()); + std::memcpy( + serialized_msg.get_rcl_serialized_message().buffer, image_msg.data(), image_msg.size() + ); + serialized_msg.get_rcl_serialized_message().buffer_length = image_msg.size(); + + // 反序列化 + serialization.deserialize_message(serialized_msg, &image); + + // 检查是否为 16UC1 深度图像 + if (image.encoding == "16UC1") { + data = image.data.data(); + width = image.width; + height = image.height; + } + } catch (const std::exception& e) { + // 反序列化失败 + } + + return {data, width, height}; +} + +std::vector DepthCompressionFilter::build_compressed_image_msg( + const std::string& format, + const std::vector& compressed_data, + uint64_t timestamp_ns +) { + // 使用 ROS2 序列化 CompressedImage + sensor_msgs::msg::CompressedImage compressed_msg; + + // 设置头部 + compressed_msg.header.stamp.sec = static_cast(timestamp_ns / 1000000000ULL); + compressed_msg.header.stamp.nanosec = static_cast(timestamp_ns % 1000000000ULL); + compressed_msg.format = format; + + // 设置压缩数据 + compressed_msg.data = compressed_data; + + // 序列化 + rclcpp::Serialization serialization; + rclcpp::SerializedMessage serialized_msg; + serialization.serialize_message(compressed_msg, &serialized_msg); + + // 复制到输出向量 + std::vector result( + serialized_msg.get_rcl_serialized_message().buffer, + serialized_msg.get_rcl_serialized_message().buffer + + serialized_msg.get_rcl_serialized_message().buffer_length + ); + + return result; +} + +} // namespace ros2_plugin diff --git a/middlewares/ros2/src/ros2_plugin.cpp b/middlewares/ros2/src/ros2_plugin.cpp index 0242d8e..6ee5e8d 100644 --- a/middlewares/ros2/src/ros2_plugin.cpp +++ b/middlewares/ros2/src/ros2_plugin.cpp @@ -148,11 +148,24 @@ bool Ros2Plugin::subscribe( } // Default QoS: keep last 10, reliable, volatile - rclcpp::QoS qos(10); - qos.reliable(); - qos.durability_volatile(); + SubscribeOptions options; + options.qos = rclcpp::QoS(10); + options.qos.reliable(); + options.qos.durability_volatile(); - return subscription_manager_->subscribe(topic_name, message_type, qos, callback); + return subscription_manager_->subscribe(topic_name, message_type, options, callback); +} + +bool Ros2Plugin::subscribe( + const std::string& topic_name, const std::string& message_type, const SubscribeOptions& options, + MessageCallback callback +) { + if (!initialized_.load()) { + RCUTILS_LOG_ERROR("Cannot subscribe: plugin not initialized"); + return false; + } + + return subscription_manager_->subscribe(topic_name, message_type, options, callback); } bool Ros2Plugin::unsubscribe(const std::string& topic_name) { diff --git a/middlewares/ros2/src/ros2_plugin_export.cpp b/middlewares/ros2/src/ros2_plugin_export.cpp index 2ebacfe..5a243f6 100644 --- a/middlewares/ros2/src/ros2_plugin_export.cpp +++ b/middlewares/ros2/src/ros2_plugin_export.cpp @@ -8,8 +8,13 @@ #include #include #include +#include #include #include +#include + +// JSON library +#include // ROS2 headers #include @@ -18,6 +23,7 @@ // Plugin implementation headers #include "ros2_plugin.hpp" #include "ros2_subscription_wrapper.hpp" +#include // For DepthCompressionConfig using namespace ros2_plugin; @@ -116,9 +122,10 @@ static int32_t axon_stop(void) { return static_cast(AXON_SUCCESS); } -// Subscribe to a topic with callback +// Subscribe to a topic with callback and optional options (JSON string, can be nullptr) static int32_t axon_subscribe( - const char* topic_name, const char* message_type, AxonMessageCallback callback, void* user_data + const char* topic_name, const char* message_type, const char* options_json, + AxonMessageCallback callback, void* user_data ) { if (!topic_name || !message_type || !callback) { return static_cast(AXON_ERROR_INVALID_ARGUMENT); @@ -131,6 +138,33 @@ static int32_t axon_subscribe( return static_cast(AXON_ERROR_NOT_INITIALIZED); } + // Parse options JSON if provided + SubscribeOptions options; + options.qos = rclcpp::QoS(10); + options.qos.reliable(); + options.qos.durability_volatile(); + + if (options_json && strlen(options_json) > 0) { + try { + nlohmann::json opts = nlohmann::json::parse(options_json); + if (opts.contains("depth_compression")) { + auto dc = opts["depth_compression"]; + DepthCompressionConfig dc_config; + dc_config.enabled = dc.value("enabled", false); + dc_config.level = dc.value("level", "medium"); + options.depth_compression = dc_config; + RCUTILS_LOG_INFO( + "Depth compression config for %s: enabled=%s, level=%s", + topic_name, + dc_config.enabled ? "true" : "false", + dc_config.level.c_str() + ); + } + } catch (const std::exception& e) { + RCUTILS_LOG_WARN("Failed to parse options JSON for %s: %s", topic_name, e.what()); + } + } + // Create lambda wrapper for the callback MessageCallback wrapper = [callback, user_data]( const std::string& topic, @@ -143,7 +177,7 @@ static int32_t axon_subscribe( ); }; - if (!g_plugin->subscribe(std::string(topic_name), std::string(message_type), wrapper)) { + if (!g_plugin->subscribe(std::string(topic_name), std::string(message_type), options, wrapper)) { return static_cast(AXON_ERROR_INTERNAL); } @@ -167,16 +201,16 @@ static int32_t axon_publish( // Plugin descriptor export // ============================================================================= -// Version information +// Version information (incremented minor version for subscribe signature change) #define AXON_ABI_VERSION_MAJOR 1 -#define AXON_ABI_VERSION_MINOR 0 +#define AXON_ABI_VERSION_MINOR 1 // Plugin vtable structure (matching loader's expectation) struct AxonPluginVtable { int32_t (*init)(const char*); int32_t (*start)(void); int32_t (*stop)(void); - int32_t (*subscribe)(const char*, const char*, AxonMessageCallback, void*); + int32_t (*subscribe)(const char*, const char*, const char*, AxonMessageCallback, void*); int32_t (*publish)(const char*, const uint8_t*, size_t, const char*); void* reserved[9]; }; diff --git a/middlewares/ros2/src/ros2_subscription_wrapper.cpp b/middlewares/ros2/src/ros2_subscription_wrapper.cpp index c50e5f3..c8fe7f2 100644 --- a/middlewares/ros2/src/ros2_subscription_wrapper.cpp +++ b/middlewares/ros2/src/ros2_subscription_wrapper.cpp @@ -6,6 +6,8 @@ #include +#include "depth_compression_filter.hpp" + namespace ros2_plugin { // ============================================================================= @@ -21,7 +23,7 @@ SubscriptionManager::~SubscriptionManager() { } bool SubscriptionManager::subscribe( - const std::string& topic_name, const std::string& message_type, const rclcpp::QoS& qos, + const std::string& topic_name, const std::string& message_type, const SubscribeOptions& options, MessageCallback callback ) { std::lock_guard lock(mutex_); @@ -33,13 +35,27 @@ bool SubscriptionManager::subscribe( } try { + // 创建压缩过滤器（如果配置了） + std::unique_ptr compression_filter; + if (options.depth_compression.has_value()) { + compression_filter = std::make_unique(*options.depth_compression); + RCUTILS_LOG_INFO( + "Depth compression enabled for topic %s: enabled=%s, level=%s", + topic_name.c_str(), + options.depth_compression->enabled ? "true" : "false", + options.depth_compression->level.c_str() + ); + } + // Create generic subscription using ROS2's built-in API - // Store callback by value to be used in lambda + // Capture topic_name by value and lookup filter in subscriptions_ map when message arrives auto subscription = node_->create_generic_subscription( topic_name, message_type, - qos, - [topic_name, message_type, callback](std::shared_ptr msg) { + options.qos, + [this, topic_name, message_type, callback]( + std::shared_ptr msg + ) { if (!callback) { return; } @@ -55,8 +71,35 @@ bool SubscriptionManager::subscribe( // Get current time as timestamp rclcpp::Time timestamp = rclcpp::Clock(RCL_SYSTEM_TIME).now(); - // Invoke callback with serialized data - callback(topic_name, message_type, data, timestamp); + // 查找压缩过滤器（如果存在） + auto it = subscriptions_.find(topic_name); + bool has_filter = (it != subscriptions_.end() && it->second.compression_filter != nullptr); + + if (has_filter) { + it->second.compression_filter->filter_and_process( + topic_name, + message_type, + data, + timestamp.nanoseconds(), + [&]( + const std::string& filtered_topic, + const std::string& filtered_type, + const std::vector& filtered_data, + uint64_t filtered_timestamp_ns + ) { + RCUTILS_LOG_DEBUG( + "Processed message on topic %s with type %s, size %zu bytes", + filtered_topic.c_str(), filtered_type.c_str(), filtered_data.size() + ); + // 将纳秒时间戳转换回 rclcpp::Time + rclcpp::Time filtered_timestamp(filtered_timestamp_ns); + callback(filtered_topic, filtered_type, filtered_data, filtered_timestamp); + } + ); + } else { + // 直接调用回调 + callback(topic_name, message_type, data, timestamp); + } } catch (const std::exception& e) { RCUTILS_LOG_ERROR( @@ -71,10 +114,11 @@ bool SubscriptionManager::subscribe( return false; } - // Store subscription info with callback + // Store subscription info with callback and filter SubscriptionInfo info; info.subscription = subscription; info.callback = callback; + info.compression_filter = std::move(compression_filter); subscriptions_[topic_name] = std::move(info); RCUTILS_LOG_INFO("Subscribed to topic: %s (%s)", topic_name.c_str(), message_type.c_str()); From f8f8102514853f44c24cc94d38ad273af5da7ca0 Mon Sep 17 00:00:00 2001 From: Liangwei XU Date: Mon, 2 Feb 2026 10:17:31 +0800 Subject: [PATCH 04/21] feat(recorder): add depth compression configuration support --- apps/axon_recorder/axon_recorder.cpp | 76 +++++++++++++++---- .../config/default_config_ros1.yaml | 2 +- .../config/default_config_ros2.yaml | 13 +++- apps/axon_recorder/config_parser.cpp | 10 +++ apps/axon_recorder/plugin_loader.hpp | 2 +- apps/axon_recorder/recorder.cpp | 18 ++++- apps/axon_recorder/recorder.hpp | 6 ++ .../integration/test_recorder_integration.cpp | 9 ++- apps/plugin_example/plugin_loader_test.cpp | 1 + 9 files changed, 116 insertions(+), 21 deletions(-) diff --git a/apps/axon_recorder/axon_recorder.cpp b/apps/axon_recorder/axon_recorder.cpp index c36ad88..7369c26 100644 --- a/apps/axon_recorder/axon_recorder.cpp +++ b/apps/axon_recorder/axon_recorder.cpp @@ -59,6 +59,7 @@ void print_usage(const char* program_name) { << " --compression ALG Compression: none, zstd, lz4 (default: zstd)\n" << " --level LEVEL Compression level (default: 3)\n" << " --queue-size SIZE Message queue capacity (default: 1024)\n" + << " --direct Start recording immediately (like rosbag record)\n" << " --version Show version information\n" << " --help Show this help message\n" << "\n" @@ -88,6 +89,11 @@ void print_usage(const char* program_name) { << " GET /rpc/stats - Get recording statistics\n" << " GET / or /health - Health check\n" << "\n" + << "Direct Recording Mode (--direct):\n" + << " When --direct is specified, the recorder starts immediately without HTTP RPC.\n" + << " This mode behaves like 'rosbag record' - recording starts on launch and stops\n" + << " on Ctrl+C. Output file is named with timestamp unless --output is specified.\n" + << "\n" << "Configuration File:\n" << " If --config is provided, most options can be specified in a YAML file.\n" << " Command-line arguments OVERRIDE config file values.\n" @@ -184,6 +190,7 @@ int main(int argc, char* argv[]) { std::string config_file; std::string cli_plugin_path; std::string cli_dataset_path; + std::string cli_output_file; std::string cli_profile; std::string cli_compression; std::string cli_output_file; @@ -250,6 +257,8 @@ int main(int argc, char* argv[]) { std::cerr << "Error: --queue-size requires a number argument" << std::endl; return 1; } + } else if (strcmp(argv[i], "--direct") == 0) { + direct_mode = true; } else { std::cerr << "Error: Unknown argument: " << argv[i] << std::endl; print_usage(argv[0]); @@ -275,6 +284,9 @@ int main(int argc, char* argv[]) { if (!cli_dataset_path.empty()) { config.dataset.path = cli_dataset_path; } + if (!cli_output_file.empty()) { + config.output_file = cli_output_file; + } if (!cli_profile.empty()) { config.profile = cli_profile; } @@ -301,6 +313,27 @@ int main(int argc, char* argv[]) { } } + // In --direct mode, generate timestamp-based output file if not specified + if (direct_mode && config.output_file.empty() && !config.dataset.path.empty()) { + // Generate timestamp-based filename: YYYYMMDD_HHMMSS.mcap + std::time_t now = std::time(nullptr); + std::tm tm_buf; + localtime_r(&now, &tm_buf); + char timestamp[32]; + std::strftime(timestamp, sizeof(timestamp), "%Y%m%d_%H%M%S", &tm_buf); + + // Ensure path ends with / + std::string path = config.dataset.path; + if (!path.empty() && path.back() != '/') { + path += '/'; + } + + config.output_file = path + std::string(timestamp) + ".mcap"; + } else if (direct_mode && config.output_file.empty()) { + // Fallback to default if no dataset path specified + config.output_file = "recording_" + std::to_string(std::time(nullptr)) + ".mcap"; + } + // Validate required arguments if (config.plugin_path.empty()) { std::cerr << "Error: --plugin (or plugin.path in config file) is required" << std::endl; @@ -314,6 +347,7 @@ int main(int argc, char* argv[]) { << " Plugin: " << config.plugin_path << "\n" << " Output: " << config.output_file << "\n" << " Dataset: " << config.dataset.path << "\n" + << " Output: " << config.output_file << "\n" << " Profile: " << config.profile << "\n" << " Compression: " << config.compression << " level " << config.compression_level << "\n" @@ -386,23 +420,37 @@ int main(int argc, char* argv[]) { std::cout << "Starting HTTP RPC server on " << config.http_server.host << ":" << config.http_server.port << "..." << std::endl; - // Register shutdown callback to set exit flag when /rpc/quit is called - recorder.set_shutdown_callback([]() { - std::cout << "\nReceived quit request via HTTP RPC..." << std::endl; - g_should_exit.store(true); - }); + if (!recorder.start()) { + std::cerr << "Error: Failed to start recording: " << recorder.get_last_error() << std::endl; + return 1; + } - if (!recorder.start_http_server(config.http_server.host, config.http_server.port)) { - std::cerr << "Warning: Failed to start HTTP server: " << recorder.get_last_error() << std::endl; - std::cerr << "Continuing without HTTP RPC control..." << std::endl; + std::cout << "Recording started. Press Ctrl+C to stop." << std::endl; } else { - std::cout << "HTTP RPC server listening on http://" << config.http_server.host << ":" - << config.http_server.port << std::endl; - } + // HTTP RPC mode - start server and wait for commands + std::cout << "Starting HTTP RPC server on " << config.http_server.host << ":" + << config.http_server.port << "..." << std::endl; + + // Register shutdown callback to set exit flag when /rpc/quit is called + recorder.set_shutdown_callback([]() { + std::cout << "\nReceived quit request via HTTP RPC..." << std::endl; + g_should_exit.store(true); + }); + + if (!recorder.start_http_server(config.http_server.host, config.http_server.port)) { + std::cerr << "Warning: Failed to start HTTP server: " << recorder.get_last_error() + << std::endl; + std::cerr << "Continuing without HTTP RPC control..." << std::endl; + } else { + std::cout << "HTTP RPC server listening on http://" << config.http_server.host << ":" + << config.http_server.port << std::endl; + } - // Recorder starts in IDLE state, waiting for RPC commands - std::cout << "Recorder ready (current state: " << recorder.get_state_string() << ")" << std::endl; - std::cout << "Waiting for RPC commands. Use Ctrl+C to quit." << std::endl; + // Recorder starts in IDLE state, waiting for RPC commands + std::cout << "Recorder ready (current state: " << recorder.get_state_string() << ")" + << std::endl; + std::cout << "Waiting for RPC commands. Use Ctrl+C to quit." << std::endl; + } // Wait for quit signal (recording controlled via RPC) while (!g_should_exit.load()) { diff --git a/apps/axon_recorder/config/default_config_ros1.yaml b/apps/axon_recorder/config/default_config_ros1.yaml index a6e95c7..4a64599 100644 --- a/apps/axon_recorder/config/default_config_ros1.yaml +++ b/apps/axon_recorder/config/default_config_ros1.yaml @@ -22,7 +22,7 @@ plugin: # ============================================================================ dataset: # Output directory for MCAP files - path: /data/recordings + path: /tmp/axon # Output filename (optional, defaults to /recording.mcap) output_file: recording.mcap diff --git a/apps/axon_recorder/config/default_config_ros2.yaml b/apps/axon_recorder/config/default_config_ros2.yaml index 02ed38e..4afb09c 100644 --- a/apps/axon_recorder/config/default_config_ros2.yaml +++ b/apps/axon_recorder/config/default_config_ros2.yaml @@ -22,7 +22,9 @@ plugin: # ============================================================================ dataset: # Output directory for MCAP files - path: /data/recordings + # When using HTTP RPC mode, output files are named as: /.mcap + # The task_id is provided via the /rpc/config endpoint + path: /tmp/axon # Output filename (optional, defaults to /recording.mcap) output_file: recording.mcap @@ -78,6 +80,15 @@ subscriptions: batch_size: 300 flush_interval_ms: 10000 + # Depth image with compression (16UC1 -> CompressedImage with RVL+LZ4) + - name: /camera/camera/depth/image_rect_raw + message_type: sensor_msgs/Image + batch_size: 300 + flush_interval_ms: 10000 + depth_compression: + enabled: true + level: medium # fast, medium, or max + # ============================================================================ # Recording Configuration # ============================================================================ diff --git a/apps/axon_recorder/config_parser.cpp b/apps/axon_recorder/config_parser.cpp index 5c6e510..9650283 100644 --- a/apps/axon_recorder/config_parser.cpp +++ b/apps/axon_recorder/config_parser.cpp @@ -158,6 +158,16 @@ bool ConfigParser::parse_subscriptions( if (subscription_node["flush_interval_ms"]) { subscription.flush_interval_ms = subscription_node["flush_interval_ms"].as(); } + // Parse depth_compression section + if (subscription_node["depth_compression"]) { + const auto& dc_node = subscription_node["depth_compression"]; + if (dc_node["enabled"]) { + subscription.depth_compression.enabled = dc_node["enabled"].as(); + } + if (dc_node["level"]) { + subscription.depth_compression.level = dc_node["level"].as(); + } + } subscriptions.push_back(subscription); } return true; diff --git a/apps/axon_recorder/plugin_loader.hpp b/apps/axon_recorder/plugin_loader.hpp index 722294d..48fa645 100644 --- a/apps/axon_recorder/plugin_loader.hpp +++ b/apps/axon_recorder/plugin_loader.hpp @@ -40,7 +40,7 @@ using AxonMessageCallback = void (*)( using AxonInitFn = AxonStatus (*)(const char*); using AxonStartFn = AxonStatus (*)(); using AxonStopFn = AxonStatus (*)(); -using AxonSubscribeFn = AxonStatus (*)(const char*, const char*, AxonMessageCallback, void*); +using AxonSubscribeFn = AxonStatus (*)(const char*, const char*, const char*, AxonMessageCallback, void*); using AxonPublishFn = AxonStatus (*)(const char*, const uint8_t*, size_t, const char*); // Plugin vtable structure diff --git a/apps/axon_recorder/recorder.cpp b/apps/axon_recorder/recorder.cpp index 43609ca..2ee7fd0 100644 --- a/apps/axon_recorder/recorder.cpp +++ b/apps/axon_recorder/recorder.cpp @@ -12,6 +12,8 @@ #include #include +#include + #include "mcap_writer_wrapper.hpp" namespace axon { @@ -541,6 +543,9 @@ bool AxonRecorder::message_handler( return false; // No active recording session } + // Note: Depth compression is now handled by the ROS2 plugin + // The plugin converts Image to CompressedImage if compression is enabled + // Get channel ID for this topic from recording session uint16_t channel_id = recording_session_->get_channel_id(topic); if (channel_id == 0) { @@ -634,8 +639,19 @@ bool AxonRecorder::setup_subscriptions() { // Setup subscriptions via plugin for (const auto& sub : config_.subscriptions) { + // Build options JSON for depth compression (if enabled) + std::string options_json; + if (sub.depth_compression.enabled) { + nlohmann::json opts; + opts["depth_compression"]["enabled"] = sub.depth_compression.enabled; + opts["depth_compression"]["level"] = sub.depth_compression.level; + options_json = opts.dump(); + } + AxonStatus status = descriptor->vtable->subscribe( - sub.topic_name.c_str(), sub.message_type.c_str(), message_callback, this + sub.topic_name.c_str(), sub.message_type.c_str(), + options_json.empty() ? nullptr : options_json.c_str(), + message_callback, this ); if (status != AXON_SUCCESS) { diff --git a/apps/axon_recorder/recorder.hpp b/apps/axon_recorder/recorder.hpp index 980d2b9..12b6d7e 100644 --- a/apps/axon_recorder/recorder.hpp +++ b/apps/axon_recorder/recorder.hpp @@ -63,6 +63,12 @@ struct SubscriptionConfig { // Batch writing configuration size_t batch_size = 1; // Number of messages to batch before writing (1 = immediate) int flush_interval_ms = 100; // Maximum time to wait before flushing (ms) + + // Depth compression configuration + struct DepthCompression { + bool enabled = false; + std::string level = "medium"; // fast, medium, max + } depth_compression; }; /** diff --git a/apps/axon_recorder/test/integration/test_recorder_integration.cpp b/apps/axon_recorder/test/integration/test_recorder_integration.cpp index 8cf0e09..285e4ea 100644 --- a/apps/axon_recorder/test/integration/test_recorder_integration.cpp +++ b/apps/axon_recorder/test/integration/test_recorder_integration.cpp @@ -89,8 +89,10 @@ class MockPlugin { // Simulate message subscription static axon::AxonStatus subscribe( - const char* topic, const char* type, axon::AxonMessageCallback callback, void* user_data + const char* topic, const char* type, const char* options_json, + axon::AxonMessageCallback callback, void* user_data ) { + (void)options_json; // Ignore options in mock if (MockPlugin::instance && callback) { MockPlugin::instance->topics.push_back(topic); MockPlugin::instance->types.push_back(type); @@ -158,9 +160,10 @@ static axon::AxonStatus mock_stop() { } static axon::AxonStatus mock_subscribe( - const char* topic, const char* type, axon::AxonMessageCallback callback, void* user_data + const char* topic, const char* type, const char* options_json, + axon::AxonMessageCallback callback, void* user_data ) { - return MockPlugin::subscribe(topic, type, callback, user_data); + return MockPlugin::subscribe(topic, type, options_json, callback, user_data); } static axon::AxonStatus mock_publish( diff --git a/apps/plugin_example/plugin_loader_test.cpp b/apps/plugin_example/plugin_loader_test.cpp index 2e91718..8430be5 100644 --- a/apps/plugin_example/plugin_loader_test.cpp +++ b/apps/plugin_example/plugin_loader_test.cpp @@ -141,6 +141,7 @@ bool test_subscribe(PluginLoader& loader, const std::string& plugin_name) { AxonStatus status = descriptor->vtable->subscribe( topic.c_str(), type.c_str(), + nullptr, // options_json (none for this test) message_callback, nullptr // user_data ); From 0f95682fe3deebf6dcf41c89eccf3366d7dfd7c5 Mon Sep 17 00:00:00 2001 From: Liangwei XU Date: Mon, 2 Feb 2026 14:50:50 +0800 Subject: [PATCH 05/21] refactor(recorder): remove deprecated --direct mode --- apps/axon_recorder/axon_recorder.cpp | 80 +++++++--------------------- 1 file changed, 18 insertions(+), 62 deletions(-) diff --git a/apps/axon_recorder/axon_recorder.cpp b/apps/axon_recorder/axon_recorder.cpp index 7369c26..0142f91 100644 --- a/apps/axon_recorder/axon_recorder.cpp +++ b/apps/axon_recorder/axon_recorder.cpp @@ -59,7 +59,6 @@ void print_usage(const char* program_name) { << " --compression ALG Compression: none, zstd, lz4 (default: zstd)\n" << " --level LEVEL Compression level (default: 3)\n" << " --queue-size SIZE Message queue capacity (default: 1024)\n" - << " --direct Start recording immediately (like rosbag record)\n" << " --version Show version information\n" << " --help Show this help message\n" << "\n" @@ -89,11 +88,6 @@ void print_usage(const char* program_name) { << " GET /rpc/stats - Get recording statistics\n" << " GET / or /health - Health check\n" << "\n" - << "Direct Recording Mode (--direct):\n" - << " When --direct is specified, the recorder starts immediately without HTTP RPC.\n" - << " This mode behaves like 'rosbag record' - recording starts on launch and stops\n" - << " on Ctrl+C. Output file is named with timestamp unless --output is specified.\n" - << "\n" << "Configuration File:\n" << " If --config is provided, most options can be specified in a YAML file.\n" << " Command-line arguments OVERRIDE config file values.\n" @@ -193,7 +187,6 @@ int main(int argc, char* argv[]) { std::string cli_output_file; std::string cli_profile; std::string cli_compression; - std::string cli_output_file; int cli_compression_level = -1; size_t cli_queue_capacity = 0; bool simple_mode = false; @@ -257,8 +250,6 @@ int main(int argc, char* argv[]) { std::cerr << "Error: --queue-size requires a number argument" << std::endl; return 1; } - } else if (strcmp(argv[i], "--direct") == 0) { - direct_mode = true; } else { std::cerr << "Error: Unknown argument: " << argv[i] << std::endl; print_usage(argv[0]); @@ -313,27 +304,6 @@ int main(int argc, char* argv[]) { } } - // In --direct mode, generate timestamp-based output file if not specified - if (direct_mode && config.output_file.empty() && !config.dataset.path.empty()) { - // Generate timestamp-based filename: YYYYMMDD_HHMMSS.mcap - std::time_t now = std::time(nullptr); - std::tm tm_buf; - localtime_r(&now, &tm_buf); - char timestamp[32]; - std::strftime(timestamp, sizeof(timestamp), "%Y%m%d_%H%M%S", &tm_buf); - - // Ensure path ends with / - std::string path = config.dataset.path; - if (!path.empty() && path.back() != '/') { - path += '/'; - } - - config.output_file = path + std::string(timestamp) + ".mcap"; - } else if (direct_mode && config.output_file.empty()) { - // Fallback to default if no dataset path specified - config.output_file = "recording_" + std::to_string(std::time(nullptr)) + ".mcap"; - } - // Validate required arguments if (config.plugin_path.empty()) { std::cerr << "Error: --plugin (or plugin.path in config file) is required" << std::endl; @@ -343,11 +313,10 @@ int main(int argc, char* argv[]) { // Print configuration std::cout << "Axon Recorder Configuration:\n" - << " Mode: " << (simple_mode ? "Simple (direct recording)" : "HTTP RPC") << "\n" + << " Mode: " << (simple_mode ? "Simple" : "HTTP RPC") << "\n" << " Plugin: " << config.plugin_path << "\n" << " Output: " << config.output_file << "\n" << " Dataset: " << config.dataset.path << "\n" - << " Output: " << config.output_file << "\n" << " Profile: " << config.profile << "\n" << " Compression: " << config.compression << " level " << config.compression_level << "\n" @@ -415,43 +384,30 @@ int main(int argc, char* argv[]) { return 0; } - // HTTP RPC mode: start HTTP server and wait for commands - // Start HTTP RPC server + // HTTP RPC mode - start server and wait for commands std::cout << "Starting HTTP RPC server on " << config.http_server.host << ":" << config.http_server.port << "..." << std::endl; - if (!recorder.start()) { - std::cerr << "Error: Failed to start recording: " << recorder.get_last_error() << std::endl; - return 1; - } - - std::cout << "Recording started. Press Ctrl+C to stop." << std::endl; - } else { - // HTTP RPC mode - start server and wait for commands - std::cout << "Starting HTTP RPC server on " << config.http_server.host << ":" - << config.http_server.port << "..." << std::endl; - - // Register shutdown callback to set exit flag when /rpc/quit is called - recorder.set_shutdown_callback([]() { - std::cout << "\nReceived quit request via HTTP RPC..." << std::endl; - g_should_exit.store(true); - }); - - if (!recorder.start_http_server(config.http_server.host, config.http_server.port)) { - std::cerr << "Warning: Failed to start HTTP server: " << recorder.get_last_error() - << std::endl; - std::cerr << "Continuing without HTTP RPC control..." << std::endl; - } else { - std::cout << "HTTP RPC server listening on http://" << config.http_server.host << ":" - << config.http_server.port << std::endl; - } + // Register shutdown callback to set exit flag when /rpc/quit is called + recorder.set_shutdown_callback([]() { + std::cout << "\nReceived quit request via HTTP RPC..." << std::endl; + g_should_exit.store(true); + }); - // Recorder starts in IDLE state, waiting for RPC commands - std::cout << "Recorder ready (current state: " << recorder.get_state_string() << ")" + if (!recorder.start_http_server(config.http_server.host, config.http_server.port)) { + std::cerr << "Warning: Failed to start HTTP server: " << recorder.get_last_error() << std::endl; - std::cout << "Waiting for RPC commands. Use Ctrl+C to quit." << std::endl; + std::cerr << "Continuing without HTTP RPC control..." << std::endl; + } else { + std::cout << "HTTP RPC server listening on http://" << config.http_server.host << ":" + << config.http_server.port << std::endl; } + // Recorder starts in IDLE state, waiting for RPC commands + std::cout << "Recorder ready (current state: " << recorder.get_state_string() << ")" + << std::endl; + std::cout << "Waiting for RPC commands. Use Ctrl+C to quit." << std::endl; + // Wait for quit signal (recording controlled via RPC) while (!g_should_exit.load()) { std::this_thread::sleep_for(std::chrono::seconds(1)); From 1f764f98c26f6c85bbf05c7256df09911eceff1a Mon Sep 17 00:00:00 2001 From: Liangwei XU Date: Mon, 2 Feb 2026 17:54:49 +0800 Subject: [PATCH 06/21] feat(recorder): fix depth compression channel registration --- CMakeLists.txt | 5 + apps/axon_recorder/axon_recorder.cpp | 6 +- .../config/default_config_ros2.yaml | 6 +- apps/axon_recorder/config_parser.cpp | 18 +- apps/axon_recorder/plugin_loader.hpp | 3 +- apps/axon_recorder/recorder.cpp | 49 +- apps/axon_recorder/recorder.hpp | 4 +- apps/axon_recorder/recording_session.cpp | 71 ++- apps/axon_recorder/recording_session.hpp | 16 +- .../integration/test_recorder_integration.cpp | 4 +- .../test/unit/test_recording_session.cpp | 34 +- .../test/unit/test_worker_thread_pool.cpp | 438 ++++++++++-------- apps/axon_recorder/worker_thread_pool.cpp | 14 +- apps/axon_recorder/worker_thread_pool.hpp | 8 +- middlewares/filters/CMakeLists.txt | 7 +- middlewares/ros2/CMakeLists.txt | 31 +- .../ros2/include/depth_compression_filter.hpp | 16 +- .../ros2/src/depth_compression_filter.cpp | 65 ++- middlewares/ros2/src/ros2_plugin_export.cpp | 16 +- .../ros2/src/ros2_subscription_wrapper.cpp | 19 +- middlewares/ros2/test/test_ros2_plugin.cpp | 47 +- 21 files changed, 527 insertions(+), 350 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0624c3a..331236b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -119,6 +119,11 @@ if(EXISTS "${AXON_FILTERS_DIR}/CMakeLists.txt") add_subdirectory(${AXON_FILTERS_DIR} ${CMAKE_CURRENT_BINARY_DIR}/filters) endif() +# Middleware plugins (ROS1, ROS2, Zenoh) +if(EXISTS "${AXON_MIDDLEWARES_DIR}/CMakeLists.txt") + add_subdirectory(${AXON_MIDDLEWARES_DIR} ${CMAKE_CURRENT_BINARY_DIR}/middlewares) +endif() + # ============================================================================= # Applications # ============================================================================= diff --git a/apps/axon_recorder/axon_recorder.cpp b/apps/axon_recorder/axon_recorder.cpp index 0142f91..8759509 100644 --- a/apps/axon_recorder/axon_recorder.cpp +++ b/apps/axon_recorder/axon_recorder.cpp @@ -395,8 +395,7 @@ int main(int argc, char* argv[]) { }); if (!recorder.start_http_server(config.http_server.host, config.http_server.port)) { - std::cerr << "Warning: Failed to start HTTP server: " << recorder.get_last_error() - << std::endl; + std::cerr << "Warning: Failed to start HTTP server: " << recorder.get_last_error() << std::endl; std::cerr << "Continuing without HTTP RPC control..." << std::endl; } else { std::cout << "HTTP RPC server listening on http://" << config.http_server.host << ":" @@ -404,8 +403,7 @@ int main(int argc, char* argv[]) { } // Recorder starts in IDLE state, waiting for RPC commands - std::cout << "Recorder ready (current state: " << recorder.get_state_string() << ")" - << std::endl; + std::cout << "Recorder ready (current state: " << recorder.get_state_string() << ")" << std::endl; std::cout << "Waiting for RPC commands. Use Ctrl+C to quit." << std::endl; // Wait for quit signal (recording controlled via RPC) diff --git a/apps/axon_recorder/config/default_config_ros2.yaml b/apps/axon_recorder/config/default_config_ros2.yaml index 4afb09c..8cc5132 100644 --- a/apps/axon_recorder/config/default_config_ros2.yaml +++ b/apps/axon_recorder/config/default_config_ros2.yaml @@ -82,12 +82,10 @@ subscriptions: # Depth image with compression (16UC1 -> CompressedImage with RVL+LZ4) - name: /camera/camera/depth/image_rect_raw - message_type: sensor_msgs/Image + message_type: sensor_msgs/msg/Image batch_size: 300 flush_interval_ms: 10000 - depth_compression: - enabled: true - level: medium # fast, medium, or max + depth_compression: true # ============================================================================ # Recording Configuration diff --git a/apps/axon_recorder/config_parser.cpp b/apps/axon_recorder/config_parser.cpp index 9650283..346a7d9 100644 --- a/apps/axon_recorder/config_parser.cpp +++ b/apps/axon_recorder/config_parser.cpp @@ -158,14 +158,20 @@ bool ConfigParser::parse_subscriptions( if (subscription_node["flush_interval_ms"]) { subscription.flush_interval_ms = subscription_node["flush_interval_ms"].as(); } - // Parse depth_compression section + // Parse depth_compression section (can be boolean or object) if (subscription_node["depth_compression"]) { const auto& dc_node = subscription_node["depth_compression"]; - if (dc_node["enabled"]) { - subscription.depth_compression.enabled = dc_node["enabled"].as(); - } - if (dc_node["level"]) { - subscription.depth_compression.level = dc_node["level"].as(); + // Check if it's a boolean (simple form: depth_compression: true) + if (dc_node.IsScalar()) { + subscription.depth_compression.enabled = dc_node.as(); + } else if (dc_node.IsMap()) { + // Object form: depth_compression: { enabled: true, level: fast } + if (dc_node["enabled"]) { + subscription.depth_compression.enabled = dc_node["enabled"].as(); + } + if (dc_node["level"]) { + subscription.depth_compression.level = dc_node["level"].as(); + } } } subscriptions.push_back(subscription); diff --git a/apps/axon_recorder/plugin_loader.hpp b/apps/axon_recorder/plugin_loader.hpp index 48fa645..907e6d9 100644 --- a/apps/axon_recorder/plugin_loader.hpp +++ b/apps/axon_recorder/plugin_loader.hpp @@ -40,7 +40,8 @@ using AxonMessageCallback = void (*)( using AxonInitFn = AxonStatus (*)(const char*); using AxonStartFn = AxonStatus (*)(); using AxonStopFn = AxonStatus (*)(); -using AxonSubscribeFn = AxonStatus (*)(const char*, const char*, const char*, AxonMessageCallback, void*); +using AxonSubscribeFn = + AxonStatus (*)(const char*, const char*, const char*, AxonMessageCallback, void*); using AxonPublishFn = AxonStatus (*)(const char*, const uint8_t*, size_t, const char*); // Plugin vtable structure diff --git a/apps/axon_recorder/recorder.cpp b/apps/axon_recorder/recorder.cpp index 2ee7fd0..2265b5b 100644 --- a/apps/axon_recorder/recorder.cpp +++ b/apps/axon_recorder/recorder.cpp @@ -4,6 +4,8 @@ #include "recorder.hpp" +#include + #include #include #include @@ -12,8 +14,6 @@ #include #include -#include - #include "mcap_writer_wrapper.hpp" namespace axon { @@ -177,7 +177,7 @@ bool AxonRecorder::start() { // Setup subscriptions via plugin if (!setup_subscriptions()) { - set_error_helper("Failed to setup subscriptions"); + set_error_helper("Failed to setup subscriptions: " + get_last_error()); state_manager_.transition_to(RecorderState::IDLE, error_msg); recording_session_.reset(); return false; @@ -525,6 +525,7 @@ void AxonRecorder::on_message( // Create MessageItem and push to worker pool MessageItem item; item.timestamp_ns = static_cast(timestamp); + item.message_type = message_type; // Save the message type item.raw_data.assign(message_data, message_data + message_size); // Push to the topic's queue (worker pool handles this) @@ -536,8 +537,8 @@ void AxonRecorder::on_message( } bool AxonRecorder::message_handler( - const std::string& topic, int64_t timestamp_ns, const uint8_t* data, size_t data_size, - uint32_t sequence + const std::string& topic, const std::string& message_type, int64_t timestamp_ns, + const uint8_t* data, size_t data_size, uint32_t sequence ) { if (!recording_session_) { return false; // No active recording session @@ -545,11 +546,12 @@ bool AxonRecorder::message_handler( // Note: Depth compression is now handled by the ROS2 plugin // The plugin converts Image to CompressedImage if compression is enabled + // message_type may differ from the original subscription type - // Get channel ID for this topic from recording session - uint16_t channel_id = recording_session_->get_channel_id(topic); + // Get or create channel ID for this topic + message_type combination + uint16_t channel_id = recording_session_->get_or_create_channel(topic, message_type); if (channel_id == 0) { - return false; // Channel not found + return false; // Channel not found or failed to create } uint64_t log_time_ns = static_cast(timestamp_ns); @@ -590,9 +592,10 @@ bool AxonRecorder::register_topics() { return false; } - // Register channel + // Register channel using composite key (topic + message_type) + // This allows the same topic to have different message types (e.g., Image and CompressedImage) uint16_t channel_id = recording_session_->register_channel( - sub.topic_name, config_.profile == "ros1" ? "ros1" : "cdr", schema_id + sub.topic_name, sub.message_type, config_.profile == "ros1" ? "ros1" : "cdr", schema_id ); if (channel_id == 0) { @@ -608,11 +611,12 @@ bool AxonRecorder::register_topics() { auto handler = std::bind( &AxonRecorder::message_handler, this, - std::placeholders::_1, - std::placeholders::_2, - std::placeholders::_3, - std::placeholders::_4, - std::placeholders::_5 + std::placeholders::_1, // topic + std::placeholders::_2, // message_type + std::placeholders::_3, // timestamp_ns + std::placeholders::_4, // data + std::placeholders::_5, // data_size + std::placeholders::_6 // sequence ); if (!worker_pool_->create_topic_worker(sub.topic_name, handler)) { @@ -632,8 +636,13 @@ bool AxonRecorder::setup_subscriptions() { } const auto* descriptor = plugin_loader_.get_descriptor(plugins[0]); - if (!descriptor || !descriptor->vtable || !descriptor->vtable->subscribe) { - set_error_helper("Plugin does not support subscribe"); + if (!descriptor || !descriptor->vtable) { + set_error_helper("Invalid plugin descriptor or vtable"); + return false; + } + + if (!descriptor->vtable->subscribe) { + set_error_helper("Plugin does not support subscribe (null function pointer)"); return false; } @@ -649,9 +658,11 @@ bool AxonRecorder::setup_subscriptions() { } AxonStatus status = descriptor->vtable->subscribe( - sub.topic_name.c_str(), sub.message_type.c_str(), + sub.topic_name.c_str(), + sub.message_type.c_str(), options_json.empty() ? nullptr : options_json.c_str(), - message_callback, this + message_callback, + this ); if (status != AXON_SUCCESS) { diff --git a/apps/axon_recorder/recorder.hpp b/apps/axon_recorder/recorder.hpp index 12b6d7e..71a1288 100644 --- a/apps/axon_recorder/recorder.hpp +++ b/apps/axon_recorder/recorder.hpp @@ -336,8 +336,8 @@ class AxonRecorder { * Called by per-topic worker threads to process messages */ bool message_handler( - const std::string& topic, int64_t timestamp_ns, const uint8_t* data, size_t data_size, - uint32_t sequence + const std::string& topic, const std::string& message_type, int64_t timestamp_ns, + const uint8_t* data, size_t data_size, uint32_t sequence ); /** diff --git a/apps/axon_recorder/recording_session.cpp b/apps/axon_recorder/recording_session.cpp index 898ca22..0271390 100644 --- a/apps/axon_recorder/recording_session.cpp +++ b/apps/axon_recorder/recording_session.cpp @@ -153,18 +153,21 @@ uint16_t RecordingSession::register_schema( } uint16_t RecordingSession::register_channel( - const std::string& topic, const std::string& message_encoding, uint16_t schema_id, - const std::unordered_map& metadata + const std::string& topic, const std::string& message_type, const std::string& message_encoding, + uint16_t schema_id, const std::unordered_map& metadata ) { if (!is_open()) { AXON_LOG_WARN("Cannot register channel, session not open"); return 0; } + // Create a composite key for topic+message_type + std::string composite_key = topic + "\n" + message_type; + // Check if already registered { std::lock_guard lock(registry_mutex_); - auto it = topic_channel_ids_.find(topic); + auto it = topic_channel_ids_.find(composite_key); if (it != topic_channel_ids_.end()) { return it->second; } @@ -172,13 +175,16 @@ uint16_t RecordingSession::register_channel( uint16_t channel_id = writer_->register_channel(topic, message_encoding, schema_id, metadata); if (channel_id == 0) { - AXON_LOG_ERROR("Failed to register channel" << axon::logging::kv("topic", topic)); + AXON_LOG_ERROR( + "Failed to register channel" << axon::logging::kv("topic", topic) + << axon::logging::kv("message_type", message_type) + ); return 0; } { std::lock_guard lock(registry_mutex_); - topic_channel_ids_[topic] = channel_id; + topic_channel_ids_[composite_key] = channel_id; } return channel_id; @@ -207,6 +213,61 @@ uint16_t RecordingSession::get_channel_id(const std::string& topic) const { return (it != topic_channel_ids_.end()) ? it->second : 0; } +uint16_t RecordingSession::get_or_create_channel( + const std::string& topic, const std::string& message_type +) { + // Create a composite key for topic+message_type + std::string composite_key = topic + "\n" + message_type; + + // Check if channel already exists for this topic+type combination + { + std::lock_guard lock(registry_mutex_); + auto it = topic_channel_ids_.find(composite_key); + if (it != topic_channel_ids_.end()) { + return it->second; + } + } + + // Get or create schema for this message type + uint16_t schema_id = get_schema_id(message_type); + if (schema_id == 0) { + // Schema not registered, auto-register it + // This can happen when depth compression filter creates CompressedImage messages + AXON_LOG_DEBUG("Auto-registering schema for message type: " << message_type); + schema_id = register_schema(message_type, "ros2msg", ""); + if (schema_id == 0) { + AXON_LOG_ERROR( + "Failed to auto-register schema for: " << message_type + << ", cannot create channel for topic: " << topic + ); + return 0; + } + AXON_LOG_DEBUG( + "Successfully auto-registered schema for: " << message_type + << " with schema_id: " << schema_id + ); + } + + // Register new channel + uint16_t channel_id = register_channel(topic, message_type, "cdr", schema_id, {}); + if (channel_id == 0) { + return 0; + } + + // Store with composite key + { + std::lock_guard lock(registry_mutex_); + topic_channel_ids_[composite_key] = channel_id; + } + + AXON_LOG_INFO( + "Created new channel for compressed topic: " << topic << " with type: " << message_type + << " channel_id: " << channel_id + ); + + return channel_id; +} + uint16_t RecordingSession::get_schema_id(const std::string& message_type) const { std::lock_guard lock(registry_mutex_); auto it = message_type_schema_ids_.find(message_type); diff --git a/apps/axon_recorder/recording_session.hpp b/apps/axon_recorder/recording_session.hpp index 3cc1fee..c53a2dd 100644 --- a/apps/axon_recorder/recording_session.hpp +++ b/apps/axon_recorder/recording_session.hpp @@ -105,14 +105,15 @@ class RecordingSession { * Register a channel (topic). * * @param topic Topic name (e.g., "/camera/image_raw") + * @param message_type Message type (e.g., "sensor_msgs/msg/Image") * @param message_encoding Message encoding ("ros1", "cdr", etc.) * @param schema_id Schema ID from register_schema() * @param metadata Optional channel metadata * @return Channel ID for use with write(), or 0 on failure */ uint16_t register_channel( - const std::string& topic, const std::string& message_encoding, uint16_t schema_id, - const std::unordered_map& metadata = {} + const std::string& topic, const std::string& message_type, const std::string& message_encoding, + uint16_t schema_id, const std::unordered_map& metadata = {} ); /** @@ -140,6 +141,17 @@ class RecordingSession { */ uint16_t get_channel_id(const std::string& topic) const; + /** + * Get or create a channel ID for a topic with a specific message type. + * If the topic+message_type combination doesn't exist, a new channel is created. + * This is used for dynamic message type conversion (e.g., depth compression). + * + * @param topic Topic name + * @param message_type Message type name + * @return Channel ID, or 0 if failed + */ + uint16_t get_or_create_channel(const std::string& topic, const std::string& message_type); + /** * Get the schema ID for a message type. * diff --git a/apps/axon_recorder/test/integration/test_recorder_integration.cpp b/apps/axon_recorder/test/integration/test_recorder_integration.cpp index 285e4ea..be440db 100644 --- a/apps/axon_recorder/test/integration/test_recorder_integration.cpp +++ b/apps/axon_recorder/test/integration/test_recorder_integration.cpp @@ -160,8 +160,8 @@ static axon::AxonStatus mock_stop() { } static axon::AxonStatus mock_subscribe( - const char* topic, const char* type, const char* options_json, - axon::AxonMessageCallback callback, void* user_data + const char* topic, const char* type, const char* options_json, axon::AxonMessageCallback callback, + void* user_data ) { return MockPlugin::subscribe(topic, type, options_json, callback, user_data); } diff --git a/apps/axon_recorder/test/unit/test_recording_session.cpp b/apps/axon_recorder/test/unit/test_recording_session.cpp index 6c18295..66711ba 100644 --- a/apps/axon_recorder/test/unit/test_recording_session.cpp +++ b/apps/axon_recorder/test/unit/test_recording_session.cpp @@ -225,7 +225,8 @@ TEST_F(RecordingSessionTest, RegisterChannel) { std::string topic = "/test/topic"; std::string encoding = "cdr"; - uint16_t channel_id = session.register_channel(topic, encoding, schema_id); + std::string message_type = "std_msgs/msg/String"; + uint16_t channel_id = session.register_channel(topic, message_type, encoding, schema_id); EXPECT_NE(channel_id, 0); EXPECT_EQ(session.get_channel_id(topic), channel_id); @@ -234,7 +235,7 @@ TEST_F(RecordingSessionTest, RegisterChannel) { TEST_F(RecordingSessionTest, RegisterChannelFailsWhenNotOpen) { RecordingSession session; - uint16_t channel_id = session.register_channel("/test", "cdr", 1); + uint16_t channel_id = session.register_channel("/test", "std_msgs/msg/String", "cdr", 1); EXPECT_EQ(channel_id, 0); } @@ -249,8 +250,8 @@ TEST_F(RecordingSessionTest, RegisterChannelReturnsSameIdForDuplicate) { uint16_t schema_id = session.register_schema("std_msgs/msg/String", "ros2msg", "string data"); std::string topic = "/test/topic"; - uint16_t id1 = session.register_channel(topic, "cdr", schema_id); - uint16_t id2 = session.register_channel(topic, "cdr", schema_id); + uint16_t id1 = session.register_channel(topic, "std_msgs/msg/String", "cdr", schema_id); + uint16_t id2 = session.register_channel(topic, "std_msgs/msg/String", "cdr", schema_id); EXPECT_EQ(id1, id2); } @@ -268,7 +269,8 @@ TEST_F(RecordingSessionTest, RegisterChannelWithMetadata) { {"qos_depth", "10"}, {"qos_reliable", "true"} }; - uint16_t channel_id = session.register_channel("/test", "cdr", schema_id, metadata); + uint16_t channel_id = + session.register_channel("/test", "std_msgs/msg/String", "cdr", schema_id, metadata); EXPECT_NE(channel_id, 0); } @@ -282,9 +284,9 @@ TEST_F(RecordingSessionTest, RegisterMultipleChannels) { uint16_t schema_id = session.register_schema("std_msgs/msg/String", "ros2msg", "string data"); - uint16_t id1 = session.register_channel("/topic1", "cdr", schema_id); - uint16_t id2 = session.register_channel("/topic2", "cdr", schema_id); - uint16_t id3 = session.register_channel("/topic3", "cdr", schema_id); + uint16_t id1 = session.register_channel("/topic1", "std_msgs/msg/String", "cdr", schema_id); + uint16_t id2 = session.register_channel("/topic2", "std_msgs/msg/String", "cdr", schema_id); + uint16_t id3 = session.register_channel("/topic3", "std_msgs/msg/String", "cdr", schema_id); EXPECT_NE(id1, 0); EXPECT_NE(id2, 0); @@ -305,7 +307,7 @@ TEST_F(RecordingSessionTest, WriteMessage) { ASSERT_TRUE(session.open(path, options)); uint16_t schema_id = session.register_schema("std_msgs/msg/String", "ros2msg", "string data"); - uint16_t channel_id = session.register_channel("/test", "cdr", schema_id); + uint16_t channel_id = session.register_channel("/test", "std_msgs/msg/String", "cdr", schema_id); std::string data = "test message"; uint64_t timestamp = std::chrono::duration_cast( @@ -338,7 +340,7 @@ TEST_F(RecordingSessionTest, WriteMultipleMessages) { ASSERT_TRUE(session.open(path, options)); uint16_t schema_id = session.register_schema("std_msgs/msg/String", "ros2msg", "string data"); - uint16_t channel_id = session.register_channel("/test", "cdr", schema_id); + uint16_t channel_id = session.register_channel("/test", "std_msgs/msg/String", "cdr", schema_id); uint64_t timestamp = std::chrono::duration_cast( std::chrono::system_clock::now().time_since_epoch() @@ -389,8 +391,8 @@ TEST_F(RecordingSessionTest, StatisticsAfterRegistrations) { session.register_schema("sensor_msgs/msg/Image", "ros2msg", "uint32 height"); uint16_t schema_id = session.register_schema("std_msgs/msg/String", "ros2msg", "string data"); - session.register_channel("/topic1", "cdr", schema_id); - session.register_channel("/topic2", "cdr", schema_id); + session.register_channel("/topic1", "std_msgs/msg/String", "cdr", schema_id); + session.register_channel("/topic2", "std_msgs/msg/String", "cdr", schema_id); auto stats = session.get_stats(); @@ -406,7 +408,7 @@ TEST_F(RecordingSessionTest, StatisticsAfterWriting) { session.open(path, options); uint16_t schema_id = session.register_schema("std_msgs/msg/String", "ros2msg", "string data"); - uint16_t channel_id = session.register_channel("/test", "cdr", schema_id); + uint16_t channel_id = session.register_channel("/test", "std_msgs/msg/String", "cdr", schema_id); uint64_t timestamp = std::chrono::duration_cast( std::chrono::system_clock::now().time_since_epoch() @@ -468,7 +470,7 @@ TEST_F(RecordingSessionTest, FlushWhenOpen) { session.open(path, options); uint16_t schema_id = session.register_schema("std_msgs/msg/String", "ros2msg", "string data"); - uint16_t channel_id = session.register_channel("/test", "cdr", schema_id); + uint16_t channel_id = session.register_channel("/test", "std_msgs/msg/String", "cdr", schema_id); std::string data = "test message"; uint64_t timestamp = std::chrono::duration_cast( @@ -547,7 +549,7 @@ TEST_F(RecordingSessionTest, SidecarContainsChecksum) { // Write some data uint16_t schema_id = session.register_schema("std_msgs/msg/String", "ros2msg", "string data"); - uint16_t channel_id = session.register_channel("/test", "cdr", schema_id); + uint16_t channel_id = session.register_channel("/test", "std_msgs/msg/String", "cdr", schema_id); std::string data = "test message for checksum"; uint64_t timestamp = std::chrono::duration_cast( @@ -632,7 +634,7 @@ TEST_F(RecordingSessionTest, ConcurrentWrites) { ASSERT_TRUE(session.open(path, options)); uint16_t schema_id = session.register_schema("std_msgs/msg/String", "ros2msg", "string data"); - uint16_t channel_id = session.register_channel("/test", "cdr", schema_id); + uint16_t channel_id = session.register_channel("/test", "std_msgs/msg/String", "cdr", schema_id); constexpr int num_threads = 4; constexpr int messages_per_thread = 50; diff --git a/apps/axon_recorder/test/unit/test_worker_thread_pool.cpp b/apps/axon_recorder/test/unit/test_worker_thread_pool.cpp index a0d32b2..5d19c9b 100644 --- a/apps/axon_recorder/test/unit/test_worker_thread_pool.cpp +++ b/apps/axon_recorder/test/unit/test_worker_thread_pool.cpp @@ -79,6 +79,7 @@ TEST_F(WorkerThreadPoolTest, CreateTopicWorker) { std::atomic message_count{0}; auto handler = [&]( const std::string& /* topic */, + const std::string& /* message_type */, int64_t /* ts */, const uint8_t* /* data */, size_t /* size */, @@ -97,9 +98,10 @@ TEST_F(WorkerThreadPoolTest, CreateTopicWorker) { } TEST_F(WorkerThreadPoolTest, CannotCreateDuplicateTopic) { - auto handler = [](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - return true; - }; + auto handler = + [](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + return true; + }; EXPECT_TRUE(pool_->create_topic_worker("/test", handler)); EXPECT_FALSE(pool_->create_topic_worker("/test", handler)); @@ -107,9 +109,10 @@ TEST_F(WorkerThreadPoolTest, CannotCreateDuplicateTopic) { } TEST_F(WorkerThreadPoolTest, RemoveTopicWorker) { - auto handler = [](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - return true; - }; + auto handler = + [](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + return true; + }; pool_->create_topic_worker("/topic1", handler); pool_->create_topic_worker("/topic2", handler); @@ -124,18 +127,19 @@ TEST_F(WorkerThreadPoolTest, RemoveTopicWorker) { } TEST_F(WorkerThreadPoolTest, TryPush) { - auto handler = [](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - return true; - }; + auto handler = + [](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + return true; + }; pool_->create_topic_worker("/test", handler); // Push should succeed when pool exists - MessageItem item(12345, {0x01, 0x02}); + MessageItem item(12345, "test_type", {0x01, 0x02}); EXPECT_TRUE(pool_->try_push("/test", std::move(item))); // Push to non-existent topic should fail - MessageItem item2(12345, {0x01}); + MessageItem item2(12345, "test_type", {0x01}); EXPECT_FALSE(pool_->try_push("/nonexistent", std::move(item2))); } @@ -146,6 +150,7 @@ TEST_F(WorkerThreadPoolTest, MessageProcessing) { auto handler = [&]( const std::string& topic, + const std::string& /* message_type */, int64_t ts, const uint8_t* /* data */, size_t /* size */, @@ -165,7 +170,7 @@ TEST_F(WorkerThreadPoolTest, MessageProcessing) { // Push some messages for (int i = 0; i < 10; ++i) { - MessageItem item(i * 1000, {static_cast(i)}); + MessageItem item(i * 1000, "test_type", {static_cast(i)}); EXPECT_TRUE(pool_->try_push("/test", std::move(item))); } @@ -193,10 +198,11 @@ TEST_F(WorkerThreadPoolTest, MessageProcessing) { TEST_F(WorkerThreadPoolTest, PauseResume) { std::atomic processed_count{0}; - auto handler = [&](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - processed_count.fetch_add(1); - return true; - }; + auto handler = + [&](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + processed_count.fetch_add(1); + return true; + }; pool_->create_topic_worker("/test", handler); pool_->start(); @@ -208,7 +214,7 @@ TEST_F(WorkerThreadPoolTest, PauseResume) { // Push messages while paused for (int i = 0; i < 5; ++i) { - MessageItem item(i, {static_cast(i)}); + MessageItem item(i, "test_type", {static_cast(i)}); pool_->try_push("/test", std::move(item)); } @@ -241,16 +247,17 @@ TEST_F(WorkerThreadPoolTest, PauseResume) { TEST_F(WorkerThreadPoolTest, StatsTracking) { std::atomic should_succeed{true}; - auto handler = [&](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - return should_succeed.load(); - }; + auto handler = + [&](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + return should_succeed.load(); + }; pool_->create_topic_worker("/test", handler); pool_->start(); // Push messages for (int i = 0; i < 10; ++i) { - MessageItem item(i, {static_cast(i)}); + MessageItem item(i, "test_type", {static_cast(i)}); pool_->try_push("/test", std::move(item)); } @@ -267,9 +274,10 @@ TEST_F(WorkerThreadPoolTest, StatsTracking) { } TEST_F(WorkerThreadPoolTest, AggregateStats) { - auto handler = [](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - return true; - }; + auto handler = + [](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + return true; + }; pool_->create_topic_worker("/topic1", handler); pool_->create_topic_worker("/topic2", handler); @@ -277,13 +285,13 @@ TEST_F(WorkerThreadPoolTest, AggregateStats) { // Push to topic1 for (int i = 0; i < 5; ++i) { - MessageItem item(i, {static_cast(i)}); + MessageItem item(i, "test_type", {static_cast(i)}); pool_->try_push("/topic1", std::move(item)); } // Push to topic2 for (int i = 0; i < 3; ++i) { - MessageItem item(i, {static_cast(i)}); + MessageItem item(i, "test_type", {static_cast(i)}); pool_->try_push("/topic2", std::move(item)); } @@ -301,16 +309,17 @@ TEST_F(WorkerThreadPoolTest, AggregateStats) { TEST_F(WorkerThreadPoolTest, QueueDrainOnStop) { std::atomic processed{0}; - auto handler = [&](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - processed.fetch_add(1); - return true; - }; + auto handler = + [&](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + processed.fetch_add(1); + return true; + }; pool_->create_topic_worker("/test", handler); // Don't start yet - just queue messages for (int i = 0; i < 100; ++i) { - MessageItem item(i, {static_cast(i % 256)}); + MessageItem item(i, "test_type", {static_cast(i % 256)}); pool_->try_push("/test", std::move(item)); } @@ -326,18 +335,19 @@ TEST_F(WorkerThreadPoolTest, SequenceNumbers) { std::vector received_sequences; std::mutex seq_mutex; - auto handler = [&](const std::string&, int64_t, const uint8_t*, size_t, uint32_t seq) { - std::lock_guard lock(seq_mutex); - received_sequences.push_back(seq); - return true; - }; + auto handler = + [&](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t seq) { + std::lock_guard lock(seq_mutex); + received_sequences.push_back(seq); + return true; + }; pool_->create_topic_worker("/test", handler); pool_->start(); // Push messages for (int i = 0; i < 10; ++i) { - MessageItem item(i, {static_cast(i)}); + MessageItem item(i, "test_type", {static_cast(i)}); pool_->try_push("/test", std::move(item)); } @@ -376,9 +386,10 @@ TEST_F(WorkerThreadPoolTest, StopCalledMultipleTimes) { } TEST_F(WorkerThreadPoolTest, TryPushAfterStop) { - auto handler = [](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - return true; - }; + auto handler = + [](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + return true; + }; pool_->create_topic_worker("/test", handler); pool_->start(); @@ -386,7 +397,7 @@ TEST_F(WorkerThreadPoolTest, TryPushAfterStop) { // Try to push after stop - behavior depends on implementation // The queue might still accept items even after stop - MessageItem item(12345, {0x01, 0x02}); + MessageItem item(12345, "test_type", {0x01, 0x02}); bool result = pool_->try_push("/test", std::move(item)); // Just verify it doesn't crash - result may vary @@ -404,9 +415,10 @@ TEST_F(WorkerThreadPoolTest, GetTopicStatsNonexistentTopic) { } TEST_F(WorkerThreadPoolTest, CreateTopicWorkerDuplicateName) { - auto handler = [](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - return true; - }; + auto handler = + [](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + return true; + }; // First creation should succeed EXPECT_TRUE(pool_->create_topic_worker("/duplicate", handler)); @@ -422,9 +434,10 @@ TEST_F(WorkerThreadPoolTest, CreateTopicWorkerDuplicateName) { } TEST_F(WorkerThreadPoolTest, RemoveNonexistentTopic) { - auto handler = [](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - return true; - }; + auto handler = + [](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + return true; + }; pool_->create_topic_worker("/exists", handler); EXPECT_EQ(pool_->topic_count(), 1); @@ -456,25 +469,27 @@ TEST_F(WorkerThreadPoolTest, CreateTopicWorkerWhileRunning) { pool_->start(); EXPECT_TRUE(pool_->is_running()); - auto handler = [](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - return true; - }; + auto handler = + [](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + return true; + }; // Should be able to create topic while running EXPECT_TRUE(pool_->create_topic_worker("/new_topic", handler)); EXPECT_EQ(pool_->topic_count(), 1); // Push to new topic - MessageItem item(12345, {0x01}); + MessageItem item(12345, "test_type", {0x01}); EXPECT_TRUE(pool_->try_push("/new_topic", std::move(item))); pool_->stop(); } TEST_F(WorkerThreadPoolTest, RemoveTopicWorkerWhileRunning) { - auto handler = [](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - return true; - }; + auto handler = + [](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + return true; + }; pool_->create_topic_worker("/topic1", handler); pool_->create_topic_worker("/topic2", handler); @@ -487,11 +502,11 @@ TEST_F(WorkerThreadPoolTest, RemoveTopicWorkerWhileRunning) { EXPECT_EQ(pool_->topic_count(), 1); // Push to removed topic should fail - MessageItem item(12345, {0x01}); + MessageItem item(12345, "test_type", {0x01}); EXPECT_FALSE(pool_->try_push("/topic1", std::move(item))); // Push to remaining topic should work - MessageItem item2(12345, {0x02}); + MessageItem item2(12345, "test_type", {0x02}); EXPECT_TRUE(pool_->try_push("/topic2", std::move(item2))); pool_->stop(); @@ -500,16 +515,17 @@ TEST_F(WorkerThreadPoolTest, RemoveTopicWorkerWhileRunning) { TEST_F(WorkerThreadPoolTest, HandlerReturnsFailure) { std::atomic call_count{0}; - auto handler = [&](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - call_count.fetch_add(1); - return false; // Always fail - }; + auto handler = + [&](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + call_count.fetch_add(1); + return false; // Always fail + }; pool_->create_topic_worker("/test", handler); pool_->start(); for (int i = 0; i < 5; ++i) { - MessageItem item(i, {static_cast(i)}); + MessageItem item(i, "test_type", {static_cast(i)}); pool_->try_push("/test", std::move(item)); } @@ -532,17 +548,18 @@ TEST_F(WorkerThreadPoolTest, EmptyMessageData) { std::atomic handler_called{false}; size_t received_size = 999; - auto handler = [&](const std::string&, int64_t, const uint8_t*, size_t size, uint32_t) { - handler_called = true; - received_size = size; - return true; - }; + auto handler = + [&](const std::string&, const std::string&, int64_t, const uint8_t*, size_t size, uint32_t) { + handler_called = true; + received_size = size; + return true; + }; pool_->create_topic_worker("/test", handler); pool_->start(); // Push empty message - MessageItem item(12345, {}); // Empty data + MessageItem item(12345, "test_type", {}); // Empty data pool_->try_push("/test", std::move(item)); std::this_thread::sleep_for(std::chrono::milliseconds(100)); @@ -555,17 +572,18 @@ TEST_F(WorkerThreadPoolTest, EmptyMessageData) { TEST_F(WorkerThreadPoolTest, LargeMessageData) { size_t received_size = 0; - auto handler = [&](const std::string&, int64_t, const uint8_t*, size_t size, uint32_t) { - received_size = size; - return true; - }; + auto handler = + [&](const std::string&, const std::string&, int64_t, const uint8_t*, size_t size, uint32_t) { + received_size = size; + return true; + }; pool_->create_topic_worker("/test", handler); pool_->start(); // Push large message (1MB) std::vector large_data(1024 * 1024, 0xAB); - MessageItem item(12345, std::move(large_data)); + MessageItem item(12345, "test_type", std::move(large_data)); pool_->try_push("/test", std::move(item)); std::this_thread::sleep_for(std::chrono::milliseconds(200)); @@ -575,9 +593,10 @@ TEST_F(WorkerThreadPoolTest, LargeMessageData) { } TEST_F(WorkerThreadPoolTest, ManyTopics) { - auto handler = [](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - return true; - }; + auto handler = + [](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + return true; + }; // Create many topics const int num_topics = 50; @@ -596,15 +615,16 @@ TEST_F(WorkerThreadPoolTest, ManyTopics) { TEST_F(WorkerThreadPoolTest, ZeroTimestamp) { int64_t received_ts = -1; - auto handler = [&](const std::string&, int64_t ts, const uint8_t*, size_t, uint32_t) { - received_ts = ts; - return true; - }; + auto handler = + [&](const std::string&, const std::string&, int64_t ts, const uint8_t*, size_t, uint32_t) { + received_ts = ts; + return true; + }; pool_->create_topic_worker("/test", handler); pool_->start(); - MessageItem item(0, {0x01}); // Zero timestamp + MessageItem item(0, "test_type", {0x01}); // Zero timestamp pool_->try_push("/test", std::move(item)); std::this_thread::sleep_for(std::chrono::milliseconds(100)); @@ -616,15 +636,16 @@ TEST_F(WorkerThreadPoolTest, ZeroTimestamp) { TEST_F(WorkerThreadPoolTest, NegativeTimestamp) { int64_t received_ts = 0; - auto handler = [&](const std::string&, int64_t ts, const uint8_t*, size_t, uint32_t) { - received_ts = ts; - return true; - }; + auto handler = + [&](const std::string&, const std::string&, int64_t ts, const uint8_t*, size_t, uint32_t) { + received_ts = ts; + return true; + }; pool_->create_topic_worker("/test", handler); pool_->start(); - MessageItem item(-12345, {0x01}); // Negative timestamp + MessageItem item(-12345, "test_type", {0x01}); // Negative timestamp pool_->try_push("/test", std::move(item)); std::this_thread::sleep_for(std::chrono::milliseconds(100)); @@ -652,9 +673,10 @@ TEST_F(WorkerThreadPoolTest, ConfigCustomValues) { auto custom_pool = std::make_unique(config); - auto handler = [](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - return true; - }; + auto handler = + [](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + return true; + }; EXPECT_TRUE(custom_pool->create_topic_worker("/test", handler)); custom_pool->start(); @@ -668,20 +690,21 @@ TEST_F(WorkerThreadPoolTest, ConfigCustomValues) { TEST_F(WorkerThreadPoolTest, HandlerThrowsException) { std::atomic call_count{0}; - auto handler = [&](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - call_count++; - if (call_count == 2) { - throw std::runtime_error("Simulated handler error"); - } - return true; - }; + auto handler = + [&](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + call_count++; + if (call_count == 2) { + throw std::runtime_error("Simulated handler error"); + } + return true; + }; pool_->create_topic_worker("/test", handler); pool_->start(); // Push multiple messages for (int i = 0; i < 5; ++i) { - MessageItem item(1000 + i, {0x01, 0x02}); + MessageItem item(1000 + i, "test_type", {0x01, 0x02}); pool_->try_push("/test", std::move(item)); } @@ -695,16 +718,17 @@ TEST_F(WorkerThreadPoolTest, HandlerThrowsException) { TEST_F(WorkerThreadPoolTest, HandlerReturnsFalse) { std::atomic call_count{0}; - auto handler = [&](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - call_count++; - return false; // Always return failure - }; + auto handler = + [&](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + call_count++; + return false; // Always return failure + }; pool_->create_topic_worker("/test", handler); pool_->start(); for (int i = 0; i < 10; ++i) { - MessageItem item(1000 + i, {0x01}); + MessageItem item(1000 + i, "test_type", {0x01}); pool_->try_push("/test", std::move(item)); } @@ -716,14 +740,15 @@ TEST_F(WorkerThreadPoolTest, HandlerReturnsFalse) { } TEST_F(WorkerThreadPoolTest, PushWhileStopped) { - auto handler = [](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - return true; - }; + auto handler = + [](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + return true; + }; pool_->create_topic_worker("/test", handler); // Don't start the pool - MessageItem item(1000, {0x01}); + MessageItem item(1000, "test_type", {0x01}); bool result = pool_->try_push("/test", std::move(item)); // Push might succeed (queued) or fail depending on implementation @@ -734,7 +759,7 @@ TEST_F(WorkerThreadPoolTest, PushWhileStopped) { TEST_F(WorkerThreadPoolTest, PushToNonExistentTopicWhileRunning) { pool_->start(); - MessageItem item(1000, {0x01}); + MessageItem item(1000, "test_type", {0x01}); bool result = pool_->try_push("/nonexistent", std::move(item)); EXPECT_FALSE(result); @@ -745,16 +770,17 @@ TEST_F(WorkerThreadPoolTest, PushToNonExistentTopicWhileRunning) { TEST_F(WorkerThreadPoolTest, CreateTopicWhileRunning) { pool_->start(); - auto handler = [](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - return true; - }; + auto handler = + [](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + return true; + }; // Creating topic while running should work EXPECT_TRUE(pool_->create_topic_worker("/dynamic_topic", handler)); EXPECT_EQ(pool_->topic_count(), 1); // Should be able to push to the new topic - MessageItem item(1000, {0x01}); + MessageItem item(1000, "test_type", {0x01}); EXPECT_TRUE(pool_->try_push("/dynamic_topic", std::move(item))); pool_->stop(); @@ -764,20 +790,21 @@ TEST_F(WorkerThreadPoolTest, StopWhileProcessing) { std::atomic processed{0}; std::atomic in_handler{false}; - auto handler = [&](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - in_handler = true; - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - processed++; - in_handler = false; - return true; - }; + auto handler = + [&](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + in_handler = true; + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + processed++; + in_handler = false; + return true; + }; pool_->create_topic_worker("/test", handler); pool_->start(); // Push many messages for (int i = 0; i < 100; ++i) { - MessageItem item(1000 + i, {0x01}); + MessageItem item(1000 + i, "test_type", {0x01}); pool_->try_push("/test", std::move(item)); } @@ -796,18 +823,19 @@ TEST_F(WorkerThreadPoolTest, StopWhileProcessing) { TEST_F(WorkerThreadPoolTest, PauseResumeWhileProcessing) { std::atomic processed{0}; - auto handler = [&](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - processed++; - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - return true; - }; + auto handler = + [&](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + processed++; + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + return true; + }; pool_->create_topic_worker("/test", handler); pool_->start(); // Push messages for (int i = 0; i < 50; ++i) { - MessageItem item(1000 + i, {0x01}); + MessageItem item(1000 + i, "test_type", {0x01}); pool_->try_push("/test", std::move(item)); } @@ -836,10 +864,11 @@ TEST_F(WorkerThreadPoolTest, QueueOverflow) { auto small_pool = std::make_unique(config); - auto handler = [](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Slow - return true; - }; + auto handler = + [](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Slow + return true; + }; small_pool->create_topic_worker("/test", handler); small_pool->start(); @@ -847,7 +876,7 @@ TEST_F(WorkerThreadPoolTest, QueueOverflow) { // Try to push more than capacity int pushed = 0; for (int i = 0; i < 100; ++i) { - MessageItem item(1000 + i, {0x01}); + MessageItem item(1000 + i, "test_type", {0x01}); if (small_pool->try_push("/test", std::move(item))) { pushed++; } @@ -863,17 +892,18 @@ TEST_F(WorkerThreadPoolTest, QueueOverflow) { TEST_F(WorkerThreadPoolTest, StatsAccumulateCorrectly) { std::atomic processed{0}; - auto handler = [&](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - processed++; - return true; - }; + auto handler = + [&](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + processed++; + return true; + }; pool_->create_topic_worker("/test", handler); pool_->start(); // Push and process some messages for (int i = 0; i < 10; ++i) { - MessageItem item(1000 + i, {0x01}); + MessageItem item(1000 + i, "test_type", {0x01}); pool_->try_push("/test", std::move(item)); } @@ -892,9 +922,10 @@ TEST_F(WorkerThreadPoolTest, StatsAccumulateCorrectly) { } TEST_F(WorkerThreadPoolTest, EmptyTopicName) { - auto handler = [](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - return true; - }; + auto handler = + [](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + return true; + }; // Empty topic name bool result = pool_->create_topic_worker("", handler); @@ -905,9 +936,10 @@ TEST_F(WorkerThreadPoolTest, EmptyTopicName) { } TEST_F(WorkerThreadPoolTest, SpecialCharacterTopicName) { - auto handler = [](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - return true; - }; + auto handler = + [](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + return true; + }; // Topics with special characters EXPECT_TRUE(pool_->create_topic_worker("/topic/with/slashes", handler)); @@ -918,9 +950,10 @@ TEST_F(WorkerThreadPoolTest, SpecialCharacterTopicName) { } TEST_F(WorkerThreadPoolTest, ConcurrentTopicCreation) { - auto handler = [](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - return true; - }; + auto handler = + [](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + return true; + }; std::vector threads; std::atomic success_count{0}; @@ -950,17 +983,18 @@ TEST_F(WorkerThreadPoolTest, ConcurrentTopicCreation) { // ============================================================================ TEST_F(WorkerThreadPoolTest, ConcurrentStopCalls) { - auto handler = [](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - return true; - }; + auto handler = + [](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + return true; + }; pool_->create_topic_worker("/test", handler); pool_->start(); // Push some messages to keep workers busy for (int i = 0; i < 50; ++i) { - MessageItem item(1000 + i, {0x01}); + MessageItem item(1000 + i, "test_type", {0x01}); pool_->try_push("/test", std::move(item)); } @@ -980,9 +1014,10 @@ TEST_F(WorkerThreadPoolTest, ConcurrentStopCalls) { } TEST_F(WorkerThreadPoolTest, ConcurrentStartCalls) { - auto handler = [](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - return true; - }; + auto handler = + [](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + return true; + }; pool_->create_topic_worker("/test", handler); @@ -1005,10 +1040,11 @@ TEST_F(WorkerThreadPoolTest, ConcurrentStartCalls) { TEST_F(WorkerThreadPoolTest, ConcurrentPushDuringStop) { std::atomic processed{0}; - auto handler = [&](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - processed++; - return true; - }; + auto handler = + [&](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + processed++; + return true; + }; pool_->create_topic_worker("/test", handler); pool_->start(); @@ -1020,7 +1056,7 @@ TEST_F(WorkerThreadPoolTest, ConcurrentPushDuringStop) { for (int t = 0; t < 4; ++t) { push_threads.emplace_back([this, &stop_started, t]() { for (int i = 0; i < 100 && !stop_started.load(); ++i) { - MessageItem item(1000 + t * 100 + i, {0x01}); + MessageItem item(1000 + t * 100 + i, "test_type", {0x01}); pool_->try_push("/test", std::move(item)); std::this_thread::sleep_for(std::chrono::microseconds(100)); } @@ -1046,11 +1082,12 @@ TEST_F(WorkerThreadPoolTest, ConcurrentPushDuringStop) { TEST_F(WorkerThreadPoolTest, RemoveTopicWhileRunningDoesNotCrash) { std::atomic processed{0}; - auto handler = [&](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - processed++; - std::this_thread::sleep_for(std::chrono::milliseconds(5)); - return true; - }; + auto handler = + [&](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + processed++; + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + return true; + }; pool_->create_topic_worker("/topic1", handler); pool_->create_topic_worker("/topic2", handler); @@ -1061,9 +1098,9 @@ TEST_F(WorkerThreadPoolTest, RemoveTopicWhileRunningDoesNotCrash) { std::thread push_thread([this, &done]() { int i = 0; while (!done.load()) { - MessageItem item1(1000 + i, {0x01}); + MessageItem item1(1000 + i, "test_type", {0x01}); pool_->try_push("/topic1", std::move(item1)); - MessageItem item2(2000 + i, {0x02}); + MessageItem item2(2000 + i, "test_type", {0x02}); pool_->try_push("/topic2", std::move(item2)); i++; std::this_thread::sleep_for(std::chrono::microseconds(100)); @@ -1090,17 +1127,18 @@ TEST_F(WorkerThreadPoolTest, PauseResumeWithConditionVariable) { std::atomic processed{0}; std::atomic in_pause{false}; - auto handler = [&](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - processed++; - return true; - }; + auto handler = + [&](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + processed++; + return true; + }; pool_->create_topic_worker("/test", handler); pool_->start(); // Push initial messages for (int i = 0; i < 10; ++i) { - MessageItem item(1000 + i, {0x01}); + MessageItem item(1000 + i, "test_type", {0x01}); pool_->try_push("/test", std::move(item)); } @@ -1114,7 +1152,7 @@ TEST_F(WorkerThreadPoolTest, PauseResumeWithConditionVariable) { // Push more messages while paused for (int i = 0; i < 10; ++i) { - MessageItem item(2000 + i, {0x01}); + MessageItem item(2000 + i, "test_type", {0x01}); pool_->try_push("/test", std::move(item)); } @@ -1146,17 +1184,18 @@ TEST_F(WorkerThreadPoolTest, PauseResumeWithConditionVariable) { TEST_F(WorkerThreadPoolTest, StopWhilePaused) { std::atomic processed{0}; - auto handler = [&](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - processed++; - return true; - }; + auto handler = + [&](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + processed++; + return true; + }; pool_->create_topic_worker("/test", handler); pool_->start(); // Push messages for (int i = 0; i < 20; ++i) { - MessageItem item(1000 + i, {0x01}); + MessageItem item(1000 + i, "test_type", {0x01}); pool_->try_push("/test", std::move(item)); } @@ -1179,21 +1218,22 @@ TEST_F(WorkerThreadPoolTest, SharedPtrContextPreventsUseAfterFree) { std::atomic handler_running{false}; std::atomic can_exit{false}; - auto handler = [&](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - handler_running = true; - // Simulate slow processing - while (!can_exit.load()) { - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - handler_running = false; - return true; - }; + auto handler = + [&](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + handler_running = true; + // Simulate slow processing + while (!can_exit.load()) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + handler_running = false; + return true; + }; pool_->create_topic_worker("/test", handler); pool_->start(); // Push a message that will be processed slowly - MessageItem item(1000, {0x01}); + MessageItem item(1000, "test_type", {0x01}); pool_->try_push("/test", std::move(item)); // Wait for handler to start @@ -1222,10 +1262,11 @@ TEST_F(WorkerThreadPoolTest, SharedPtrContextPreventsUseAfterFree) { TEST_F(WorkerThreadPoolTest, YieldInsteadOfBusySpin) { std::atomic processed{0}; - auto handler = [&](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - processed++; - return true; - }; + auto handler = + [&](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + processed++; + return true; + }; WorkerThreadPool::Config config; config.queue_capacity_per_topic = 1024; @@ -1240,7 +1281,7 @@ TEST_F(WorkerThreadPoolTest, YieldInsteadOfBusySpin) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Push a message - MessageItem item(1000, {0x01}); + MessageItem item(1000, "test_type", {0x01}); test_pool->try_push("/test", std::move(item)); // Wait for processing @@ -1252,9 +1293,10 @@ TEST_F(WorkerThreadPoolTest, YieldInsteadOfBusySpin) { } TEST_F(WorkerThreadPoolTest, SharedLockForConcurrentReads) { - auto handler = [](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { - return true; - }; + auto handler = + [](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) { + return true; + }; // Create multiple topics for (int i = 0; i < 10; ++i) { @@ -1277,7 +1319,7 @@ TEST_F(WorkerThreadPoolTest, SharedLockForConcurrentReads) { read_count++; // Push operations - MessageItem item(1000 + t * 100 + i, {0x01}); + MessageItem item(1000 + t * 100 + i, "test_type", {0x01}); if (pool_->try_push("/topic" + std::to_string(t % 10), std::move(item))) { push_count++; } @@ -1305,7 +1347,8 @@ TEST_F(WorkerThreadPoolTest, SharedLockForConcurrentReads) { TEST_F(WorkerThreadPoolTest, StdExceptionDuringDrain) { // Handler that always throws std::exception // Covers: catch (const std::exception& e) branch in drain loop - auto handler = [](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) -> bool { + auto handler = + [](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) -> bool { throw std::runtime_error("Simulated drain exception"); }; @@ -1313,7 +1356,7 @@ TEST_F(WorkerThreadPoolTest, StdExceptionDuringDrain) { // Queue messages without starting - they will be processed during drain for (int i = 0; i < 5; ++i) { - MessageItem item(1000 + i, {0x01}); + MessageItem item(1000 + i, "test_type", {0x01}); pool_->try_push("/test", std::move(item)); } @@ -1329,7 +1372,8 @@ TEST_F(WorkerThreadPoolTest, StdExceptionDuringDrain) { TEST_F(WorkerThreadPoolTest, UnknownExceptionDuringDrain) { // Handler that throws a non-std::exception (e.g., int) // Covers: catch (...) branch in drain loop - auto handler = [](const std::string&, int64_t, const uint8_t*, size_t, uint32_t) -> bool { + auto handler = + [](const std::string&, const std::string&, int64_t, const uint8_t*, size_t, uint32_t) -> bool { throw 42; // Non-std::exception type }; @@ -1337,7 +1381,7 @@ TEST_F(WorkerThreadPoolTest, UnknownExceptionDuringDrain) { // Queue messages without starting for (int i = 0; i < 5; ++i) { - MessageItem item(1000 + i, {0x01}); + MessageItem item(1000 + i, "test_type", {0x01}); pool_->try_push("/test", std::move(item)); } diff --git a/apps/axon_recorder/worker_thread_pool.cpp b/apps/axon_recorder/worker_thread_pool.cpp index 1678c6e..ce47736 100644 --- a/apps/axon_recorder/worker_thread_pool.cpp +++ b/apps/axon_recorder/worker_thread_pool.cpp @@ -283,7 +283,12 @@ void WorkerThreadPool::worker_thread_func(std::shared_ptr context) bool success = false; try { success = context->handler( - topic, item.timestamp_ns, item.raw_data.data(), item.raw_data.size(), seq + topic, + item.message_type, + item.timestamp_ns, + item.raw_data.data(), + item.raw_data.size(), + seq ); } catch (const std::exception& e) { AXON_LOG_ERROR( @@ -324,7 +329,12 @@ void WorkerThreadPool::worker_thread_func(std::shared_ptr context) uint32_t seq = context->stats.sequence.fetch_add(1, std::memory_order_relaxed); try { if (context->handler( - topic, item.timestamp_ns, item.raw_data.data(), item.raw_data.size(), seq + topic, + item.message_type, + item.timestamp_ns, + item.raw_data.data(), + item.raw_data.size(), + seq )) { context->stats.written.fetch_add(1, std::memory_order_relaxed); } diff --git a/apps/axon_recorder/worker_thread_pool.hpp b/apps/axon_recorder/worker_thread_pool.hpp index 00d246b..d48252f 100644 --- a/apps/axon_recorder/worker_thread_pool.hpp +++ b/apps/axon_recorder/worker_thread_pool.hpp @@ -28,12 +28,14 @@ namespace recorder { */ struct MessageItem { int64_t timestamp_ns = 0; + std::string message_type; // ROS2 message type (e.g., "sensor_msgs::msg::Image") std::vector raw_data; // Owned raw message bytes MessageItem() = default; - MessageItem(int64_t ts, std::vector&& data) + MessageItem(int64_t ts, const std::string& type, std::vector&& data) : timestamp_ns(ts) + , message_type(type) , raw_data(std::move(data)) {} // Move-only for zero-copy @@ -110,8 +112,8 @@ class WorkerThreadPool { * Returns: true if message was processed successfully */ using MessageHandler = std::function; /** diff --git a/middlewares/filters/CMakeLists.txt b/middlewares/filters/CMakeLists.txt index 766ca1f..c301f45 100644 --- a/middlewares/filters/CMakeLists.txt +++ b/middlewares/filters/CMakeLists.txt @@ -53,10 +53,10 @@ endif() target_include_directories(axon_filters PUBLIC $ + $ $ PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src - ${DEPTHLITEZ_DIR}/include ) target_link_libraries(axon_filters @@ -65,6 +65,11 @@ target_link_libraries(axon_filters ${ZSTD_LIBRARIES} ) +# Ensure PIC is used for static library (required when linking into shared library) +set_target_properties(axon_filters PROPERTIES + POSITION_INDEPENDENT_CODE ON +) + target_compile_options(axon_filters PRIVATE -Wall -Wextra -Wpedantic diff --git a/middlewares/ros2/CMakeLists.txt b/middlewares/ros2/CMakeLists.txt index 8cfb72d..c46f66f 100644 --- a/middlewares/ros2/CMakeLists.txt +++ b/middlewares/ros2/CMakeLists.txt @@ -76,6 +76,7 @@ add_library(axon_ros2_plugin SHARED target_include_directories(axon_ros2_plugin PUBLIC $ $ + ${ROS2_INCLUDE_DIR} ) # Link ROS2 libraries @@ -84,6 +85,13 @@ target_link_libraries(axon_ros2_plugin PUBLIC nlohmann_json::nlohmann_json ) +# Use ament_target_dependencies for ROS2 dependencies (handles all includes and linking) +ament_target_dependencies(axon_ros2_plugin PUBLIC + rclcpp + sensor_msgs + std_msgs +) + # Link filters library for depth compression if(TARGET axon::filters) target_link_libraries(axon_ros2_plugin PRIVATE axon::filters) @@ -92,14 +100,6 @@ if(TARGET axon::filters) ) endif() -# For ROS2 type support macros - we need to link against message libraries -if(TARGET std_msgs__rosidl_typesupport_cpp) - target_link_libraries(axon_ros2_plugin PRIVATE std_msgs__rosidl_typesupport_cpp) -endif() -if(TARGET sensor_msgs__rosidl_typesupport_cpp) - target_link_libraries(axon_ros2_plugin PRIVATE sensor_msgs__rosidl_typesupport_cpp) -endif() - # Export all symbols for dynamic loading target_compile_options(axon_ros2_plugin PRIVATE -fvisibility=default) @@ -128,6 +128,21 @@ if(AXON_BUILD_TESTS) GTest::GTest ) + # Use ament_target_dependencies for ROS2 dependencies in tests + ament_target_dependencies(${test_name} + rclcpp + sensor_msgs + std_msgs + ) + + # Add filters include directory for tests + if(TARGET axon::filters) + target_include_directories(${test_name} PRIVATE + ${AXON_MIDDLEWARES_DIR}/filters/include + ${AXON_MIDDLEWARES_DIR}/filters/depthlitez/include + ) + endif() + # Add test labels set_target_properties(${test_name} PROPERTIES LABELS "unit;ros2_plugin" diff --git a/middlewares/ros2/include/depth_compression_filter.hpp b/middlewares/ros2/include/depth_compression_filter.hpp index 0fe95d4..e414d83 100644 --- a/middlewares/ros2/include/depth_compression_filter.hpp +++ b/middlewares/ros2/include/depth_compression_filter.hpp @@ -6,7 +6,6 @@ #define ROS2_DEPTH_COMPRESSION_FILTER_HPP #include - #include #include #include @@ -33,9 +32,7 @@ class DepthCompressionFilter { * @brief 处理后的消息回调类型 */ using ProcessedCallback = std::function& data, + const std::string& topic, const std::string& message_type, const std::vector& data, uint64_t timestamp_ns )>; @@ -55,11 +52,8 @@ class DepthCompressionFilter { * @param callback 处理后的消息回调 */ void filter_and_process( - const std::string& topic, - const std::string& message_type, - const std::vector& data, - uint64_t timestamp_ns, - ProcessedCallback callback + const std::string& topic, const std::string& message_type, const std::vector& data, + uint64_t timestamp_ns, ProcessedCallback callback ); private: @@ -78,9 +72,7 @@ class DepthCompressionFilter { * @brief 构建 CompressedImage 消息 */ std::vector build_compressed_image_msg( - const std::string& format, - const std::vector& compressed_data, - uint64_t timestamp_ns + const std::string& format, const std::vector& compressed_data, uint64_t timestamp_ns ); }; diff --git a/middlewares/ros2/src/depth_compression_filter.cpp b/middlewares/ros2/src/depth_compression_filter.cpp index f77ec8a..ec4852f 100644 --- a/middlewares/ros2/src/depth_compression_filter.cpp +++ b/middlewares/ros2/src/depth_compression_filter.cpp @@ -4,8 +4,10 @@ #include "depth_compression_filter.hpp" -#include +#include +#include #include +#include #include @@ -13,7 +15,7 @@ namespace ros2_plugin { DepthCompressionFilter::DepthCompressionFilter(const DepthCompressionConfig& config) : config_(config) { - // 配置压缩器 + // Configure compressor axon::depth::DepthCompressor::Config compressor_config; if (config.level == "fast") { compressor_config.level = dlz::CompressionLevel::kFast; @@ -26,53 +28,50 @@ DepthCompressionFilter::DepthCompressionFilter(const DepthCompressionConfig& con } void DepthCompressionFilter::filter_and_process( - const std::string& topic, - const std::string& message_type, - const std::vector& data, - uint64_t timestamp_ns, - ProcessedCallback callback + const std::string& topic, const std::string& message_type, const std::vector& data, + uint64_t timestamp_ns, ProcessedCallback callback ) { - // 只处理 sensor_msgs::msg::Image - if (message_type != "sensor_msgs::msg::Image" && - message_type != "sensor_msgs/msg/Image") { - // 不是图像消息，直接传递 + // Only process sensor_msgs::msg::Image + if (message_type != "sensor_msgs::msg::Image" && message_type != "sensor_msgs/msg/Image") { + // Not an image message, pass through directly callback(topic, message_type, data, timestamp_ns); return; } - // 尝试提取并压缩深度图像 + // Try to extract and compress depth image try { auto [depth_data, width, height] = extract_depth_data(data); if (depth_data != nullptr && width > 0 && height > 0) { - // 压缩深度图像 + // Compress depth image std::vector compressed_data; if (compressor_.compress(depth_data, width, height, compressed_data)) { - // 构建压缩图像消息 + // Build compressed image message std::string format = compressor_.get_compression_format(); std::vector compressed_msg = build_compressed_image_msg(format, compressed_data, timestamp_ns); - // 回调处理后的消息 - callback(topic, "sensor_msgs::msg::CompressedImage", compressed_msg, timestamp_ns); + // Callback with processed message + callback(topic, "sensor_msgs/msg/CompressedImage", compressed_msg, timestamp_ns); return; } } } catch (const std::exception& e) { - // 压缩失败，回退到原始消息 + // Compression failed, fall back to original message } - // 压缩失败或不是深度图像，传递原始消息 + // Compression failed or not a depth image, pass through original message callback(topic, message_type, data, timestamp_ns); } -std::tuple -DepthCompressionFilter::extract_depth_data(const std::vector& image_msg) { +std::tuple DepthCompressionFilter::extract_depth_data( + const std::vector& image_msg +) { const uint8_t* data = nullptr; size_t width = 0; size_t height = 0; - // 使用 ROS2 反序列化 + // Use ROS2 deserialization try { rclcpp::Serialization serialization; sensor_msgs::msg::Image image; @@ -83,44 +82,42 @@ DepthCompressionFilter::extract_depth_data(const std::vector& image_msg ); serialized_msg.get_rcl_serialized_message().buffer_length = image_msg.size(); - // 反序列化 - serialization.deserialize_message(serialized_msg, &image); + // Deserialize + serialization.deserialize_message(&serialized_msg, &image); - // 检查是否为 16UC1 深度图像 + // Check if it's a 16UC1 depth image if (image.encoding == "16UC1") { data = image.data.data(); width = image.width; height = image.height; } } catch (const std::exception& e) { - // 反序列化失败 + // Deserialization failed } return {data, width, height}; } std::vector DepthCompressionFilter::build_compressed_image_msg( - const std::string& format, - const std::vector& compressed_data, - uint64_t timestamp_ns + const std::string& format, const std::vector& compressed_data, uint64_t timestamp_ns ) { - // 使用 ROS2 序列化 CompressedImage + // Use ROS2 serialization for CompressedImage sensor_msgs::msg::CompressedImage compressed_msg; - // 设置头部 + // Set header compressed_msg.header.stamp.sec = static_cast(timestamp_ns / 1000000000ULL); compressed_msg.header.stamp.nanosec = static_cast(timestamp_ns % 1000000000ULL); compressed_msg.format = format; - // 设置压缩数据 + // Set compressed data compressed_msg.data = compressed_data; - // 序列化 + // Serialize rclcpp::Serialization serialization; rclcpp::SerializedMessage serialized_msg; - serialization.serialize_message(compressed_msg, &serialized_msg); + serialization.serialize_message(&compressed_msg, &serialized_msg); - // 复制到输出向量 + // Copy to output vector std::vector result( serialized_msg.get_rcl_serialized_message().buffer, serialized_msg.get_rcl_serialized_message().buffer + diff --git a/middlewares/ros2/src/ros2_plugin_export.cpp b/middlewares/ros2/src/ros2_plugin_export.cpp index 5a243f6..b40b7a8 100644 --- a/middlewares/ros2/src/ros2_plugin_export.cpp +++ b/middlewares/ros2/src/ros2_plugin_export.cpp @@ -20,10 +20,9 @@ #include #include -// Plugin implementation headers +#include "depth_compressor.hpp" #include "ros2_plugin.hpp" #include "ros2_subscription_wrapper.hpp" -#include // For DepthCompressionConfig using namespace ros2_plugin; @@ -128,6 +127,12 @@ static int32_t axon_subscribe( AxonMessageCallback callback, void* user_data ) { if (!topic_name || !message_type || !callback) { + RCUTILS_LOG_ERROR( + "Invalid argument: topic=%p, type=%p, callback=%p", + (void*)topic_name, + (void*)message_type, + (void*)callback + ); return static_cast(AXON_ERROR_INVALID_ARGUMENT); } @@ -228,7 +233,12 @@ struct AxonPluginDescriptor { // Static vtable static AxonPluginVtable ros2_vtable = { - axon_init, axon_start, axon_stop, axon_subscribe, axon_publish, {nullptr} + axon_init, + axon_start, + axon_stop, + axon_subscribe, + axon_publish, + {nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr} }; // Exported plugin descriptor diff --git a/middlewares/ros2/src/ros2_subscription_wrapper.cpp b/middlewares/ros2/src/ros2_subscription_wrapper.cpp index c8fe7f2..d542bab 100644 --- a/middlewares/ros2/src/ros2_subscription_wrapper.cpp +++ b/middlewares/ros2/src/ros2_subscription_wrapper.cpp @@ -35,7 +35,7 @@ bool SubscriptionManager::subscribe( } try { - // 创建压缩过滤器（如果配置了） + // Create compression filter (if configured) std::unique_ptr compression_filter; if (options.depth_compression.has_value()) { compression_filter = std::make_unique(*options.depth_compression); @@ -53,9 +53,7 @@ bool SubscriptionManager::subscribe( topic_name, message_type, options.qos, - [this, topic_name, message_type, callback]( - std::shared_ptr msg - ) { + [this, topic_name, message_type, callback](std::shared_ptr msg) { if (!callback) { return; } @@ -71,9 +69,10 @@ bool SubscriptionManager::subscribe( // Get current time as timestamp rclcpp::Time timestamp = rclcpp::Clock(RCL_SYSTEM_TIME).now(); - // 查找压缩过滤器（如果存在） + // Look up compression filter (if exists) auto it = subscriptions_.find(topic_name); - bool has_filter = (it != subscriptions_.end() && it->second.compression_filter != nullptr); + bool has_filter = + (it != subscriptions_.end() && it->second.compression_filter != nullptr); if (has_filter) { it->second.compression_filter->filter_and_process( @@ -87,17 +86,13 @@ bool SubscriptionManager::subscribe( const std::vector& filtered_data, uint64_t filtered_timestamp_ns ) { - RCUTILS_LOG_DEBUG( - "Processed message on topic %s with type %s, size %zu bytes", - filtered_topic.c_str(), filtered_type.c_str(), filtered_data.size() - ); - // 将纳秒时间戳转换回 rclcpp::Time + // Convert nanosecond timestamp back to rclcpp::Time rclcpp::Time filtered_timestamp(filtered_timestamp_ns); callback(filtered_topic, filtered_type, filtered_data, filtered_timestamp); } ); } else { - // 直接调用回调 + // Direct callback callback(topic_name, message_type, data, timestamp); } diff --git a/middlewares/ros2/test/test_ros2_plugin.cpp b/middlewares/ros2/test/test_ros2_plugin.cpp index fb0444c..6a45527 100644 --- a/middlewares/ros2/test/test_ros2_plugin.cpp +++ b/middlewares/ros2/test/test_ros2_plugin.cpp @@ -483,8 +483,9 @@ TEST_F(SubscriptionManagerTest, SubscribeToTopic) { callback_count++; }; - rclcpp::QoS qos(10); - bool result = manager_->subscribe("/test_topic", "std_msgs/msg/String", qos, callback); + ros2_plugin::SubscribeOptions options; + options.qos = rclcpp::QoS(10); + bool result = manager_->subscribe("/test_topic", "std_msgs/msg/String", options, callback); // Subscribe should succeed (creates subscription even if no publisher) EXPECT_TRUE(result); @@ -501,8 +502,9 @@ TEST_F(SubscriptionManagerTest, UnsubscribeFromTopic) { auto callback = [](const std::string&, const std::string&, const std::vector&, rclcpp::Time) {}; - rclcpp::QoS qos(10); - ASSERT_TRUE(manager_->subscribe("/test_topic", "std_msgs/msg/String", qos, callback)); + ros2_plugin::SubscribeOptions options; + options.qos = rclcpp::QoS(10); + ASSERT_TRUE(manager_->subscribe("/test_topic", "std_msgs/msg/String", options, callback)); // Unsubscribe should succeed EXPECT_TRUE(manager_->unsubscribe("/test_topic")); @@ -522,11 +524,14 @@ TEST_F(SubscriptionManagerTest, DoubleSubscribeSameTopic) { auto callback = [](const std::string&, const std::string&, const std::vector&, rclcpp::Time) {}; - rclcpp::QoS qos(10); - ASSERT_TRUE(manager_->subscribe("/test_topic", "std_msgs/msg/String", qos, callback)); + ros2_plugin::SubscribeOptions options; + options.qos = rclcpp::QoS(10); + ASSERT_TRUE(manager_->subscribe("/test_topic", "std_msgs/msg/String", options, callback)); // Second subscribe should return true (already subscribed, but not an error) - bool result = manager_->subscribe("/test_topic", "std_msgs/msg/String", qos, callback); + ros2_plugin::SubscribeOptions options2; + options2.qos = rclcpp::QoS(10); + bool result = manager_->subscribe("/test_topic", "std_msgs/msg/String", options2, callback); EXPECT_TRUE(result); // Should still only have one subscription @@ -543,9 +548,13 @@ TEST_F(SubscriptionManagerTest, SubscribeToMultipleTopics) { rclcpp::QoS qos(10); - EXPECT_TRUE(manager_->subscribe("/topic1", "std_msgs/msg/String", qos, callback)); - EXPECT_TRUE(manager_->subscribe("/topic2", "std_msgs/msg/Int32", qos, callback)); - EXPECT_TRUE(manager_->subscribe("/topic3", "sensor_msgs/msg/Image", qos, callback)); + ros2_plugin::SubscribeOptions options; + options.qos = rclcpp::QoS(10); + EXPECT_TRUE(manager_->subscribe("/topic1", "std_msgs/msg/String", options, callback)); + options.qos = rclcpp::QoS(10); + EXPECT_TRUE(manager_->subscribe("/topic2", "std_msgs/msg/Int32", options, callback)); + options.qos = rclcpp::QoS(10); + EXPECT_TRUE(manager_->subscribe("/topic3", "sensor_msgs/msg/Image", options, callback)); auto topics = manager_->get_subscribed_topics(); EXPECT_EQ(topics.size(), 3); @@ -558,10 +567,12 @@ TEST_F(SubscriptionManagerTest, GetSubscribedTopicsReturnsCorrectTopics) { auto callback = [](const std::string&, const std::string&, const std::vector&, rclcpp::Time) {}; - rclcpp::QoS qos(10); + ros2_plugin::SubscribeOptions options; + options.qos = rclcpp::QoS(10); - manager_->subscribe("/topic1", "std_msgs/msg/String", qos, callback); - manager_->subscribe("/topic2", "std_msgs/msg/Int32", qos, callback); + manager_->subscribe("/topic1", "std_msgs/msg/String", options, callback); + options.qos = rclcpp::QoS(10); + manager_->subscribe("/topic2", "std_msgs/msg/Int32", options, callback); auto topics = manager_->get_subscribed_topics(); EXPECT_EQ(topics.size(), 2); @@ -589,8 +600,9 @@ TEST_F(SubscriptionManagerTest, CallbackIsStored) { callback_count++; }; - rclcpp::QoS qos(10); - bool result = manager_->subscribe("/test_topic", "std_msgs/msg/String", qos, callback); + ros2_plugin::SubscribeOptions options; + options.qos = rclcpp::QoS(10); + bool result = manager_->subscribe("/test_topic", "std_msgs/msg/String", options, callback); EXPECT_TRUE(result); // We can't verify callback is actually called without publishing, @@ -604,8 +616,9 @@ TEST_F(SubscriptionManagerTest, DestructorCleansUpSubscriptions) { auto callback = [](const std::string&, const std::string&, const std::vector&, rclcpp::Time) {}; - rclcpp::QoS qos(10); - ASSERT_TRUE(manager_->subscribe("/test_topic", "std_msgs/msg/String", qos, callback)); + ros2_plugin::SubscribeOptions options; + options.qos = rclcpp::QoS(10); + ASSERT_TRUE(manager_->subscribe("/test_topic", "std_msgs/msg/String", options, callback)); // Destroy manager manager_.reset(); From acd85e7c837f75d8446a4def3665d22ee35c3bc6 Mon Sep 17 00:00:00 2001 From: Liangwei XU Date: Mon, 2 Feb 2026 19:45:57 +0800 Subject: [PATCH 07/21] feat(ros1): add depth compression filter for ROS1 plugin --- middlewares/ros1/CMakeLists.txt | 27 ++++ .../ros1/include/depth_compression_filter.hpp | 81 ++++++++++++ .../include/ros1_subscription_wrapper.hpp | 12 ++ .../ros1/src/depth_compression_filter.cpp | 115 ++++++++++++++++++ .../ros1/src/ros1_subscription_wrapper.cpp | 87 +++++++++++++ 5 files changed, 322 insertions(+) create mode 100644 middlewares/ros1/include/depth_compression_filter.hpp create mode 100644 middlewares/ros1/src/depth_compression_filter.cpp diff --git a/middlewares/ros1/CMakeLists.txt b/middlewares/ros1/CMakeLists.txt index 86a4d5e..eb7d7b7 100644 --- a/middlewares/ros1/CMakeLists.txt +++ b/middlewares/ros1/CMakeLists.txt @@ -55,6 +55,7 @@ add_library(axon_ros1_plugin SHARED src/ros1_plugin.cpp src/ros1_subscription_wrapper.cpp src/ros1_plugin_export.cpp + src/depth_compression_filter.cpp ) target_include_directories(axon_ros1_plugin PUBLIC @@ -63,6 +64,19 @@ target_include_directories(axon_ros1_plugin PUBLIC ${ROS1_INCLUDE_DIR} ) +# Add filters include directory for depth compression +target_include_directories(axon_ros1_plugin PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/../filters/include + ${CMAKE_CURRENT_SOURCE_DIR}/../filters/depthlitez/include +) + +# Find and link LZ4 and ZSTD for depth compression +find_package(PkgConfig QUIET) +if(PKG_CONFIG_FOUND) + pkg_check_modules(LZ4 liblz4 IMPORTED_TARGET) + pkg_check_modules(ZSTD libzstd IMPORTED_TARGET) +endif() + # Link ROS1 libraries target_link_libraries(axon_ros1_plugin PUBLIC ${ROSCPP_LIB} @@ -72,6 +86,19 @@ target_link_libraries(axon_ros1_plugin PUBLIC nlohmann_json::nlohmann_json ) +# Link filters library for depth compression +target_link_libraries(axon_ros1_plugin PRIVATE + axon_filters +) + +# Link LZ4 and ZSTD if found +if(TARGET PkgConfig::LZ4) + target_link_libraries(axon_ros1_plugin PRIVATE PkgConfig::LZ4) +endif() +if(TARGET PkgConfig::ZSTD) + target_link_libraries(axon_ros1_plugin PRIVATE PkgConfig::ZSTD) +endif() + # Export all symbols for dynamic loading target_compile_options(axon_ros1_plugin PRIVATE -fvisibility=default) diff --git a/middlewares/ros1/include/depth_compression_filter.hpp b/middlewares/ros1/include/depth_compression_filter.hpp new file mode 100644 index 0000000..35aa792 --- /dev/null +++ b/middlewares/ros1/include/depth_compression_filter.hpp @@ -0,0 +1,81 @@ +// SPDX-FileCopyrightText: 2026 ArcheBase +// +// SPDX-License-Identifier: MulanPSL-2.0 + +#ifndef ROS1_DEPTH_COMPRESSION_FILTER_HPP +#define ROS1_DEPTH_COMPRESSION_FILTER_HPP + +#include +#include +#include +#include + +namespace ros1_plugin { + +/** + * @brief 深度压缩配置 + */ +struct DepthCompressionConfig { + bool enabled = false; + std::string level = "medium"; // fast, medium, max +}; + +/** + * @brief 深度图像压缩过滤器 + * + * 在 ROS1 插件中使用，将 sensor_msgs/Image (16UC1) 压缩为 + * sensor_msgs/CompressedImage 格式。 + */ +class DepthCompressionFilter { +public: + /** + * @brief 处理后的消息回调类型 + */ + using ProcessedCallback = std::function& data, + uint64_t timestamp + )>; + + /** + * @brief 构造函数 + * @param config 深度压缩配置 + */ + explicit DepthCompressionFilter(const DepthCompressionConfig& config); + + /** + * @brief 过滤并处理消息 + * + * @param topic 主题名称 + * @param message_type 消息类型 + * @param data 原始消息数据（ROS 序列化的 sensor_msgs/Image） + * @param timestamp 时间戳（纳秒） + * @param callback 处理后的消息回调 + */ + void filter_and_process( + const std::string& topic, const std::string& message_type, const std::vector& data, + uint64_t timestamp, ProcessedCallback callback + ); + +private: + DepthCompressionConfig config_; + axon::depth::DepthCompressor compressor_; + + /** + * @brief 从 ROS Image 消息中提取深度数据 + * @return 提取的深度数据指针，宽度，高度 + */ + std::tuple extract_depth_data( + const std::vector& image_msg + ); + + /** + * @brief 构建 CompressedImage 消息 + */ + std::vector build_compressed_image_msg( + const std::string& format, const std::vector& compressed_data, uint64_t timestamp + ); +}; + +} // namespace ros1_plugin + +#endif // ROS1_DEPTH_COMPRESSION_FILTER_HPP diff --git a/middlewares/ros1/include/ros1_subscription_wrapper.hpp b/middlewares/ros1/include/ros1_subscription_wrapper.hpp index f7a705a..7c55d9f 100644 --- a/middlewares/ros1/include/ros1_subscription_wrapper.hpp +++ b/middlewares/ros1/include/ros1_subscription_wrapper.hpp @@ -10,12 +10,17 @@ #include #include #include +#include #include #include #include namespace ros1_plugin { +// Forward declarations +struct DepthCompressionConfig; +class DepthCompressionFilter; + // Message callback type - passes raw serialized message data using MessageCallback = std::function& depth_compression + ); + // Unsubscribe from a topic bool unsubscribe(const std::string& topic_name); @@ -45,6 +56,7 @@ class SubscriptionManager { ros::Subscriber subscriber; MessageCallback callback; std::string message_type; + std::shared_ptr depth_filter; }; std::unordered_map subscriptions_; mutable std::mutex mutex_; diff --git a/middlewares/ros1/src/depth_compression_filter.cpp b/middlewares/ros1/src/depth_compression_filter.cpp new file mode 100644 index 0000000..477d351 --- /dev/null +++ b/middlewares/ros1/src/depth_compression_filter.cpp @@ -0,0 +1,115 @@ +// SPDX-FileCopyrightText: 2026 ArcheBase +// +// SPDX-License-Identifier: MulanPSL-2.0 + +#include "depth_compression_filter.hpp" + +#include +#include +#include + +#include + +namespace ros1_plugin { + +DepthCompressionFilter::DepthCompressionFilter(const DepthCompressionConfig& config) + : config_(config) { + // Configure compressor + axon::depth::DepthCompressor::Config compressor_config; + if (config.level == "fast") { + compressor_config.level = dlz::CompressionLevel::kFast; + } else if (config.level == "max") { + compressor_config.level = dlz::CompressionLevel::kMax; + } else { + compressor_config.level = dlz::CompressionLevel::kMedium; + } + compressor_.set_config(compressor_config); +} + +void DepthCompressionFilter::filter_and_process( + const std::string& topic, const std::string& message_type, const std::vector& data, + uint64_t timestamp, ProcessedCallback callback +) { + // Only process sensor_msgs/Image + if (message_type != "sensor_msgs/Image") { + // Not an image message, pass through directly + callback(topic, message_type, data, timestamp); + return; + } + + // Try to extract and compress depth image + try { + auto [depth_data, width, height] = extract_depth_data(data); + + if (depth_data != nullptr && width > 0 && height > 0) { + // Compress depth image + std::vector compressed_data; + if (compressor_.compress(depth_data, width, height, compressed_data)) { + // Build compressed image message + std::string format = compressor_.get_compression_format(); + std::vector compressed_msg = + build_compressed_image_msg(format, compressed_data, timestamp); + + // Callback with processed message + callback(topic, "sensor_msgs/CompressedImage", compressed_msg, timestamp); + return; + } + } + } catch (const std::exception& e) { + // Compression failed, fall back to original message + } + + // Compression failed or not a depth image, pass through original message + callback(topic, message_type, data, timestamp); +} + +std::tuple DepthCompressionFilter::extract_depth_data( + const std::vector& image_msg +) { + const uint8_t* data = nullptr; + size_t width = 0; + size_t height = 0; + + // Use ROS1 deserialization + try { + sensor_msgs::Image image; + ros::serialization::IStream stream(const_cast(image_msg.data()), image_msg.size()); + ros::serialization::deserialize(stream, image); + + // Check if it's a 16UC1 depth image + if (image.encoding == "16UC1") { + data = image.data.data(); + width = image.width; + height = image.height; + } + } catch (const std::exception& e) { + // Deserialization failed + } + + return {data, width, height}; +} + +std::vector DepthCompressionFilter::build_compressed_image_msg( + const std::string& format, const std::vector& compressed_data, uint64_t timestamp +) { + // Use ROS1 serialization for CompressedImage + sensor_msgs::CompressedImage compressed_msg; + + // Set header + compressed_msg.header.stamp.fromNSec(timestamp); + compressed_msg.format = format; + + // Set compressed data + compressed_msg.data = compressed_data; + + // Serialize + uint32_t serial_size = ros::serialization::serializationLength(compressed_msg); + std::vector result(serial_size); + + ros::serialization::OStream stream(result.data(), serial_size); + ros::serialization::serialize(stream, compressed_msg); + + return result; +} + +} // namespace ros1_plugin diff --git a/middlewares/ros1/src/ros1_subscription_wrapper.cpp b/middlewares/ros1/src/ros1_subscription_wrapper.cpp index b8cac4e..dad522c 100644 --- a/middlewares/ros1/src/ros1_subscription_wrapper.cpp +++ b/middlewares/ros1/src/ros1_subscription_wrapper.cpp @@ -8,6 +8,8 @@ #include #include +#include "depth_compression_filter.hpp" + namespace ros1_plugin { // ============================================================================= @@ -92,6 +94,91 @@ bool SubscriptionManager::subscribe( } } +bool SubscriptionManager::subscribe( + const std::string& topic_name, const std::string& message_type, uint32_t queue_size, + MessageCallback callback, const std::optional& depth_compression +) { + std::lock_guard lock(mutex_); + + // Check if already subscribed + if (subscriptions_.find(topic_name) != subscriptions_.end()) { + ROS_WARN("Already subscribed to topic: %s", topic_name.c_str()); + return true; + } + + // Create depth compression filter if enabled + std::shared_ptr depth_filter; + if (depth_compression && depth_compression->enabled) { + depth_filter = std::make_shared(*depth_compression); + ROS_INFO( + "Depth compression enabled for topic: %s (level: %s)", + topic_name.c_str(), + depth_compression->level.c_str() + ); + } + + try { + // Use topic_tools::ShapeShifter to subscribe to any message type + auto subscriber = node_handle_->subscribe( + topic_name, + queue_size, + [topic_name, message_type, callback, depth_filter]( + const typename topic_tools::ShapeShifter::ConstPtr& msg + ) { + if (!callback) { + return; + } + + try { + // Get the message definition (type) from the ShapeShifter + std::string actual_type = msg->getDataType(); + + // Serialize the message to a byte array + uint32_t serial_size = ros::serialization::serializationLength(*msg); + std::vector data(serial_size); + + ros::serialization::OStream stream(data.data(), serial_size); + ros::serialization::serialize(stream, *msg); + + // Get current time as timestamp (nanoseconds) + uint64_t timestamp = static_cast(ros::Time::now().toNSec()); + + // Apply depth compression filter if available + if (depth_filter) { + depth_filter->filter_and_process(topic_name, actual_type, data, timestamp, callback); + } else { + // Invoke callback directly with serialized data + callback(topic_name, actual_type, data, timestamp); + } + + } catch (const std::exception& e) { + ROS_ERROR("Failed to handle message on topic %s: %s", topic_name.c_str(), e.what()); + } + } + ); + + if (!subscriber) { + ROS_ERROR("Failed to create subscription for: %s", topic_name.c_str()); + return false; + } + + // Store subscription info with callback + SubscriptionInfo info; + info.subscriber = subscriber; + info.callback = callback; + info.message_type = message_type; + subscriptions_[topic_name] = std::move(info); + + ROS_INFO("Subscribed to topic: %s (%s)", topic_name.c_str(), message_type.c_str()); + + return true; + + } catch (const std::exception& e) { + ROS_ERROR("Exception subscribing to %s: %s", topic_name.c_str(), e.what()); + return false; + } +} + bool SubscriptionManager::unsubscribe(const std::string& topic_name) { std::lock_guard lock(mutex_); From 798e7e0b7545e35f6f828d2e04a907dcbe03691d Mon Sep 17 00:00:00 2001 From: Liangwei XU Date: Mon, 2 Feb 2026 19:47:09 +0800 Subject: [PATCH 08/21] docs(changelog): add version 0.3.0 release notes --- CHANGELOG | 25 +- CLAUDE.md | 25 +- CMakeLists.txt | 2 +- README.md | 2 +- apps/axon_recorder/CMakeLists.txt | 2 +- docs/designs/depth-compression-filter.md | 513 +++++++++++++++++++++++ 6 files changed, 560 insertions(+), 9 deletions(-) create mode 100644 docs/designs/depth-compression-filter.md diff --git a/CHANGELOG b/CHANGELOG index 76ffe05..ff8d98c 100644 --- a/CHANGELOG +++ b/CHANGELOG @@ -12,6 +12,28 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ## [Unreleased] +## [0.3.0] - 2025-02-02 + +### Added +- **filters**: Implement depth image compression filter for ROS1 plugin + - Add `DepthCompressionFilter` class for ROS1 Noetic + - Support `sensor_msgs/Image` to `sensor_msgs/CompressedImage` transformation + - Integrate DepthLiteZ compression library with 5-10x compression ratio + - Support configurable compression levels: fast, medium, max + - Pass-through for non-depth images or compression failures + +- **filters**: Add depth compression support to ROS2 plugin + - Extend `DepthCompressionFilter` class for ROS2 (Humble/Jazzy/Rolling) + - Auto-register `sensor_msgs/msg/CompressedImage` schema in MCAP + - Update channel registration to use composite keys (topic + message_type) + - Configuration support via YAML with boolean and object syntax + +- **middleware**: Add depthlitez submodule for specialized depth compression + - Shared compression library used by both ROS1 and ROS2 plugins + +### Changed +- **version**: Update project version to 0.3.0 + ## [0.2.0] - 2025-01-22 ### Added @@ -115,7 +137,8 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 --- -[Unreleased]: https://github.com/ArcheBase/Axon/compare/v0.2.0...HEAD +[Unreleased]: https://github.com/ArcheBase/Axon/compare/v0.3.0...HEAD +[0.3.0]: https://github.com/ArcheBase/Axon/compare/v0.2.0...v0.3.0 [0.2.0]: https://github.com/ArcheBase/Axon/compare/v0.1.0...v0.2.0 [0.1.0]: https://github.com/ArcheBase/Axon/compare/v0.0.1...v0.1.0 [0.0.1]: https://github.com/ArcheBase/Axon/releases/tag/v0.0.1 diff --git a/CLAUDE.md b/CLAUDE.md index fa20eb0..0a5d7f2 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -11,7 +11,8 @@ This file provides guidance to Claude Code (claude.ai/code) when working with co - Lock-free: Per-topic SPSC queues for zero-copy message handling - Fleet-ready: Server-controlled recording via HTTP RPC API (see [docs/designs/rpc-api-design.md](docs/designs/rpc-api-design.md)) - Crash-resilient: S3 uploader with SQLite state persistence and recovery -- Plugin-based: Middleware-agnostic core with ROS1/ROS2 plugins +- Plugin-based: Middleware-agnostic core with ROS1/ROS2/Zenoh plugins +- Filter-ready: Extensible data processing pipeline for compression and transformation ## Build and Test Commands @@ -133,17 +134,21 @@ Axon/ │ ├── axon_logging/ # Logging infrastructure (no ROS dependencies) │ └── axon_uploader/ # S3 uploader (no ROS dependencies) │ -├── middlewares/ # Middleware-specific plugins +├── middlewares/ # Middleware-specific plugins and filters │ ├── ros1/ # ROS1 (Noetic) plugin → libaxon_ros1.so -│ └── ros2/ # ROS2 (Humble/Jazzy/Rolling) plugin → libaxon_ros2.so -│ └── src/ros2_plugin/ # ROS2 plugin implementation +│ ├── ros2/ # ROS2 (Humble/Jazzy/Rolling) plugin → libaxon_ros2.so +│ │ └── src/ros2_plugin/ # ROS2 plugin implementation +│ ├── zenoh/ # Zenoh plugin → libaxon_zenoh.so +│ └── filters/ # Data processing filters (shared across plugins) +│ └── depthlitez/ # Depth image compression library │ ├── apps/ # Main applications │ ├── axon_recorder/ # Plugin loader and HTTP RPC server │ └── plugin_example/ # Example plugin implementation │ └── docs/designs/ # Design documents - └── rpc-api-design.md # HTTP RPC API specification + ├── rpc-api-design.md # HTTP RPC API specification + └── depth-compression-filter.md # Depth compression design ``` **Plugin ABI Interface:** @@ -158,6 +163,7 @@ The plugin interface is defined in [apps/axon_recorder/plugin_loader.hpp](apps/a 3. Core libraries can be tested independently of ROS 4. Only required middleware plugins need to be deployed 5. Middleware-specific bugs are isolated to plugin code +6. Filters in `middlewares/filters/` can be shared across plugins ### State Machine @@ -207,6 +213,13 @@ IDLE → READY → RECORDING ↔ PAUSED - Console, file, and ROS sinks - Severity filtering and rotation +**5. Depth Compression Filter** ([middlewares/filters/depthlitez/](middlewares/filters/depthlitez/)) + - Specialized compression for 16-bit depth images + - 5-10x compression ratio using DepthLiteZ algorithm + - Configurable compression levels (fast/medium/max) + - Integrates with ROS2 plugin via `DepthCompressionFilter` + - See [docs/designs/depth-compression-filter.md](docs/designs/depth-compression-filter.md) + ### Threading Model | Thread | Responsibility | @@ -594,6 +607,8 @@ grep -r "AXON_ROS1\|AXON_ROS2" build/ | ROS1 plugin | `middlewares/ros1/src/ros1_plugin/` (CMake) | | ROS2 plugin | `middlewares/ros2/src/ros2_plugin/` (CMake) | | Zenoh plugin | `middlewares/zenoh/` (CMake) | +| Filters | `middlewares/filters/` (shared data processing) | +| Depth compression | `middlewares/filters/depthlitez/` | | Main app (HTTP RPC) | `apps/axon_recorder/` | | Plugin example | `apps/plugin_example/` | | Plugin ABI interface | `apps/axon_recorder/plugin_loader.hpp` | diff --git a/CMakeLists.txt b/CMakeLists.txt index 331236b..434880d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,7 +14,7 @@ # ============================================================================= cmake_minimum_required(VERSION 3.14) -project(Axon VERSION 0.2.0 LANGUAGES CXX) +project(Axon VERSION 0.3.0 LANGUAGES CXX) # ============================================================================= # Centralized Path Definitions diff --git a/README.md b/README.md index 1cfdf6b..315f610 100644 --- a/README.md +++ b/README.md @@ -6,7 +6,7 @@ A high-performance, plugin-based data recorder with ROS1/ROS2 compatibility and fleet-ready HTTP RPC control for task-centric recording to MCAP format. -**Current Version:** 0.2.0 +**Current version:** 0.3.0 ## Architecture diff --git a/apps/axon_recorder/CMakeLists.txt b/apps/axon_recorder/CMakeLists.txt index 227396d..fc283f0 100644 --- a/apps/axon_recorder/CMakeLists.txt +++ b/apps/axon_recorder/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.12) -project(axon_recorder VERSION 0.2.0) +project(axon_recorder VERSION 0.3.0) # Use parent project version if available if(DEFINED AXON_REPO_ROOT AND EXISTS "${AXON_REPO_ROOT}/CMakeLists.txt") diff --git a/docs/designs/depth-compression-filter.md b/docs/designs/depth-compression-filter.md new file mode 100644 index 0000000..f1f12d1 --- /dev/null +++ b/docs/designs/depth-compression-filter.md @@ -0,0 +1,513 @@ +# Depth Compression Filter Design Document + +## Overview + +This document describes the design and implementation of the depth compression filter in Axon Recorder. Depth data (16-bit depth images) consume significant storage space, and specialized compression algorithms can greatly reduce storage requirements. + +**Goals**: +- Compress depth image data during recording +- Transparent to plugin layer, supporting ROS2 +- Configurable compression parameters + +## Background + +### Depth Data Characteristics + +| Property | Value | +|----------|-------| +| Data Type | `uint16_t` (16-bit) | +| Typical Resolutions | 640x480, 1280x1024, 1920x1080 | +| Raw Size | 640x480x2 = 614 KB/frame | +| Frame Rate | 30 FPS | +| Uncompressed Bandwidth | ~18 MB/s | + +### Depth Data Format + +Depth images typically use these ROS message types: +- `sensor_msgs/msg/Image` (ROS2) +- Encoding: `16UC1` (16-bit unsigned integer, 1 channel) + +### Compression Library: DepthLiteZ + +The `middlewares/filters/depthlitez` submodule provides specialized depth compression algorithms: +- **Input**: 16-bit depth data raw data +- **Output**: Compressed data stream +- **Compression Ratio**: Typically 5-10x +- **Lossy/Lossless**: Supports configurable quality parameters + +## Architecture Design + +### Overall Architecture + +``` +┌─────────────────────────────────────────────────────────────┐ +│ Axon Recorder │ +├─────────────────────────────────────────────────────────────┤ +│ │ +│ ┌──────────────┐ ┌──────────────┐ ┌────────────┐│ +│ │ ROS2 │ │ SPSC Queue │ │ MCAP ││ +│ │ Plugin │─────▶│ (per-topic) │─────▶│ Writer ││ +│ └──────────────┘ └──────────────┘ └────────────┘│ +│ ▲ ▲ │ +│ │ │ │ +│ Subscribe Write │ +│ │ │ │ +│ │ ┌──────────────────────────────┐ │ │ +│ │ │ Depth Compression Filter │ │ │ +│ │ │ ┌─────────────────────────┐ │ │ │ +│ │ │ │ is_depth_image()? │ │ │ │ +│ │ │ └───────────┬─────────────┘ │ │ │ +│ │ │ │ Yes │ │ │ +│ │ │ ┌───────────▼─────────────┐ │ │ │ +│ │ │ │ DepthCompressor:: │ │ │ │ +│ │ │ │ compress() │ │ │ │ +│ │ │ └───────────┬─────────────┘ │ │ │ +│ │ │ │ │ │ │ +│ │ │ ┌───────────▼─────────────┐ │ │ │ +│ │ │ │ Serialize Compressed │──┼──▶ │ +│ │ │ │ Image │ │ │ │ +│ │ │ └─────────────────────────┘ │ │ │ +│ │ └──────────────────────────────┘ │ │ +│ │ │ │ +└─────────┼──────────────────────────────────────────────┼──────┘ + │ │ + ROS Topics MCAP File + /camera/depth/* +``` + +### Key Design Decisions + +1. **Compression Location**: Executed in ROS2 plugin subscription callback + - ✅ No plugin callback thread blocking + - ✅ Compression happens before queuing to worker threads + - ✅ Worker threads write pre-compressed data + +2. **Compression Recognition**: Via configuration file + - Topics with `depth_compression: true` enable compression + - Validates `message_type` is `sensor_msgs/msg/Image` + - Validates image encoding is `16UC1` + +3. **Compressed Storage**: Uses `sensor_msgs/msg/CompressedImage` message type + - Compressed data stored in `CompressedImage.data` field + - Compression format identifier in `CompressedImage.format`: `dlz_fast`, `dlz_medium`, `dlz_max` + - MCAP schema becomes `sensor_msgs/msg/CompressedImage` + - Readers identify compression format via `format` field for decompression + +## Component Design + +### 1. DepthCompressor Class + +**Location**: `middlewares/filters/depth_compressor.hpp` + +```cpp +#include // depthlitez compression library + +namespace axon { +namespace depth { + +class DepthCompressor { +public: + struct Config { + dlz::CompressionLevel level = dlz::CompressionLevel::kFast; // Default: fast + std::string encoding = "16UC1"; // Input encoding format + }; + + /** + * Compress depth image data + * @param depth_data 16-bit depth data (row-major) + * @param width Image width + * @param height Image height + * @param compressed_data Output compressed data + * @return true if compression succeeded + */ + bool compress( + const uint8_t* depth_data, + size_t width, + size_t height, + std::vector& compressed_data + ); + + /** + * Get compression format identifier for current config + * @return Compression format string for CompressedImage.format ("dlz_fast", "dlz_medium", "dlz_max") + */ + std::string get_compression_format() const; + + /** + * Set compressor configuration + */ + void set_config(const Config& config); + +private: + Config config_; +}; + +} // namespace depth +} // namespace axon +``` + +### 2. DepthCompressionFilter Class + +**Location**: `middlewares/ros2/include/depth_compression_filter.hpp` + +```cpp +namespace ros2_plugin { + +struct DepthCompressionConfig { + bool enabled = false; + std::string level = "fast"; // fast, medium, max (maps to dlz::CompressionLevel) +}; + +/** + * @brief Depth image compression filter for ROS2 plugin + * + * Processes sensor_msgs::msg::Image messages and compresses 16UC1 depth images + * to sensor_msgs::msg::CompressedImage format using DepthLiteZ compression. + */ +class DepthCompressionFilter { +public: + explicit DepthCompressionFilter(const DepthCompressionConfig& config); + + /** + * Filter and process messages + * - Checks if message is an Image with 16UC1 encoding + * - Compresses depth data if applicable + * - Passes through non-depth or failed compression unchanged + */ + void filter_and_process( + const std::string& topic, + const std::string& message_type, + const std::vector& data, + uint64_t timestamp_ns, + ProcessedCallback callback + ); + +private: + /** + * Extract depth data from serialized Image message + * @return Tuple of (data pointer, width, height) or (nullptr, 0, 0) + */ + std::tuple extract_depth_data( + const std::vector& image_msg + ); + + /** + * Build CompressedImage message with compressed data + */ + std::vector build_compressed_image_msg( + const std::string& format, + const std::vector& compressed_data, + uint64_t timestamp_ns + ); + + DepthCompressor compressor_; + DepthCompressionConfig config_; +}; + +} // namespace ros2_plugin +``` + +### 3. Configuration Extension + +**Location**: `apps/axon_recorder/recorder.hpp` + +```cpp +struct DepthCompression { + bool enabled = false; + std::string level = "fast"; // fast, medium, max (maps to dlz::CompressionLevel) +}; + +struct SubscriptionConfig { + std::string topic_name; + std::string message_type; + + // Depth compression configuration (optional) + std::optional depth_compression; + + // Existing configuration + size_t batch_size = 1; + int flush_interval_ms = 100; +}; +``` + +## Configuration + +### YAML Configuration Examples + +**Simple syntax** (boolean): +```yaml +# config/default_config_ros2.yaml +subscriptions: + # Depth image with compression enabled + - name: /camera/depth/image_rect_raw + message_type: sensor_msgs/msg/Image + batch_size: 300 + flush_interval_ms: 10000 + depth_compression: true # Enable with default "fast" level + + # Regular image without compression + - name: /camera/color/image_raw + message_type: sensor_msgs/msg/Image + # No depth_compression field or false = disabled +``` + +**Advanced syntax** (object): +```yaml +subscriptions: + - name: /camera/depth/image_rect_raw + message_type: sensor_msgs/msg/Image + depth_compression: + enabled: true + level: max # fast, medium, or max +``` + +**Supported compression levels**: +- `fast` (default): 、fastest speed +- `medium`: balanced +- `max`: best compression + +## Recording Flow + +### Message Processing Pipeline + +``` +ROS2 Topic (sensor_msgs::msg::Image) + │ + ▼ +ROS2 Generic Subscription Callback + │ + ├─▶ DepthCompressionFilter::filter_and_process() + │ │ + │ ├─▶ Is message_type == sensor_msgs::msg::Image? + │ │ │ Yes + │ │ ▼ + │ │ Extract depth data from CDR serialization + │ │ │ + │ │ ├─▶ Is encoding == 16UC1? + │ │ │ │ Yes + │ │ │ ▼ + │ │ │ DepthCompressor::compress() + │ │ │ │ + │ │ │ ├─▶ Compression successful + │ │ │ │ │ + │ │ │ │ ▼ + │ │ │ │ Build CompressedImage message + │ │ │ │ │ + │ │ │ │ ▼ + │ │ │ │ Change message_type to sensor_msgs::msg::CompressedImage + │ │ │ │ + │ │ │ └─▶ Compression failed → Pass through original + │ │ │ + │ │ └─▶ Not 16UC1 or compression disabled → Pass through + │ │ + │ ▼ + │ SPSC Queue (compressed or original message) + │ │ + ▼ ▼ +Worker Thread (per topic) + │ + ├─▶ Message handler writes to MCAP + │ │ + │ ▼ + │ MCAP Writer + │ │ + ▼ ▼ +MCAP File (CompressedImage type for compressed depth, Image type for others) +``` + +**Key Implementation Detail:** +- Compression occurs **before** queuing in the ROS2 subscription callback +- This ensures compression doesn't block worker threads +- Worker threads only handle pre-compressed data for efficient writing + +## Implementation Status + +### Phase 1: Basic Integration ✅ +- [x] Create `DepthCompressor` wrapper class +- [x] Implement `DepthCompressionFilter` with CDR deserialization +- [x] Integrate compression logic in ROS2 subscription callback +- [x] Add configuration file support (boolean and object syntax) +- [x] Auto-register CompressedImage schema in MCAP +- [x] Update channel registration to use composite keys (topic + message_type) + +### Phase 2: Testing and Optimization +- [ ] Performance benchmarking +- [ ] Memory usage optimization +- [ ] Unit tests and integration tests + +## Performance Considerations + +### Expected Performance Metrics + +| Metric | Target Value | +|--------|--------------| +| Compression Latency | < 5 ms/frame (640x480) | +| Compression Ratio | 5-10x | +| CPU Usage | < 10% (single core) | +| Memory Overhead | < 50 MB | + +### Threading Impact Analysis + +**Since compression occurs in the ROS2 subscription callback before queuing:** + +``` +Compression time (in callback): T_compress ≈ 3 ms +Queue write time: T_queue ≈ 0.1 ms +Worker write time: T_write ≈ 1 ms +Total time per message: T_total = T_compress + T_queue + T_write ≈ 4.1 ms + +Maximum frame rate: 1000 / 4.1 ≈ 244 FPS (per topic) +``` + +**Advantages of callback-based compression:** +- Worker threads are not blocked by compression +- Multiple topics can process compression in parallel (one callback thread per topic) +- No contention on worker thread pool +- Consistent write throughput + +**Trade-offs:** +- Callback thread latency increases (acceptable for typical depth camera frame rates of 30 FPS) +- Memory allocation during compression (mitigated by reusing buffers where possible) + +## Error Handling + +| Error Scenario | Handling Strategy | +|----------------|-------------------| +| Compression failure | Fall back to writing original data, log warning | +| Unsupported encoding | Skip compression, write original data (16UC1 only) | +| Out of memory | Log error, write original data | +| Configuration error | Validate at startup, refuse to start if invalid | + +**Current Implementation:** +- All compression errors are caught in `filter_and_process()` and fall back to pass-through +- Non-16UC1 images are passed through unchanged +- Missing or invalid depth_compression config defaults to disabled + +## Usage Examples + +### Command Line Usage + +```bash +# Using configuration file (recommended) +axon_recorder --config config/default_config_ros2.yaml + +# The config file enables compression per topic: +# subscriptions: +# - name: /camera/depth/image_rect_raw +# message_type: sensor_msgs/msg/Image +# depth_compression: true # Enable with default "fast" level +``` + +### Configuration Examples + +**Simple boolean syntax** (default "fast" level): +```yaml +subscriptions: + - name: /camera/depth/image_rect_raw + message_type: sensor_msgs/msg/Image + batch_size: 300 + flush_interval_ms: 10000 + depth_compression: true # Enable with default "fast" level +``` + +**Advanced object syntax** (custom level): +```yaml +subscriptions: + - name: /camera/depth/image_rect_raw + message_type: sensor_msgs/msg/Image + depth_compression: + enabled: true + level: max # Options: fast, medium, max +``` + +**Compression levels:** +- `fast` (default):fastest speed +- `medium`: balanced +- `max`: best compression + +### Programmatic Usage + +The depth compression is handled automatically by the ROS2 plugin when enabled in configuration. The compression filter: + +1. Intercepts `sensor_msgs::msg::Image` messages in the subscription callback +2. Checks if encoding is `16UC1` +3. Compresses depth data using DepthLiteZ +4. Transforms message to `sensor_msgs::msg::CompressedImage` +5. Updates message type metadata for MCAP schema registration + +**Compression format identifiers** (stored in `CompressedImage.format`): +- `dlz::CompressionLevel::kFast` → `"dlz_fast"` +- `dlz::CompressionLevel::kMedium` → `"dlz_medium"` +- `dlz::CompressionLevel::kMax` → `"dlz_max"` + +## Testing Plan + +### Unit Tests + +```cpp +// Test cases for DepthCompressor +TEST(DepthCompressor, Compress16UC1) { + axon::depth::DepthCompressor compressor; + std::vector depth_data(640 * 480 * 2); + std::vector compressed; + + ASSERT_TRUE(compressor.compress(depth_data.data(), 640, 480, compressed)); + EXPECT_GT(depth_data.size(), compressed.size()); +} + +TEST(DepthCompressor, CompressionRatio) { + // Test real-world compression ratios +} + +TEST(DepthCompressor, InvalidInput) { + // Test error handling +} + +// Test cases for DepthCompressionFilter +TEST(DepthCompressionFilter, NonDepthImagePassthrough) { + // Test that non-16UC1 images pass through unchanged +} + +TEST(DepthCompressionFilter, CompressionFailurePassthrough) { + // Test fallback on compression failure +} +``` + +### Integration Tests + +```bash +# Start ROS2 depth camera publisher +ros2 launch depth_camera_publisher.launch.py + +# Record with compression enabled +axon_recorder --config config/default_config_ros2.yaml + +# Verify MCAP contents: +# - Topic should use sensor_msgs/msg/CompressedImage schema +# - CompressedImage.format should be "dlz_fast", "dlz_medium", or "dlz_max" +# - Decompressed data should match original depth images +``` + +### Verification Steps + +1. **Check MCAP schema:** + ```bash + mcap info recording.mcap | grep CompressedImage + ``` + +2. **Verify compression format:** + - Use Foxglove Studio or `mcap dump` to inspect messages + - Confirm `CompressedImage.format` field contains compression identifier + +3. **Validate decompression:** + - Implement reader that decompresses based on `format` field + - Compare with uncompressed depth images + +## References + +- [depthlitez repository](../middlewares/filters/depthlitez) - Depth compression algorithm library +- [MCAP specification](https://mcap.dev/spec) - MCAP file format documentation +- [ROS2 sensor messages](https://github.com/ros2/common_interfaces) - Message type definitions +- [CDR serialization specification](https://www.omg.org/spec/DDS/1.4/PDF) - ROS2 serialization format +- [ROS2 plugin implementation](middlewares/ros2/src/ros2_plugin/) - Plugin code +- [Configuration examples](apps/axon_recorder/config/default_config_ros2.yaml) - Sample configurations From e467149bcde4d08a36a9a35fd267dbd904867e27 Mon Sep 17 00:00:00 2001 From: Liangwei XU Date: Mon, 2 Feb 2026 20:05:09 +0800 Subject: [PATCH 09/21] build: make depth compression opt-in via AXON_ENABLE_DEPTH_COMPRESSION --- .gitignore | 6 +- CHANGELOG | 22 ++-- middlewares/filters/CMakeLists.txt | 108 +++++++++++++----- middlewares/filters/src/stub.cpp | 6 + middlewares/ros1/CMakeLists.txt | 66 +++++++---- .../include/ros1_subscription_wrapper.hpp | 23 +++- .../ros1/src/ros1_subscription_wrapper.cpp | 7 ++ middlewares/ros2/CMakeLists.txt | 52 +++++++-- .../include/ros2_subscription_wrapper.hpp | 24 +++- .../ros2/src/ros2_subscription_wrapper.cpp | 31 ++++- 10 files changed, 265 insertions(+), 80 deletions(-) create mode 100644 middlewares/filters/src/stub.cpp diff --git a/.gitignore b/.gitignore index ab2b1ac..33bed94 100644 --- a/.gitignore +++ b/.gitignore @@ -148,4 +148,8 @@ core/build_uploader/ # Cache directories .cache/ -core/filters/ + +# DepthLiteZ private submodule (not included in public releases) +# Users with access can initialize manually with: +# git submodule update --init --recursive middlewares/filters/depthlitez +middlewares/filters/depthlitez/ diff --git a/CHANGELOG b/CHANGELOG index ff8d98c..ed69677 100644 --- a/CHANGELOG +++ b/CHANGELOG @@ -15,24 +15,28 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ## [0.3.0] - 2025-02-02 ### Added -- **filters**: Implement depth image compression filter for ROS1 plugin - - Add `DepthCompressionFilter` class for ROS1 Noetic +- **filters**: Implement depth image compression filter (opt-in feature) + - Add `DepthCompressionFilter` class for ROS1 Noetic and ROS2 (Humble/Jazzy/Rolling) - Support `sensor_msgs/Image` to `sensor_msgs/CompressedImage` transformation - Integrate DepthLiteZ compression library with 5-10x compression ratio - Support configurable compression levels: fast, medium, max - Pass-through for non-depth images or compression failures + - **Note**: Depth compression is **disabled by default** for open-source builds + - Requires private DepthLiteZ submodule access + - Enable with: `cmake -DAXON_ENABLE_DEPTH_COMPRESSION=ON` + - See documentation for setup instructions -- **filters**: Add depth compression support to ROS2 plugin - - Extend `DepthCompressionFilter` class for ROS2 (Humble/Jazzy/Rolling) - - Auto-register `sensor_msgs/msg/CompressedImage` schema in MCAP - - Update channel registration to use composite keys (topic + message_type) - - Configuration support via YAML with boolean and object syntax - -- **middleware**: Add depthlitez submodule for specialized depth compression +- **middleware**: Add depthlitez submodule reference for specialized depth compression - Shared compression library used by both ROS1 and ROS2 plugins + - Submodule is listed in `.gitmodules` but excluded from public builds + - Users with access can initialize: `git submodule update --init --recursive middlewares/filters/depthlitez` ### Changed - **version**: Update project version to 0.3.0 +- **build**: Make depth compression an opt-in feature via `AXON_ENABLE_DEPTH_COMPRESSION` CMake option + - Default OFF for open-source compatibility + - When OFF, depth compression code is excluded from build + - ROS1/ROS2 plugins build successfully without DepthLiteZ dependency ## [0.2.0] - 2025-01-22 diff --git a/middlewares/filters/CMakeLists.txt b/middlewares/filters/CMakeLists.txt index c301f45..c3311c3 100644 --- a/middlewares/filters/CMakeLists.txt +++ b/middlewares/filters/CMakeLists.txt @@ -1,5 +1,9 @@ # Depth Filters Library # Provides depth image compression using DepthLiteZ +# +# NOTE: Depth compression requires the private DepthLiteZ submodule. +# By default, depth compression is DISABLED for open-source builds. +# To enable: cmake -DAXON_ENABLE_DEPTH_COMPRESSION=ON cmake_minimum_required(VERSION 3.16) @@ -15,35 +19,61 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) # Export compile commands set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +# ============================================================================= # Options +# ============================================================================= option(BUILD_TESTING "Build tests" ON) option(BUILD_SHARED "Build shared library" OFF) -# Find dependencies -find_package(PkgConfig REQUIRED) +# Depth compression option (default OFF for open-source builds) +option(AXON_ENABLE_DEPTH_COMPRESSION "Enable depth image compression (requires private DepthLiteZ submodule)" OFF) -# Find LZ4 -pkg_check_modules(LZ4 REQUIRED liblz4) +if(AXON_ENABLE_DEPTH_COMPRESSION) + message(STATUS "Depth compression: ENABLED") +else() + message(STATUS "Depth compression: DISABLED (default for open-source builds)") +endif() -# Find ZSTD -pkg_check_modules(ZSTD REQUIRED libzstd) +# ============================================================================= +# Find dependencies +# ============================================================================= +find_package(PkgConfig REQUIRED) -# DepthLiteZ submodule -set(DEPTHLITEZ_DIR ${CMAKE_CURRENT_SOURCE_DIR}/depthlitez) -if(NOT EXISTS ${DEPTHLITEZ_DIR}/include/dlz.hpp) - message(WARNING "DepthLiteZ submodule not found. Run: git submodule update --init --recursive") +# LZ4 and ZSTD are only needed if depth compression is enabled +if(AXON_ENABLE_DEPTH_COMPRESSION) + pkg_check_modules(LZ4 REQUIRED liblz4) + pkg_check_modules(ZSTD REQUIRED libzstd) + + # DepthLiteZ submodule + set(DEPTHLITEZ_DIR ${CMAKE_CURRENT_SOURCE_DIR}/depthlitez) + if(NOT EXISTS ${DEPTHLITEZ_DIR}/include/dlz.hpp) + message(FATAL_ERROR + "AXON_ENABLE_DEPTH_COMPRESSION is ON but DepthLiteZ submodule not found.\n" + " DepthLiteZ is a private repository. To enable depth compression:\n" + " 1. Ensure you have access to the DepthLiteZ repository\n" + " 2. Run: git submodule update --init --recursive middlewares/filters/depthlitez\n" + " 3. Or rebuild with: cmake -DAXON_ENABLE_DEPTH_COMPRESSION=OFF" + ) + endif() +else() + message(STATUS "Skipping LZ4/ZSTD dependency checks (depth compression disabled)") endif() +# ============================================================================= # Sources -set(SOURCES - src/depth_compressor.cpp -) +# ============================================================================= +set(SOURCES "") +set(HEADERS "") -set(HEADERS - include/depth_compressor.hpp -) +if(AXON_ENABLE_DEPTH_COMPRESSION) + list(APPEND SOURCES src/depth_compressor.cpp) + list(APPEND HEADERS include/depth_compressor.hpp) +else() + # Use stub source when depth compression is disabled + list(APPEND SOURCES src/stub.cpp) +endif() -# Create library +# Create library (even if empty, for dependency consistency) if(BUILD_SHARED) add_library(axon_filters SHARED ${SOURCES}) else() @@ -53,17 +83,24 @@ endif() target_include_directories(axon_filters PUBLIC $ - $ $ PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src ) -target_link_libraries(axon_filters - PUBLIC - ${LZ4_LIBRARIES} - ${ZSTD_LIBRARIES} -) +# Add DepthLiteZ include directory only if enabled +if(AXON_ENABLE_DEPTH_COMPRESSION) + target_include_directories(axon_filters + PUBLIC + $ + ) + target_link_libraries(axon_filters + PUBLIC + ${LZ4_LIBRARIES} + ${ZSTD_LIBRARIES} + ) + target_compile_definitions(axon_filters PRIVATE AXON_ENABLE_DEPTH_COMPRESSION) +endif() # Ensure PIC is used for static library (required when linking into shared library) set_target_properties(axon_filters PROPERTIES @@ -78,8 +115,10 @@ target_compile_options(axon_filters # Alias for consistent naming add_library(axon::filters ALIAS axon_filters) +# ============================================================================= # Tests -if(BUILD_TESTING) +# ============================================================================= +if(BUILD_TESTING AND AXON_ENABLE_DEPTH_COMPRESSION) enable_testing() find_package(GTest REQUIRED) @@ -100,9 +139,13 @@ if(BUILD_TESTING) target_include_directories(${TEST_NAME} PRIVATE ${DEPTHLITEZ_DIR}/include) add_test(NAME ${TEST_NAME} COMMAND ${TEST_NAME}) endforeach() +elseif(BUILD_TESTING) + message(STATUS "Tests disabled (require AXON_ENABLE_DEPTH_COMPRESSION=ON)") endif() +# ============================================================================= # Installation +# ============================================================================= include(GNUInstallDirs) install(TARGETS axon_filters @@ -112,12 +155,23 @@ install(TARGETS axon_filters RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} ) -install(FILES ${HEADERS} - DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/axon/filters -) +if(AXON_ENABLE_DEPTH_COMPRESSION) + install(FILES ${HEADERS} + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/axon/filters + ) +endif() install(EXPORT axon_filters_targets FILE axon_filters_targets.cmake NAMESPACE axon:: DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/axon_filters ) + +# ============================================================================= +# Summary +# ============================================================================= +message(STATUS "") +message(STATUS "Axon Filters Configuration:") +message(STATUS " Depth Compression: ${AXON_ENABLE_DEPTH_COMPRESSION}") +message(STATUS " Build Shared: ${BUILD_SHARED}") +message(STATUS "") diff --git a/middlewares/filters/src/stub.cpp b/middlewares/filters/src/stub.cpp new file mode 100644 index 0000000..7a23d79 --- /dev/null +++ b/middlewares/filters/src/stub.cpp @@ -0,0 +1,6 @@ +// SPDX-FileCopyrightText: 2026 ArcheBase +// +// SPDX-License-Identifier: MulanPSL-2.0 + +// Stub source file for building axon_filters when depth compression is disabled +// This ensures the library target can always be created, even when empty diff --git a/middlewares/ros1/CMakeLists.txt b/middlewares/ros1/CMakeLists.txt index eb7d7b7..bf5c555 100644 --- a/middlewares/ros1/CMakeLists.txt +++ b/middlewares/ros1/CMakeLists.txt @@ -19,6 +19,17 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +# ============================================================================= +# Options +# ============================================================================= +option(AXON_ENABLE_DEPTH_COMPRESSION "Enable depth image compression (requires private DepthLiteZ)" OFF) + +if(AXON_ENABLE_DEPTH_COMPRESSION) + message(STATUS "ROS1 Plugin: Depth compression ENABLED") +else() + message(STATUS "ROS1 Plugin: Depth compression DISABLED") +endif() + # ============================================================================= # Find ROS1 # ============================================================================= @@ -51,11 +62,22 @@ endif() # Build the Plugin Library # ============================================================================= -add_library(axon_ros1_plugin SHARED +# Core sources (always built) +set(CORE_SOURCES src/ros1_plugin.cpp src/ros1_subscription_wrapper.cpp src/ros1_plugin_export.cpp - src/depth_compression_filter.cpp +) + +# Depth compression sources (conditional) +set(DEPTH_SOURCES "") +if(AXON_ENABLE_DEPTH_COMPRESSION) + list(APPEND DEPTH_SOURCES src/depth_compression_filter.cpp) +endif() + +add_library(axon_ros1_plugin SHARED + ${CORE_SOURCES} + ${DEPTH_SOURCES} ) target_include_directories(axon_ros1_plugin PUBLIC @@ -64,17 +86,20 @@ target_include_directories(axon_ros1_plugin PUBLIC ${ROS1_INCLUDE_DIR} ) -# Add filters include directory for depth compression -target_include_directories(axon_ros1_plugin PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/../filters/include - ${CMAKE_CURRENT_SOURCE_DIR}/../filters/depthlitez/include -) +# Add filters include directory for depth compression (conditional) +if(AXON_ENABLE_DEPTH_COMPRESSION) + target_include_directories(axon_ros1_plugin PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/../filters/include + ${CMAKE_CURRENT_SOURCE_DIR}/../filters/depthlitez/include + ) + target_compile_definitions(axon_ros1_plugin PRIVATE AXON_ENABLE_DEPTH_COMPRESSION) +endif() -# Find and link LZ4 and ZSTD for depth compression -find_package(PkgConfig QUIET) -if(PKG_CONFIG_FOUND) - pkg_check_modules(LZ4 liblz4 IMPORTED_TARGET) - pkg_check_modules(ZSTD libzstd IMPORTED_TARGET) +# Find and link LZ4 and ZSTD for depth compression (conditional) +if(AXON_ENABLE_DEPTH_COMPRESSION) + find_package(PkgConfig REQUIRED) + pkg_check_modules(LZ4 REQUIRED liblz4) + pkg_check_modules(ZSTD REQUIRED libzstd) endif() # Link ROS1 libraries @@ -86,17 +111,11 @@ target_link_libraries(axon_ros1_plugin PUBLIC nlohmann_json::nlohmann_json ) -# Link filters library for depth compression -target_link_libraries(axon_ros1_plugin PRIVATE - axon_filters -) - -# Link LZ4 and ZSTD if found -if(TARGET PkgConfig::LZ4) - target_link_libraries(axon_ros1_plugin PRIVATE PkgConfig::LZ4) -endif() -if(TARGET PkgConfig::ZSTD) - target_link_libraries(axon_ros1_plugin PRIVATE PkgConfig::ZSTD) +# Link filters library for depth compression (conditional) +if(AXON_ENABLE_DEPTH_COMPRESSION) + target_link_libraries(axon_ros1_plugin PRIVATE axon_filters) + target_link_libraries(axon_ros1_plugin PRIVATE ${LZ4_LIBRARIES}) + target_link_libraries(axon_ros1_plugin PRIVATE ${ZSTD_LIBRARIES}) endif() # Export all symbols for dynamic loading @@ -167,5 +186,6 @@ message(STATUS "Axon ROS1 Plugin Configuration:") message(STATUS " C++ Standard: ${CMAKE_CXX_STANDARD}") message(STATUS " Build Type: ${CMAKE_BUILD_TYPE}") message(STATUS " ROS1 Root: ${ROS1_DIR}") +message(STATUS " Depth Compression: ${AXON_ENABLE_DEPTH_COMPRESSION}") message(STATUS " Tests Enabled: ${AXON_BUILD_TESTS}") message(STATUS "") diff --git a/middlewares/ros1/include/ros1_subscription_wrapper.hpp b/middlewares/ros1/include/ros1_subscription_wrapper.hpp index 7c55d9f..c60313d 100644 --- a/middlewares/ros1/include/ros1_subscription_wrapper.hpp +++ b/middlewares/ros1/include/ros1_subscription_wrapper.hpp @@ -17,10 +17,17 @@ namespace ros1_plugin { +// ============================================================================= +// Depth Compression Support (conditional compilation) +// ============================================================================= +#ifdef AXON_ENABLE_DEPTH_COMPRESSION + // Forward declarations struct DepthCompressionConfig; class DepthCompressionFilter; +#endif // AXON_ENABLE_DEPTH_COMPRESSION + // Message callback type - passes raw serialized message data using MessageCallback = std::function& depth_compression ); +#endif // AXON_ENABLE_DEPTH_COMPRESSION + // Unsubscribe from a topic bool unsubscribe(const std::string& topic_name); @@ -52,12 +63,22 @@ class SubscriptionManager { private: ros::NodeHandlePtr node_handle_; + +#ifdef AXON_ENABLE_DEPTH_COMPRESSION + // Forward declarations for conditional members + struct DepthCompressionConfig; + class DepthCompressionFilter; +#endif + struct SubscriptionInfo { ros::Subscriber subscriber; MessageCallback callback; std::string message_type; +#ifdef AXON_ENABLE_DEPTH_COMPRESSION std::shared_ptr depth_filter; +#endif }; + std::unordered_map subscriptions_; mutable std::mutex mutex_; }; diff --git a/middlewares/ros1/src/ros1_subscription_wrapper.cpp b/middlewares/ros1/src/ros1_subscription_wrapper.cpp index dad522c..a398f9f 100644 --- a/middlewares/ros1/src/ros1_subscription_wrapper.cpp +++ b/middlewares/ros1/src/ros1_subscription_wrapper.cpp @@ -8,7 +8,9 @@ #include #include +#ifdef AXON_ENABLE_DEPTH_COMPRESSION #include "depth_compression_filter.hpp" +#endif namespace ros1_plugin { @@ -94,6 +96,8 @@ bool SubscriptionManager::subscribe( } } +#ifdef AXON_ENABLE_DEPTH_COMPRESSION + bool SubscriptionManager::subscribe( const std::string& topic_name, const std::string& message_type, uint32_t queue_size, MessageCallback callback, const std::optional& depth_compression @@ -167,6 +171,7 @@ bool SubscriptionManager::subscribe( info.subscriber = subscriber; info.callback = callback; info.message_type = message_type; + info.depth_filter = depth_filter; subscriptions_[topic_name] = std::move(info); ROS_INFO("Subscribed to topic: %s (%s)", topic_name.c_str(), message_type.c_str()); @@ -179,6 +184,8 @@ bool SubscriptionManager::subscribe( } } +#endif // AXON_ENABLE_DEPTH_COMPRESSION + bool SubscriptionManager::unsubscribe(const std::string& topic_name) { std::lock_guard lock(mutex_); diff --git a/middlewares/ros2/CMakeLists.txt b/middlewares/ros2/CMakeLists.txt index c46f66f..d849ed1 100644 --- a/middlewares/ros2/CMakeLists.txt +++ b/middlewares/ros2/CMakeLists.txt @@ -19,6 +19,17 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +# ============================================================================= +# Options +# ============================================================================= +option(AXON_ENABLE_DEPTH_COMPRESSION "Enable depth image compression (requires private DepthLiteZ)" OFF) + +if(AXON_ENABLE_DEPTH_COMPRESSION) + message(STATUS "ROS2 Plugin: Depth compression ENABLED") +else() + message(STATUS "ROS2 Plugin: Depth compression DISABLED") +endif() + # ============================================================================= # Find ROS2 # ============================================================================= @@ -66,11 +77,22 @@ endforeach() # Build the Plugin Library # ============================================================================= -add_library(axon_ros2_plugin SHARED +# Core sources (always built) +set(CORE_SOURCES src/ros2_plugin.cpp src/ros2_subscription_wrapper.cpp src/ros2_plugin_export.cpp - src/depth_compression_filter.cpp +) + +# Depth compression sources (conditional) +set(DEPTH_SOURCES "") +if(AXON_ENABLE_DEPTH_COMPRESSION) + list(APPEND DEPTH_SOURCES src/depth_compression_filter.cpp) +endif() + +add_library(axon_ros2_plugin SHARED + ${CORE_SOURCES} + ${DEPTH_SOURCES} ) target_include_directories(axon_ros2_plugin PUBLIC @@ -92,12 +114,21 @@ ament_target_dependencies(axon_ros2_plugin PUBLIC std_msgs ) -# Link filters library for depth compression -if(TARGET axon::filters) - target_link_libraries(axon_ros2_plugin PRIVATE axon::filters) - target_include_directories(axon_ros2_plugin PRIVATE - ${AXON_MIDDLEWARES_DIR}/filters/include - ) +# Link filters library for depth compression (conditional) +if(AXON_ENABLE_DEPTH_COMPRESSION) + if(TARGET axon::filters) + target_link_libraries(axon_ros2_plugin PRIVATE axon::filters) + target_include_directories(axon_ros2_plugin PRIVATE + ${AXON_MIDDLEWARES_DIR}/filters/include + ${AXON_MIDDLEWARES_DIR}/filters/depthlitez/include + ) + target_compile_definitions(axon_ros2_plugin PRIVATE AXON_ENABLE_DEPTH_COMPRESSION) + else() + message(FATAL_ERROR + "AXON_ENABLE_DEPTH_COMPRESSION is ON but axon::filters target not found. " + "Build filters first with AXON_ENABLE_DEPTH_COMPRESSION=ON." + ) + endif() endif() # Export all symbols for dynamic loading @@ -135,8 +166,8 @@ if(AXON_BUILD_TESTS) std_msgs ) - # Add filters include directory for tests - if(TARGET axon::filters) + # Add filters include directory for tests (conditional) + if(AXON_ENABLE_DEPTH_COMPRESSION AND TARGET axon::filters) target_include_directories(${test_name} PRIVATE ${AXON_MIDDLEWARES_DIR}/filters/include ${AXON_MIDDLEWARES_DIR}/filters/depthlitez/include @@ -175,5 +206,6 @@ message(STATUS "Axon ROS2 Plugin Configuration:") message(STATUS " C++ Standard: ${CMAKE_CXX_STANDARD}") message(STATUS " Build Type: ${CMAKE_BUILD_TYPE}") message(STATUS " ROS2 Root: ${ROS2_DIR}") +message(STATUS " Depth Compression: ${AXON_ENABLE_DEPTH_COMPRESSION}") message(STATUS " Tests Enabled: ${AXON_BUILD_TESTS}") message(STATUS "") diff --git a/middlewares/ros2/include/ros2_subscription_wrapper.hpp b/middlewares/ros2/include/ros2_subscription_wrapper.hpp index ed34cc5..0efd98d 100644 --- a/middlewares/ros2/include/ros2_subscription_wrapper.hpp +++ b/middlewares/ros2/include/ros2_subscription_wrapper.hpp @@ -7,7 +7,6 @@ #include -#include #include #include #include @@ -18,7 +17,24 @@ namespace ros2_plugin { -// DepthCompressionConfig is defined in depth_compression_filter.hpp +// ============================================================================= +// Depth Compression Support (conditional compilation) +// ============================================================================= +#ifdef AXON_ENABLE_DEPTH_COMPRESSION + +#include + +#else + +// Forward declarations when depth compression is disabled +struct DepthCompressionConfig { + bool enabled = false; + std::string level = "medium"; +}; + +class DepthCompressionFilter; + +#endif // AXON_ENABLE_DEPTH_COMPRESSION // Message callback type - passes raw serialized message data using MessageCallback = std::function compression_filter; +#ifdef AXON_ENABLE_DEPTH_COMPRESSION + std::shared_ptr compression_filter; +#endif }; std::unordered_map subscriptions_; mutable std::mutex mutex_; diff --git a/middlewares/ros2/src/ros2_subscription_wrapper.cpp b/middlewares/ros2/src/ros2_subscription_wrapper.cpp index d542bab..971375b 100644 --- a/middlewares/ros2/src/ros2_subscription_wrapper.cpp +++ b/middlewares/ros2/src/ros2_subscription_wrapper.cpp @@ -6,7 +6,9 @@ #include +#ifdef AXON_ENABLE_DEPTH_COMPRESSION #include "depth_compression_filter.hpp" +#endif namespace ros2_plugin { @@ -35,17 +37,27 @@ bool SubscriptionManager::subscribe( } try { +#ifdef AXON_ENABLE_DEPTH_COMPRESSION // Create compression filter (if configured) - std::unique_ptr compression_filter; - if (options.depth_compression.has_value()) { - compression_filter = std::make_unique(*options.depth_compression); + std::shared_ptr compression_filter; + if (options.depth_compression.has_value() && options.depth_compression->enabled) { + compression_filter = std::make_shared(*options.depth_compression); RCUTILS_LOG_INFO( - "Depth compression enabled for topic %s: enabled=%s, level=%s", + "Depth compression enabled for topic %s: level=%s", topic_name.c_str(), - options.depth_compression->enabled ? "true" : "false", options.depth_compression->level.c_str() ); } +#else + // Depth compression not available at compile time + if (options.depth_compression.has_value() && options.depth_compression->enabled) { + RCUTILS_LOG_WARN( + "Depth compression requested for topic %s but not enabled at build time. " + "Rebuild with -DAXON_ENABLE_DEPTH_COMPRESSION=ON to enable.", + topic_name.c_str() + ); + } +#endif // Create generic subscription using ROS2's built-in API // Capture topic_name by value and lookup filter in subscriptions_ map when message arrives @@ -69,6 +81,7 @@ bool SubscriptionManager::subscribe( // Get current time as timestamp rclcpp::Time timestamp = rclcpp::Clock(RCL_SYSTEM_TIME).now(); +#ifdef AXON_ENABLE_DEPTH_COMPRESSION // Look up compression filter (if exists) auto it = subscriptions_.find(topic_name); bool has_filter = @@ -95,6 +108,10 @@ bool SubscriptionManager::subscribe( // Direct callback callback(topic_name, message_type, data, timestamp); } +#else + // Direct callback (no depth compression support) + callback(topic_name, message_type, data, timestamp); +#endif } catch (const std::exception& e) { RCUTILS_LOG_ERROR( @@ -113,7 +130,9 @@ bool SubscriptionManager::subscribe( SubscriptionInfo info; info.subscription = subscription; info.callback = callback; - info.compression_filter = std::move(compression_filter); +#ifdef AXON_ENABLE_DEPTH_COMPRESSION + info.compression_filter = compression_filter; +#endif subscriptions_[topic_name] = std::move(info); RCUTILS_LOG_INFO("Subscribed to topic: %s (%s)", topic_name.c_str(), message_type.c_str()); From cd4afd599730bbf610625dd3fcfa33b3f9388ac5 Mon Sep 17 00:00:00 2001 From: Liangwei XU Date: Mon, 2 Feb 2026 20:25:01 +0800 Subject: [PATCH 10/21] fix(ros2): add conditional compilation for depth compression in plugin export --- middlewares/ros2/src/ros2_plugin_export.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/middlewares/ros2/src/ros2_plugin_export.cpp b/middlewares/ros2/src/ros2_plugin_export.cpp index b40b7a8..d88a2f2 100644 --- a/middlewares/ros2/src/ros2_plugin_export.cpp +++ b/middlewares/ros2/src/ros2_plugin_export.cpp @@ -20,7 +20,9 @@ #include #include +#ifdef AXON_ENABLE_DEPTH_COMPRESSION #include "depth_compressor.hpp" +#endif #include "ros2_plugin.hpp" #include "ros2_subscription_wrapper.hpp" @@ -152,6 +154,7 @@ static int32_t axon_subscribe( if (options_json && strlen(options_json) > 0) { try { nlohmann::json opts = nlohmann::json::parse(options_json); +#ifdef AXON_ENABLE_DEPTH_COMPRESSION if (opts.contains("depth_compression")) { auto dc = opts["depth_compression"]; DepthCompressionConfig dc_config; @@ -165,6 +168,16 @@ static int32_t axon_subscribe( dc_config.level.c_str() ); } +#else + (void)options; // Suppress unused warning when depth compression is disabled + if (opts.contains("depth_compression") && opts["depth_compression"].value("enabled", false)) { + RCUTILS_LOG_WARN( + "Depth compression requested for %s but not enabled at build time. " + "Rebuild with -DAXON_ENABLE_DEPTH_COMPRESSION=ON to enable.", + topic_name + ); + } +#endif } catch (const std::exception& e) { RCUTILS_LOG_WARN("Failed to parse options JSON for %s: %s", topic_name, e.what()); } From 9799b4d7d4a0ba42e7d64c124b3906f87ff65e9e Mon Sep 17 00:00:00 2001 From: Liangwei XU Date: Tue, 3 Feb 2026 00:28:30 +0800 Subject: [PATCH 11/21] fix(filters): avoid dlz.hpp namespace pollution with wrapper enums --- Makefile | 13 +++--- REUSE.toml | 6 +++ .../filters/include/depth_compressor.hpp | 35 ++++++++++------ middlewares/filters/src/depth_compressor.cpp | 29 +++++++------ .../filters/test/test_depth_compressor.cpp | 41 ++++++++++--------- .../ros1/include/depth_compression_filter.hpp | 32 +++++++-------- .../include/ros1_subscription_wrapper.hpp | 26 +++++++----- .../ros1/src/depth_compression_filter.cpp | 10 ++--- .../ros2/include/depth_compression_filter.hpp | 32 +++++++-------- .../include/ros2_subscription_wrapper.hpp | 32 ++++++++------- .../ros2/src/depth_compression_filter.cpp | 10 ++--- 11 files changed, 146 insertions(+), 120 deletions(-) diff --git a/Makefile b/Makefile index 2f95c29..a08a767 100644 --- a/Makefile +++ b/Makefile @@ -625,10 +625,11 @@ format: \( -name "*.cpp" -o -name "*.hpp" -o -name "*.h" -o -name "*.c" \) \ ! -path "*/build/*" ! -path "*/build_*/*" -print0 2>/dev/null | \ xargs -0 $(CLANG_FORMAT) -i - @printf "%s\n" "$(YELLOW) Formatting middlewares/ros1/, ros2/, and zenoh/ code...$(NC)" - @find middlewares/ros1 middlewares/ros2 middlewares/zenoh \ + @printf "%s\n" "$(YELLOW) Formatting middlewares/ code...$(NC)" + @find middlewares/ros1 middlewares/ros2 middlewares/zenoh middlewares/filters \ \( -name "*.cpp" -o -name "*.hpp" -o -name "*.h" -o -name "*.c" \) \ - ! -path "*/build/*" ! -path "*/install/*" ! -path "*/devel/*" -print0 2>/dev/null | \ + ! -path "*/build/*" ! -path "*/install/*" ! -path "*/devel/*" \ + ! -path "*/depthlitez/*" -print0 2>/dev/null | \ xargs -0 $(CLANG_FORMAT) -i @printf "%s\n" "$(YELLOW) Formatting apps/ code...$(NC)" @find apps \ @@ -641,12 +642,13 @@ format: lint: @printf "%s\n" "$(YELLOW)Linting C++ code...$(NC)" @if command -v cppcheck >/dev/null 2>&1; then \ - find core middlewares/ros1 middlewares/ros2 middlewares/zenoh apps \ + find core middlewares/ros1 middlewares/ros2 middlewares/zenoh middlewares/filters apps \ \( -name "*.cpp" -o -name "*.hpp" -o -name "*.h" -o -name "*.c" \) \ ! -path "*/build/*" \ ! -path "*/test/*" \ ! -path "*/install/*" \ ! -path "*/devel/*" \ + ! -path "*/depthlitez/*" \ -print0 2>/dev/null | \ xargs -0 cppcheck --enable=all \ --suppress=missingInclude \ @@ -1161,9 +1163,10 @@ format-ci: fi @printf "%s\n" "$(YELLOW)Checking code formatting...$(NC)" @printf "%s\n" "$(YELLOW) Using: $(CLANG_FORMAT)$(NC)" - @find core middlewares/ros1 middlewares/ros2 apps \ + @find core middlewares/ros1 middlewares/ros2 middlewares/filters apps \ \( -name "*.cpp" -o -name "*.hpp" -o -name "*.h" -o -name "*.c" \) \ ! -path "*/build/*" ! -path "*/build_*/*" ! -path "*/install/*" ! -path "*/devel/*" \ + ! -path "*/depthlitez/*" \ -print0 2>/dev/null | \ xargs -0 $(CLANG_FORMAT) --dry-run --Werror > /dev/null 2>&1 || \ (printf "%s\n" "$(RED)✗ Code formatting check failed$(NC)" && \ diff --git a/REUSE.toml b/REUSE.toml index 455c3cf..2b732ba 100644 --- a/REUSE.toml +++ b/REUSE.toml @@ -102,6 +102,12 @@ precedence = "override" SPDX-FileCopyrightText = "Copyright (c) 2026 ArcheBase" SPDX-License-Identifier = "MulanPSL-2.0" +[[annotations]] +path = ".gitmodules" +precedence = "override" +SPDX-FileCopyrightText = "Copyright (c) 2026 ArcheBase" +SPDX-License-Identifier = "MulanPSL-2.0" + [[annotations]] path = "codecov.yml" precedence = "override" diff --git a/middlewares/filters/include/depth_compressor.hpp b/middlewares/filters/include/depth_compressor.hpp index 1fa21c2..56e8472 100644 --- a/middlewares/filters/include/depth_compressor.hpp +++ b/middlewares/filters/include/depth_compressor.hpp @@ -1,14 +1,26 @@ +// SPDX-FileCopyrightText: 2026 ArcheBase +// +// SPDX-License-Identifier: MulanPSL-2.0 + #pragma once #include #include #include -#include - namespace axon { namespace depth { +/** + * @brief Compression level for depth compression + */ +enum class CompressionLevel : int { kFast = 0, kMedium = 1, kMax = 2 }; + +/** + * @brief Threading mode for compression + */ +enum class Threading : int { kSingle = 0, kMulti = 1, kAuto = 2 }; + /** * @brief Depth image compressor using DepthLiteZ library * @@ -22,9 +34,9 @@ class DepthCompressor { * @brief Compression configuration */ struct Config { - dlz::CompressionLevel level = dlz::CompressionLevel::kMedium; - dlz::Threading threading = dlz::Threading::kAuto; - int num_threads = 0; // 0 = auto-detect + CompressionLevel level = CompressionLevel::kMedium; + Threading threading = Threading::kAuto; + int num_threads = 0; // 0 = auto-detect std::string encoding = "16UC1"; // Input encoding format }; @@ -38,10 +50,7 @@ class DepthCompressor { * @return true if compression succeeded */ bool compress( - const uint8_t* depth_data, - size_t width, - size_t height, - std::vector& compressed_data + const uint8_t* depth_data, size_t width, size_t height, std::vector& compressed_data ); /** @@ -60,11 +69,13 @@ class DepthCompressor { /** * @brief Get current configuration */ - const Config& get_config() const { return config_; } + const Config& get_config() const { + return config_; + } private: Config config_; }; -} // namespace depth -} // namespace axon +} // namespace depth +} // namespace axon diff --git a/middlewares/filters/src/depth_compressor.cpp b/middlewares/filters/src/depth_compressor.cpp index b7356e7..7e85279 100644 --- a/middlewares/filters/src/depth_compressor.cpp +++ b/middlewares/filters/src/depth_compressor.cpp @@ -1,15 +1,17 @@ +// SPDX-FileCopyrightText: 2026 ArcheBase +// +// SPDX-License-Identifier: MulanPSL-2.0 + #include "depth_compressor.hpp" #include +#include namespace axon { namespace depth { bool DepthCompressor::compress( - const uint8_t* depth_data, - size_t width, - size_t height, - std::vector& compressed_data + const uint8_t* depth_data, size_t width, size_t height, std::vector& compressed_data ) { if (!depth_data || width == 0 || height == 0) { return false; @@ -29,8 +31,8 @@ bool DepthCompressor::compress( compressed_buffer, 0, compressed_size, - config_.level, - config_.threading, + static_cast(config_.level), + static_cast(config_.threading), config_.num_threads ); @@ -39,21 +41,18 @@ bool DepthCompressor::compress( } // Copy to output vector - compressed_data.assign( - compressed_buffer.begin(), - compressed_buffer.begin() + compressed_size - ); + compressed_data.assign(compressed_buffer.begin(), compressed_buffer.begin() + compressed_size); return true; } std::string DepthCompressor::get_compression_format() const { switch (config_.level) { - case dlz::CompressionLevel::kFast: + case CompressionLevel::kFast: return "dlz_fast"; - case dlz::CompressionLevel::kMedium: + case CompressionLevel::kMedium: return "dlz_medium"; - case dlz::CompressionLevel::kMax: + case CompressionLevel::kMax: return "dlz_max"; default: return "dlz_medium"; @@ -64,5 +63,5 @@ void DepthCompressor::set_config(const Config& config) { config_ = config; } -} // namespace depth -} // namespace axon +} // namespace depth +} // namespace axon diff --git a/middlewares/filters/test/test_depth_compressor.cpp b/middlewares/filters/test/test_depth_compressor.cpp index c5f4195..86d7aa1 100644 --- a/middlewares/filters/test/test_depth_compressor.cpp +++ b/middlewares/filters/test/test_depth_compressor.cpp @@ -1,7 +1,11 @@ -#include +// SPDX-FileCopyrightText: 2026 Copyright (c) 2026 ArcheBase +// +// SPDX-License-Identifier: MulanPSL-2.0 #include +#include + namespace axon { namespace depth { namespace test { @@ -64,15 +68,15 @@ TEST_F(DepthCompressorTest, CompressionRatio) { // Test compression format strings TEST_F(DepthCompressorTest, CompressionFormatStrings) { - config_.level = dlz::CompressionLevel::kFast; + config_.level = axon::depth::CompressionLevel::kFast; compressor_.set_config(config_); EXPECT_EQ(compressor_.get_compression_format(), "dlz_fast"); - config_.level = dlz::CompressionLevel::kMedium; + config_.level = axon::depth::CompressionLevel::kMedium; compressor_.set_config(config_); EXPECT_EQ(compressor_.get_compression_format(), "dlz_medium"); - config_.level = dlz::CompressionLevel::kMax; + config_.level = axon::depth::CompressionLevel::kMax; compressor_.set_config(config_); EXPECT_EQ(compressor_.get_compression_format(), "dlz_max"); } @@ -100,9 +104,10 @@ TEST_F(DepthCompressorTest, DifferentCompressionLevels) { std::vector compressed_sizes; - for (auto level : {dlz::CompressionLevel::kFast, - dlz::CompressionLevel::kMedium, - dlz::CompressionLevel::kMax}) { + for (auto level : + {axon::depth::CompressionLevel::kFast, + axon::depth::CompressionLevel::kMedium, + axon::depth::CompressionLevel::kMax}) { config_.level = level; compressor_.set_config(config_); @@ -120,10 +125,7 @@ TEST_F(DepthCompressorTest, DifferentCompressionLevels) { // Test different image sizes TEST_F(DepthCompressorTest, DifferentImageSizes) { std::vector> sizes = { - {320, 240}, - {640, 480}, - {1280, 720}, - {1920, 1080} + {320, 240}, {640, 480}, {1280, 720}, {1920, 1080} }; for (const auto& [width, height] : sizes) { @@ -133,28 +135,27 @@ TEST_F(DepthCompressorTest, DifferentImageSizes) { ASSERT_TRUE(compressor_.compress(depth_data.data(), width, height, compressed)) << "Failed for size: " << width << "x" << height; - EXPECT_GT(compressed.size(), 0) - << "Empty output for size: " << width << "x" << height; + EXPECT_GT(compressed.size(), 0) << "Empty output for size: " << width << "x" << height; } } // Test config getter/setter TEST_F(DepthCompressorTest, ConfigGetterSetter) { DepthCompressor::Config new_config; - new_config.level = dlz::CompressionLevel::kMax; - new_config.threading = dlz::Threading::kMulti; + new_config.level = axon::depth::CompressionLevel::kMax; + new_config.threading = axon::depth::Threading::kMulti; new_config.num_threads = 4; new_config.encoding = "16UC1"; compressor_.set_config(new_config); const auto& retrieved_config = compressor_.get_config(); - EXPECT_EQ(retrieved_config.level, dlz::CompressionLevel::kMax); - EXPECT_EQ(retrieved_config.threading, dlz::Threading::kMulti); + EXPECT_EQ(retrieved_config.level, axon::depth::CompressionLevel::kMax); + EXPECT_EQ(retrieved_config.threading, axon::depth::Threading::kMulti); EXPECT_EQ(retrieved_config.num_threads, 4); EXPECT_EQ(retrieved_config.encoding, "16UC1"); } -} // namespace test -} // namespace depth -} // namespace axon +} // namespace test +} // namespace depth +} // namespace axon diff --git a/middlewares/ros1/include/depth_compression_filter.hpp b/middlewares/ros1/include/depth_compression_filter.hpp index 35aa792..bec56e8 100644 --- a/middlewares/ros1/include/depth_compression_filter.hpp +++ b/middlewares/ros1/include/depth_compression_filter.hpp @@ -13,7 +13,7 @@ namespace ros1_plugin { /** - * @brief 深度压缩配置 + * @brief Depth compression configuration */ struct DepthCompressionConfig { bool enabled = false; @@ -21,15 +21,15 @@ struct DepthCompressionConfig { }; /** - * @brief 深度图像压缩过滤器 + * @brief Depth image compression filter * - * 在 ROS1 插件中使用，将 sensor_msgs/Image (16UC1) 压缩为 - * sensor_msgs/CompressedImage 格式。 + * Used in ROS1 plugin to compress sensor_msgs/Image (16UC1) to + * sensor_msgs/CompressedImage format. */ class DepthCompressionFilter { public: /** - * @brief 处理后的消息回调类型 + * @brief Processed message callback type */ using ProcessedCallback = std::function& data, @@ -37,19 +37,19 @@ class DepthCompressionFilter { )>; /** - * @brief 构造函数 - * @param config 深度压缩配置 + * @brief Constructor + * @param config Depth compression configuration */ explicit DepthCompressionFilter(const DepthCompressionConfig& config); /** - * @brief 过滤并处理消息 + * @brief Filter and process message * - * @param topic 主题名称 - * @param message_type 消息类型 - * @param data 原始消息数据（ROS 序列化的 sensor_msgs/Image） - * @param timestamp 时间戳（纳秒） - * @param callback 处理后的消息回调 + * @param topic Topic name + * @param message_type Message type + * @param data Raw message data (ROS serialized sensor_msgs/Image) + * @param timestamp Timestamp in nanoseconds + * @param callback Processed message callback */ void filter_and_process( const std::string& topic, const std::string& message_type, const std::vector& data, @@ -61,15 +61,15 @@ class DepthCompressionFilter { axon::depth::DepthCompressor compressor_; /** - * @brief 从 ROS Image 消息中提取深度数据 - * @return 提取的深度数据指针，宽度，高度 + * @brief Extract depth data from ROS Image message + * @return Tuple of depth data pointer, width, height */ std::tuple extract_depth_data( const std::vector& image_msg ); /** - * @brief 构建 CompressedImage 消息 + * @brief Build CompressedImage message */ std::vector build_compressed_image_msg( const std::string& format, const std::vector& compressed_data, uint64_t timestamp diff --git a/middlewares/ros1/include/ros1_subscription_wrapper.hpp b/middlewares/ros1/include/ros1_subscription_wrapper.hpp index c60313d..fadafbc 100644 --- a/middlewares/ros1/include/ros1_subscription_wrapper.hpp +++ b/middlewares/ros1/include/ros1_subscription_wrapper.hpp @@ -15,18 +15,28 @@ #include #include +#ifdef AXON_ENABLE_DEPTH_COMPRESSION +// Include depth compression filter for DepthCompressionConfig definition +// This is safe here because dlz.hpp is only included in .cpp files +#include "depth_compression_filter.hpp" +#else +// When depth compression is disabled, define a minimal config struct +namespace ros1_plugin { +struct DepthCompressionConfig { + bool enabled = false; + std::string level = "medium"; +}; +} // namespace ros1_plugin +#endif + namespace ros1_plugin { // ============================================================================= // Depth Compression Support (conditional compilation) // ============================================================================= #ifdef AXON_ENABLE_DEPTH_COMPRESSION - -// Forward declarations -struct DepthCompressionConfig; class DepthCompressionFilter; - -#endif // AXON_ENABLE_DEPTH_COMPRESSION +#endif // Message callback type - passes raw serialized message data using MessageCallback = std::function& data, @@ -37,19 +37,19 @@ class DepthCompressionFilter { )>; /** - * @brief 构造函数 - * @param config 深度压缩配置 + * @brief Constructor + * @param config Depth compression configuration */ explicit DepthCompressionFilter(const DepthCompressionConfig& config); /** - * @brief 过滤并处理消息 + * @brief Filter and process message * - * @param topic 主题名称 - * @param message_type 消息类型 - * @param data 原始消息数据（CDR 序列化的 sensor_msgs::msg::Image） - * @param timestamp_ns 时间戳（纳秒） - * @param callback 处理后的消息回调 + * @param topic Topic name + * @param message_type Message type + * @param data Raw message data (CDR serialized sensor_msgs::msg::Image) + * @param timestamp_ns Timestamp in nanoseconds + * @param callback Processed message callback */ void filter_and_process( const std::string& topic, const std::string& message_type, const std::vector& data, @@ -61,15 +61,15 @@ class DepthCompressionFilter { axon::depth::DepthCompressor compressor_; /** - * @brief 从 ROS Image 消息中提取深度数据 - * @return 提取的深度数据指针，宽度，高度 + * @brief Extract depth data from ROS Image message + * @return Tuple of depth data pointer, width, height */ std::tuple extract_depth_data( const std::vector& image_msg ); /** - * @brief 构建 CompressedImage 消息 + * @brief Build CompressedImage message */ std::vector build_compressed_image_msg( const std::string& format, const std::vector& compressed_data, uint64_t timestamp_ns diff --git a/middlewares/ros2/include/ros2_subscription_wrapper.hpp b/middlewares/ros2/include/ros2_subscription_wrapper.hpp index 0efd98d..ac5b01c 100644 --- a/middlewares/ros2/include/ros2_subscription_wrapper.hpp +++ b/middlewares/ros2/include/ros2_subscription_wrapper.hpp @@ -15,26 +15,28 @@ #include #include -namespace ros2_plugin { - -// ============================================================================= -// Depth Compression Support (conditional compilation) -// ============================================================================= #ifdef AXON_ENABLE_DEPTH_COMPRESSION - -#include - +// Include depth compression filter for DepthCompressionConfig definition +// This is safe here because dlz.hpp is only included in .cpp files +#include "depth_compression_filter.hpp" #else - -// Forward declarations when depth compression is disabled +// When depth compression is disabled, define a minimal config struct +namespace ros2_plugin { struct DepthCompressionConfig { bool enabled = false; std::string level = "medium"; }; +} // namespace ros2_plugin +#endif -class DepthCompressionFilter; +namespace ros2_plugin { -#endif // AXON_ENABLE_DEPTH_COMPRESSION +// ============================================================================= +// Depth Compression Support (conditional compilation) +// ============================================================================= +#ifdef AXON_ENABLE_DEPTH_COMPRESSION +class DepthCompressionFilter; +#endif // Message callback type - passes raw serialized message data using MessageCallback = std::function; /** - * 订阅选项 + * @brief Subscription options */ struct SubscribeOptions { rclcpp::QoS qos; - std::optional depth_compression; // 深度压缩配置（可选） + std::optional depth_compression; // Depth compression config (optional) - // 默认构造函数 + // Default constructor SubscribeOptions() : qos(10) {} }; diff --git a/middlewares/ros2/src/depth_compression_filter.cpp b/middlewares/ros2/src/depth_compression_filter.cpp index ec4852f..7daae88 100644 --- a/middlewares/ros2/src/depth_compression_filter.cpp +++ b/middlewares/ros2/src/depth_compression_filter.cpp @@ -17,12 +17,12 @@ DepthCompressionFilter::DepthCompressionFilter(const DepthCompressionConfig& con : config_(config) { // Configure compressor axon::depth::DepthCompressor::Config compressor_config; - if (config.level == "fast") { - compressor_config.level = dlz::CompressionLevel::kFast; - } else if (config.level == "max") { - compressor_config.level = dlz::CompressionLevel::kMax; + if (config_.level == "fast") { + compressor_config.level = axon::depth::CompressionLevel::kFast; + } else if (config_.level == "max") { + compressor_config.level = axon::depth::CompressionLevel::kMax; } else { - compressor_config.level = dlz::CompressionLevel::kMedium; + compressor_config.level = axon::depth::CompressionLevel::kMedium; } compressor_.set_config(compressor_config); } From 2a331e89be3833ab9862d905c04c8e66b6fc2300 Mon Sep 17 00:00:00 2001 From: Liangwei XU Date: Tue, 3 Feb 2026 09:46:46 +0800 Subject: [PATCH 12/21] fix(docs): fix version number --- CHANGELOG | 8 ++++---- CMakeLists.txt | 2 +- apps/axon_recorder/CMakeLists.txt | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/CHANGELOG b/CHANGELOG index ed69677..c6f5475 100644 --- a/CHANGELOG +++ b/CHANGELOG @@ -12,7 +12,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ## [Unreleased] -## [0.3.0] - 2025-02-02 +## [0.2.1] - 2025-02-03 ### Added - **filters**: Implement depth image compression filter (opt-in feature) @@ -32,7 +32,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - Users with access can initialize: `git submodule update --init --recursive middlewares/filters/depthlitez` ### Changed -- **version**: Update project version to 0.3.0 +- **version**: Update project version to 0.2.1 - **build**: Make depth compression an opt-in feature via `AXON_ENABLE_DEPTH_COMPRESSION` CMake option - Default OFF for open-source compatibility - When OFF, depth compression code is excluded from build @@ -141,8 +141,8 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 --- -[Unreleased]: https://github.com/ArcheBase/Axon/compare/v0.3.0...HEAD -[0.3.0]: https://github.com/ArcheBase/Axon/compare/v0.2.0...v0.3.0 +[Unreleased]: https://github.com/ArcheBase/Axon/compare/v0.2.1...HEAD +[0.2.1]: https://github.com/ArcheBase/Axon/compare/v0.2.0...v0.2.1 [0.2.0]: https://github.com/ArcheBase/Axon/compare/v0.1.0...v0.2.0 [0.1.0]: https://github.com/ArcheBase/Axon/compare/v0.0.1...v0.1.0 [0.0.1]: https://github.com/ArcheBase/Axon/releases/tag/v0.0.1 diff --git a/CMakeLists.txt b/CMakeLists.txt index 434880d..39c24fc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,7 +14,7 @@ # ============================================================================= cmake_minimum_required(VERSION 3.14) -project(Axon VERSION 0.3.0 LANGUAGES CXX) +project(Axon VERSION 0.2.1 LANGUAGES CXX) # ============================================================================= # Centralized Path Definitions diff --git a/apps/axon_recorder/CMakeLists.txt b/apps/axon_recorder/CMakeLists.txt index fc283f0..593bc03 100644 --- a/apps/axon_recorder/CMakeLists.txt +++ b/apps/axon_recorder/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.12) -project(axon_recorder VERSION 0.3.0) +project(axon_recorder VERSION 0.2.1) # Use parent project version if available if(DEFINED AXON_REPO_ROOT AND EXISTS "${AXON_REPO_ROOT}/CMakeLists.txt") From 0565a0b2bdaf1487bee8bcbaaad7b962f536cf0e Mon Sep 17 00:00:00 2001 From: Liangwei XU Date: Tue, 3 Feb 2026 13:11:37 +0800 Subject: [PATCH 13/21] test(recorder): fix RegisterChannel test to use get_or_create_channel --- apps/axon_recorder/test/unit/test_recording_session.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/apps/axon_recorder/test/unit/test_recording_session.cpp b/apps/axon_recorder/test/unit/test_recording_session.cpp index 66711ba..6be040b 100644 --- a/apps/axon_recorder/test/unit/test_recording_session.cpp +++ b/apps/axon_recorder/test/unit/test_recording_session.cpp @@ -224,12 +224,15 @@ TEST_F(RecordingSessionTest, RegisterChannel) { ASSERT_NE(schema_id, 0); std::string topic = "/test/topic"; - std::string encoding = "cdr"; std::string message_type = "std_msgs/msg/String"; - uint16_t channel_id = session.register_channel(topic, message_type, encoding, schema_id); + + // Use get_or_create_channel which handles composite key lookup + uint16_t channel_id = session.get_or_create_channel(topic, message_type); EXPECT_NE(channel_id, 0); - EXPECT_EQ(session.get_channel_id(topic), channel_id); + // Verify it returns the same ID on second call + uint16_t channel_id2 = session.get_or_create_channel(topic, message_type); + EXPECT_EQ(channel_id2, channel_id); } TEST_F(RecordingSessionTest, RegisterChannelFailsWhenNotOpen) { From 37185a9ec56d3f42992ab75d8ffef9e58618d8e4 Mon Sep 17 00:00:00 2001 From: Liangwei XU Date: Tue, 3 Feb 2026 13:16:50 +0800 Subject: [PATCH 14/21] fix(recorder): use null character as composite key separator --- apps/axon_recorder/recording_session.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/apps/axon_recorder/recording_session.cpp b/apps/axon_recorder/recording_session.cpp index 0271390..00b6dcc 100644 --- a/apps/axon_recorder/recording_session.cpp +++ b/apps/axon_recorder/recording_session.cpp @@ -13,6 +13,10 @@ namespace axon { namespace recorder { +// ROS topic names cannot contain null characters (per ROS naming rules) +// Using '\0' as separator ensures no collision with valid topic/message_type names +static constexpr char COMPOSITE_KEY_SEPARATOR = '\0'; + RecordingSession::RecordingSession() : writer_(std::make_unique()) {} @@ -162,7 +166,7 @@ uint16_t RecordingSession::register_channel( } // Create a composite key for topic+message_type - std::string composite_key = topic + "\n" + message_type; + std::string composite_key = topic + COMPOSITE_KEY_SEPARATOR + message_type; // Check if already registered { @@ -217,7 +221,7 @@ uint16_t RecordingSession::get_or_create_channel( const std::string& topic, const std::string& message_type ) { // Create a composite key for topic+message_type - std::string composite_key = topic + "\n" + message_type; + std::string composite_key = topic + COMPOSITE_KEY_SEPARATOR + message_type; // Check if channel already exists for this topic+type combination { From eabb78da70c7242003e09811b28aa5ff8a58de8d Mon Sep 17 00:00:00 2001 From: Liangwei XU Date: Tue, 3 Feb 2026 13:20:57 +0800 Subject: [PATCH 15/21] fix(ros2): capture compression filter to avoid race condition --- .../ros2/src/ros2_subscription_wrapper.cpp | 22 +++++++++++-------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/middlewares/ros2/src/ros2_subscription_wrapper.cpp b/middlewares/ros2/src/ros2_subscription_wrapper.cpp index 971375b..c3fac50 100644 --- a/middlewares/ros2/src/ros2_subscription_wrapper.cpp +++ b/middlewares/ros2/src/ros2_subscription_wrapper.cpp @@ -60,12 +60,20 @@ bool SubscriptionManager::subscribe( #endif // Create generic subscription using ROS2's built-in API - // Capture topic_name by value and lookup filter in subscriptions_ map when message arrives + // Capture compression_filter by value to avoid race condition with subscriptions_ map +#ifdef AXON_ENABLE_DEPTH_COMPRESSION + auto* captured_filter = compression_filter.get(); +#else + auto* captured_filter = nullptr; +#endif + auto subscription = node_->create_generic_subscription( topic_name, message_type, options.qos, - [this, topic_name, message_type, callback](std::shared_ptr msg) { + [this, topic_name, message_type, callback, captured_filter]( + std::shared_ptr msg + ) { if (!callback) { return; } @@ -82,13 +90,9 @@ bool SubscriptionManager::subscribe( rclcpp::Time timestamp = rclcpp::Clock(RCL_SYSTEM_TIME).now(); #ifdef AXON_ENABLE_DEPTH_COMPRESSION - // Look up compression filter (if exists) - auto it = subscriptions_.find(topic_name); - bool has_filter = - (it != subscriptions_.end() && it->second.compression_filter != nullptr); - - if (has_filter) { - it->second.compression_filter->filter_and_process( + // Use captured filter pointer instead of accessing subscriptions_ map + if (captured_filter) { + captured_filter->filter_and_process( topic_name, message_type, data, From 5723e3c0e2a92286a5c32790535f57a1b1461c1c Mon Sep 17 00:00:00 2001 From: Liangwei XU Date: Tue, 3 Feb 2026 13:29:46 +0800 Subject: [PATCH 16/21] fix(ros2,filters): address code review comments --- middlewares/filters/src/depth_compressor.cpp | 2 ++ middlewares/ros2/CMakeLists.txt | 9 ++--- .../ros2/src/ros2_subscription_wrapper.cpp | 35 ++++++++++--------- 3 files changed, 22 insertions(+), 24 deletions(-) diff --git a/middlewares/filters/src/depth_compressor.cpp b/middlewares/filters/src/depth_compressor.cpp index 7e85279..4192aa2 100644 --- a/middlewares/filters/src/depth_compressor.cpp +++ b/middlewares/filters/src/depth_compressor.cpp @@ -22,6 +22,8 @@ bool DepthCompressor::compress( int num_pixels = static_cast(width * height); // Compress using dlz library + // Note: dlz::CompressEx takes non-const pointer but doesn't modify input (verified by inspection) + // The const_cast is safe here - dlz only reads from the input buffer std::vector compressed_buffer; size_t compressed_size; diff --git a/middlewares/ros2/CMakeLists.txt b/middlewares/ros2/CMakeLists.txt index d849ed1..6cf3844 100644 --- a/middlewares/ros2/CMakeLists.txt +++ b/middlewares/ros2/CMakeLists.txt @@ -105,13 +105,8 @@ target_include_directories(axon_ros2_plugin PUBLIC target_link_libraries(axon_ros2_plugin PUBLIC rclcpp::rclcpp nlohmann_json::nlohmann_json -) - -# Use ament_target_dependencies for ROS2 dependencies (handles all includes and linking) -ament_target_dependencies(axon_ros2_plugin PUBLIC - rclcpp - sensor_msgs - std_msgs + ${sensor_msgs_TARGETS} + ${std_msgs_TARGETS} ) # Link filters library for depth compression (conditional) diff --git a/middlewares/ros2/src/ros2_subscription_wrapper.cpp b/middlewares/ros2/src/ros2_subscription_wrapper.cpp index c3fac50..7b71855 100644 --- a/middlewares/ros2/src/ros2_subscription_wrapper.cpp +++ b/middlewares/ros2/src/ros2_subscription_wrapper.cpp @@ -64,7 +64,7 @@ bool SubscriptionManager::subscribe( #ifdef AXON_ENABLE_DEPTH_COMPRESSION auto* captured_filter = compression_filter.get(); #else - auto* captured_filter = nullptr; + void* captured_filter = nullptr; // No compression filter available #endif auto subscription = node_->create_generic_subscription( @@ -92,22 +92,23 @@ bool SubscriptionManager::subscribe( #ifdef AXON_ENABLE_DEPTH_COMPRESSION // Use captured filter pointer instead of accessing subscriptions_ map if (captured_filter) { - captured_filter->filter_and_process( - topic_name, - message_type, - data, - timestamp.nanoseconds(), - [&]( - const std::string& filtered_topic, - const std::string& filtered_type, - const std::vector& filtered_data, - uint64_t filtered_timestamp_ns - ) { - // Convert nanosecond timestamp back to rclcpp::Time - rclcpp::Time filtered_timestamp(filtered_timestamp_ns); - callback(filtered_topic, filtered_type, filtered_data, filtered_timestamp); - } - ); + static_cast(captured_filter) + ->filter_and_process( + topic_name, + message_type, + data, + timestamp.nanoseconds(), + [&]( + const std::string& filtered_topic, + const std::string& filtered_type, + const std::vector& filtered_data, + uint64_t filtered_timestamp_ns + ) { + // Convert nanosecond timestamp back to rclcpp::Time + rclcpp::Time filtered_timestamp(filtered_timestamp_ns); + callback(filtered_topic, filtered_type, filtered_data, filtered_timestamp); + } + ); } else { // Direct callback callback(topic_name, message_type, data, timestamp); From df4b1a598a833e220c0c7ddc5b9f9d6dc88f5340 Mon Sep 17 00:00:00 2001 From: Liangwei XU Date: Tue, 3 Feb 2026 13:41:50 +0800 Subject: [PATCH 17/21] refactor(recorder): fix get_channel_id to use composite key lookup --- apps/axon_recorder/recording_session.cpp | 7 +++++-- apps/axon_recorder/recording_session.hpp | 5 +++-- apps/axon_recorder/test/unit/test_recording_session.cpp | 2 +- 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/apps/axon_recorder/recording_session.cpp b/apps/axon_recorder/recording_session.cpp index 00b6dcc..7fae0f7 100644 --- a/apps/axon_recorder/recording_session.cpp +++ b/apps/axon_recorder/recording_session.cpp @@ -211,9 +211,12 @@ bool RecordingSession::write( return success; } -uint16_t RecordingSession::get_channel_id(const std::string& topic) const { +uint16_t RecordingSession::get_channel_id( + const std::string& topic, const std::string& message_type +) const { + std::string composite_key = topic + COMPOSITE_KEY_SEPARATOR + message_type; std::lock_guard lock(registry_mutex_); - auto it = topic_channel_ids_.find(topic); + auto it = topic_channel_ids_.find(composite_key); return (it != topic_channel_ids_.end()) ? it->second : 0; } diff --git a/apps/axon_recorder/recording_session.hpp b/apps/axon_recorder/recording_session.hpp index c53a2dd..10c27af 100644 --- a/apps/axon_recorder/recording_session.hpp +++ b/apps/axon_recorder/recording_session.hpp @@ -134,12 +134,13 @@ class RecordingSession { ); /** - * Get the channel ID for a topic. + * Get the channel ID for a topic with a specific message type. * * @param topic Topic name + * @param message_type Message type name * @return Channel ID, or 0 if not registered */ - uint16_t get_channel_id(const std::string& topic) const; + uint16_t get_channel_id(const std::string& topic, const std::string& message_type) const; /** * Get or create a channel ID for a topic with a specific message type. diff --git a/apps/axon_recorder/test/unit/test_recording_session.cpp b/apps/axon_recorder/test/unit/test_recording_session.cpp index 6be040b..bde57c7 100644 --- a/apps/axon_recorder/test/unit/test_recording_session.cpp +++ b/apps/axon_recorder/test/unit/test_recording_session.cpp @@ -608,7 +608,7 @@ TEST_F(RecordingSessionTest, GetChannelIdForUnregisteredTopic) { McapWriterOptions options; session.open(path, options); - uint16_t channel_id = session.get_channel_id("/unregistered/topic"); + uint16_t channel_id = session.get_channel_id("/unregistered/topic", "std_msgs/msg/String"); EXPECT_EQ(channel_id, 0); } From 42e9b540e5bc3c7d44358f2c81fcd9e782283f1b Mon Sep 17 00:00:00 2001 From: Liangwei XU Date: Tue, 3 Feb 2026 14:12:16 +0800 Subject: [PATCH 18/21] fix(docker): use separate build dirs for zenoh publisher/subscriber --- docker/docker-compose.zenoh.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docker/docker-compose.zenoh.yml b/docker/docker-compose.zenoh.yml index d651fe0..238b25f 100644 --- a/docker/docker-compose.zenoh.yml +++ b/docker/docker-compose.zenoh.yml @@ -37,7 +37,7 @@ services: condition: service_healthy network_mode: host command: > - bash -c "cd /workspace/axon && mkdir -p build && cd build && cmake .. -DAXON_BUILD_ZENOH_PLUGIN=ON -DAXON_BUILD_ROS1_PLUGIN=OFF -DAXON_BUILD_ROS2_PLUGIN=OFF && make -j$(nproc) && echo 'Running test publisher...' && ./middlewares/zenoh_plugin/test_publisher tcp/127.0.0.1:7447 demo/test/data 10 1000" + bash -c "cd /workspace/axon && mkdir -p build/publisher && cmake -S /workspace/axon -B build/publisher -DAXON_BUILD_ZENOH_PLUGIN=ON -DAXON_BUILD_ROS1_PLUGIN=OFF -DAXON_BUILD_ROS2_PLUGIN=OFF && cmake --build build/publisher -j$$(nproc) && echo 'Running test publisher...' && build/publisher/middlewares/zenoh_plugin/test_publisher tcp/127.0.0.1:7447 demo/test/data 10 1000" # Test Subscriber - Verifies message reception test-subscriber: @@ -57,4 +57,4 @@ services: condition: service_started network_mode: host command: > - bash -c "cd /workspace/axon && mkdir -p build && cd build && cmake .. -DAXON_BUILD_ZENOH_PLUGIN=ON -DAXON_BUILD_ROS1_PLUGIN=OFF -DAXON_BUILD_ROS2_PLUGIN=OFF && make -j$(nproc) && echo 'Running test subscriber...' && ./middlewares/zenoh_plugin/test_subscriber tcp/127.0.0.1:7447 'demo/test/**' 10 30" + bash -c "cd /workspace/axon && mkdir -p build/subscriber && cmake -S /workspace/axon -B build/subscriber -DAXON_BUILD_ZENOH_PLUGIN=ON -DAXON_BUILD_ROS1_PLUGIN=OFF -DAXON_BUILD_ROS2_PLUGIN=OFF && cmake --build build/subscriber -j$$(nproc) && echo 'Running test subscriber...' && build/subscriber/middlewares/zenoh_plugin/test_subscriber tcp/127.0.0.1:7447 'demo/test/**' 10 30" From a1aa229109c996a6e96e40f3c750f8ba5af5acc2 Mon Sep 17 00:00:00 2001 From: Liangwei XU Date: Tue, 3 Feb 2026 17:04:16 +0800 Subject: [PATCH 19/21] fix(filters): update depthlitez subproject commit reference --- middlewares/filters/depthlitez | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/middlewares/filters/depthlitez b/middlewares/filters/depthlitez index 834e1d0..ff564e8 160000 --- a/middlewares/filters/depthlitez +++ b/middlewares/filters/depthlitez @@ -1 +1 @@ -Subproject commit 834e1d0ac61e609b4c8b1897e20e5c9492729837 +Subproject commit ff564e876634bd4392f220f9785dd1d18a2236a4 From bd91531a3ec4fdee71213f91abf84f5f7d692d2f Mon Sep 17 00:00:00 2001 From: Liangwei XU Date: Tue, 3 Feb 2026 17:04:52 +0800 Subject: [PATCH 20/21] refactor(filters): simplify compression logic and remove unused parameters --- middlewares/filters/src/depth_compressor.cpp | 36 +++++++------------- 1 file changed, 13 insertions(+), 23 deletions(-) diff --git a/middlewares/filters/src/depth_compressor.cpp b/middlewares/filters/src/depth_compressor.cpp index 4192aa2..683c8e1 100644 --- a/middlewares/filters/src/depth_compressor.cpp +++ b/middlewares/filters/src/depth_compressor.cpp @@ -19,46 +19,36 @@ bool DepthCompressor::compress( // Cast to int16_t (16UC1 encoding) const int16_t* depth_data_i16 = reinterpret_cast(depth_data); - int num_pixels = static_cast(width * height); - // Compress using dlz library - // Note: dlz::CompressEx takes non-const pointer but doesn't modify input (verified by inspection) - // The const_cast is safe here - dlz only reads from the input buffer + // Use CompressToFile to include DLZF header with width/height information + // This is necessary because CompressedImage message doesn't contain image dimensions + // Note: dlz::CompressToFile takes non-const pointer but doesn't modify input (verified by + // inspection) The const_cast is safe here - dlz only reads from the input buffer std::vector compressed_buffer; - size_t compressed_size; - bool success = dlz::CompressEx( + bool success = dlz::CompressToFile( const_cast(depth_data_i16), - num_pixels, + static_cast(width), + static_cast(height), compressed_buffer, - 0, - compressed_size, static_cast(config_.level), - static_cast(config_.threading), - config_.num_threads + static_cast(config_.threading) ); if (!success) { return false; } - // Copy to output vector - compressed_data.assign(compressed_buffer.begin(), compressed_buffer.begin() + compressed_size); + // Copy to output vector (includes DLZF header with magic, width, height, level) + compressed_data.assign(compressed_buffer.begin(), compressed_buffer.end()); return true; } std::string DepthCompressor::get_compression_format() const { - switch (config_.level) { - case CompressionLevel::kFast: - return "dlz_fast"; - case CompressionLevel::kMedium: - return "dlz_medium"; - case CompressionLevel::kMax: - return "dlz_max"; - default: - return "dlz_medium"; - } + // DLZF file format: [magic(4)][width(4)][height(4)][level(4)][compressed data...] + // The format is always "dlzf" since the compression level is stored in the header + return "dlzf"; } void DepthCompressor::set_config(const Config& config) { From 48a1a99fa89d9d68ab4767e9d2bcd2745f3d2017 Mon Sep 17 00:00:00 2001 From: Liangwei XU Date: Tue, 3 Feb 2026 21:32:44 +0800 Subject: [PATCH 21/21] fix: fix comments --- apps/axon_recorder/config/default_config_ros2.yaml | 2 +- middlewares/filters/include/depth_compressor.hpp | 2 -- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/apps/axon_recorder/config/default_config_ros2.yaml b/apps/axon_recorder/config/default_config_ros2.yaml index 8cc5132..968dfe7 100644 --- a/apps/axon_recorder/config/default_config_ros2.yaml +++ b/apps/axon_recorder/config/default_config_ros2.yaml @@ -80,7 +80,7 @@ subscriptions: batch_size: 300 flush_interval_ms: 10000 - # Depth image with compression (16UC1 -> CompressedImage with RVL+LZ4) + # Depth image with compression (16UC1 -> CompressedImage) - name: /camera/camera/depth/image_rect_raw message_type: sensor_msgs/msg/Image batch_size: 300 diff --git a/middlewares/filters/include/depth_compressor.hpp b/middlewares/filters/include/depth_compressor.hpp index 56e8472..fdb6af0 100644 --- a/middlewares/filters/include/depth_compressor.hpp +++ b/middlewares/filters/include/depth_compressor.hpp @@ -25,8 +25,6 @@ enum class Threading : int { kSingle = 0, kMulti = 1, kAuto = 2 }; * @brief Depth image compressor using DepthLiteZ library * * Provides compression for 16-bit depth images with configurable quality levels. - * Supports RVL (Run-Length Variable Length) encoding with optional LZ4/ZSTD - * secondary compression. */ class DepthCompressor { public: