From 471f714be7ca2fc1a663d8474b2236c8b07aa4c4 Mon Sep 17 00:00:00 2001 From: James Pace Date: Sat, 8 Nov 2025 08:47:07 -0500 Subject: [PATCH] Track uncommited. --- CMakeLists.txt | 67 +++++++++++++++++++ COLCON_IGNORE | 0 .../costmap_compressor/CostmapCompressor.hpp | 12 ++++ msg/CompressedCostmap.msg | 7 ++ package.xml | 24 +++++++ src/CostmapCompressor.cpp | 45 +++++++++++++ test/costmap_compressor_test.cpp | 28 ++++++++ 7 files changed, 183 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 COLCON_IGNORE create mode 100644 include/costmap_compressor/CostmapCompressor.hpp create mode 100644 msg/CompressedCostmap.msg create mode 100644 package.xml create mode 100644 src/CostmapCompressor.cpp create mode 100644 test/costmap_compressor_test.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..663bb3a --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,67 @@ +cmake_minimum_required(VERSION 3.8) +project(costmap_compressor) + +add_compile_options(-Wall -Wextra -Wpedantic -fsanitize=address) +add_link_options(-fsanitize=address) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(Snappy REQUIRED) +find_package(rclcpp REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/CompressedCostmap.msg" + DEPENDENCIES + "nav2_msgs" + "std_msgs" +) +rosidl_get_typesupport_target(costmap_compressor_msgs ${PROJECT_NAME} "rosidl_typesupport_cpp") + +add_library(CostmapCompressor src/CostmapCompressor.cpp) +add_library(costmap_compressor::CostmapCompressor ALIAS CostmapCompressor) +target_link_libraries(CostmapCompressor PUBLIC + snappy + ${nav2_msgs_TARGETS} + ${costmap_compressor_msgs} +) +target_compile_features(CostmapCompressor PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_include_directories(CostmapCompressor PUBLIC + $ + $) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) +install( + TARGETS CostmapCompressor + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(costmap_compressor_test test/costmap_compressor_test.cpp) + target_include_directories(costmap_compressor_test PUBLIC + $ + $ + ) + target_link_libraries(costmap_compressor_test CostmapCompressor rclcpp::rclcpp) +endif() + +ament_export_include_directories( + "include/${PROJECT_NAME}" +) +ament_export_libraries( + CostmapCompressor +) +ament_export_targets( + export_${PROJECT_NAME} +) + +ament_package() diff --git a/COLCON_IGNORE b/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/include/costmap_compressor/CostmapCompressor.hpp b/include/costmap_compressor/CostmapCompressor.hpp new file mode 100644 index 0000000..f1c88b5 --- /dev/null +++ b/include/costmap_compressor/CostmapCompressor.hpp @@ -0,0 +1,12 @@ +#pragma once + +#include +#include + +namespace costmap_compressor +{ + +costmap_compressor::msg::CompressedCostmap compress(const nav2_msgs::msg::Costmap & costmap); +nav2_msgs::msg::Costmap uncompress(const costmap_compressor::msg::CompressedCostmap & costmap); + +} // namespace costmap_compressor diff --git a/msg/CompressedCostmap.msg b/msg/CompressedCostmap.msg new file mode 100644 index 0000000..c718196 --- /dev/null +++ b/msg/CompressedCostmap.msg @@ -0,0 +1,7 @@ +# A compressed form of nav2_msgs/Costmap +std_msgs/Header header + +# MetaData for the map +nav2_msgs/CostmapMetaData metadata + +uint8[] compressed_data \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..10ddae3 --- /dev/null +++ b/package.xml @@ -0,0 +1,24 @@ + + + + costmap_compressor + 0.0.0 + TODO: Package description + James Pace + MPL-2.0 + + nav2_msgs + std_msgs + rclcpp + rosidl_default_runtime + rosidl_default_generators + ament_cmake_gtest + + ament_cmake_ros + + rosidl_interface_packages + + + ament_cmake + + diff --git a/src/CostmapCompressor.cpp b/src/CostmapCompressor.cpp new file mode 100644 index 0000000..a9c3b59 --- /dev/null +++ b/src/CostmapCompressor.cpp @@ -0,0 +1,45 @@ +#include "costmap_compressor/CostmapCompressor.hpp" + +#include +#include + +namespace costmap_compressor +{ + +costmap_compressor::msg::CompressedCostmap compress( + const nav2_msgs::msg::Costmap & costmap) +{ + costmap_compressor::msg::CompressedCostmap compressed_msg; + compressed_msg.header = costmap.header; + compressed_msg.metadata = costmap.metadata; + + auto snappy_source = snappy::ByteArraySource(reinterpret_cast(costmap.data.data()), costmap.data.size()); + std::vector compressed_array; + compressed_array.reserve(costmap.data.size()); + auto snappy_writer = snappy::UncheckedByteArraySink(reinterpret_cast(compressed_array.data())); + const auto bytes_written = snappy::Compress(&snappy_source, &snappy_writer); + + compressed_msg.compressed_data.insert(compressed_msg.compressed_data.end(), &compressed_array[0], &compressed_array[bytes_written]); + + return compressed_msg; +} + +nav2_msgs::msg::Costmap uncompress( + const costmap_compressor::msg::CompressedCostmap & compressed_msg) +{ + nav2_msgs::msg::Costmap costmap; + costmap.header = compressed_msg.header; + costmap.metadata = compressed_msg.metadata; + + auto snappy_source = snappy::ByteArraySource(reinterpret_cast(compressed_msg.compressed_data.data()), compressed_msg.compressed_data.size()); + std::vector array; + array.reserve(compressed_msg.compressed_data.size()); + auto snappy_writer = snappy::UncheckedByteArraySink(reinterpret_cast(array.data())); + const auto bytes_written = snappy::Uncompress(&snappy_source, &snappy_writer); + + costmap.data.insert(costmap.data.end(), &array[0], &array[bytes_written]); + + return costmap; +} + +} // namespace costmap_compressor diff --git a/test/costmap_compressor_test.cpp b/test/costmap_compressor_test.cpp new file mode 100644 index 0000000..04fcabf --- /dev/null +++ b/test/costmap_compressor_test.cpp @@ -0,0 +1,28 @@ +#include +#include "costmap_compressor/CostmapCompressor.hpp" +#include + +#include + +TEST(costmap_compressor_test, compress) +{ + nav2_msgs::msg::Costmap original; + original.header.frame_id = "odom"; + original.header.stamp = rclcpp::Time(1751818062, 0); + original.metadata.resolution = 1.0; + original.metadata.size_x = 20.0; + original.metadata.size_y = 20.0; + original.data.resize(original.metadata.size_x*original.metadata.size_y, 0); + + const costmap_compressor::msg::CompressedCostmap compressed = costmap_compressor::compress(original); + const nav2_msgs::msg::Costmap uncompressed = costmap_compressor::uncompress(compressed); + + EXPECT_TRUE(true); + +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}