Track uncommited.

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
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>)
+
+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
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+  )
+  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()

include/costmap_compressor/CostmapCompressor.hpp

@@ -0,0 +1,12 @@
+#pragma once
+
+#include <costmap_compressor/msg/compressed_costmap.hpp>
+#include <nav2_msgs/msg/costmap.hpp>
+
+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

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

package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>costmap_compressor</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="jpace121@gmail.com">James Pace</maintainer>
+  <license>MPL-2.0</license>
+
+  <depend>nav2_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>rclcpp</depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/CostmapCompressor.cpp

@@ -0,0 +1,45 @@
+#include "costmap_compressor/CostmapCompressor.hpp"
+
+#include <snappy.h>
+#include <snappy-sinksource.h>
+
+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<const char *>(costmap.data.data()), costmap.data.size());
+    std::vector<uint8_t> compressed_array;
+    compressed_array.reserve(costmap.data.size());
+    auto snappy_writer = snappy::UncheckedByteArraySink(reinterpret_cast<char *>(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<const char *>(compressed_msg.compressed_data.data()), compressed_msg.compressed_data.size());
+    std::vector<uint8_t> array;
+    array.reserve(compressed_msg.compressed_data.size());
+    auto snappy_writer = snappy::UncheckedByteArraySink(reinterpret_cast<char *>(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

test/costmap_compressor_test.cpp

@@ -0,0 +1,28 @@
+#include <gtest/gtest.h>
+#include "costmap_compressor/CostmapCompressor.hpp"
+#include <rclcpp/time.hpp>
+
+#include <iostream>
+
+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();
+}