From f18f70510a8d27f0ac2b56eba73f817b98849d57 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 10 Jan 2026 08:36:49 +0100 Subject: [PATCH 1/2] Add headers in conversions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- navmap_ros/include/navmap_ros/conversions.hpp | 52 +++++++++- navmap_ros/src/navmap_ros/conversions.cpp | 67 +++++++++++-- navmap_ros/tests/test_conversions.cpp | 94 ++++++++++++++++--- navmap_ros/tests/test_navmap_io.cpp | 12 ++- 4 files changed, 200 insertions(+), 25 deletions(-) diff --git a/navmap_ros/include/navmap_ros/conversions.hpp b/navmap_ros/include/navmap_ros/conversions.hpp index 1d5ef88..f073f08 100644 --- a/navmap_ros/include/navmap_ros/conversions.hpp +++ b/navmap_ros/include/navmap_ros/conversions.hpp @@ -41,6 +41,7 @@ #include #include +#include "std_msgs/msg/header.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" #include "navmap_ros_interfaces/msg/nav_map.hpp" #include "navmap_ros_interfaces/msg/nav_map_layer.hpp" @@ -81,6 +82,7 @@ constexpr uint8_t FREE_SPACE = 0; * @brief Convert a core `navmap::NavMap` into its compact ROS transport message. * * @param[in] nm Core NavMap to be serialized into a ROS message. + * @param[in] header Header to assign to the resulting message. * @return A `navmap_ros_interfaces::msg::NavMap` containing geometry (vertices, triangles), * surfaces metadata and user-defined layers. * @@ -91,12 +93,20 @@ constexpr uint8_t FREE_SPACE = 0; * @note This function does not perform IO; it only builds the message in-memory. */ navmap_ros_interfaces::msg::NavMap to_msg( - const navmap::NavMap & nm); + const navmap::NavMap & nm, const std_msgs::msg::Header & header); + +/** + * @brief Backward-compatible overload (no header provided). + * + * The returned message header will be default-constructed. + */ +navmap_ros_interfaces::msg::NavMap to_msg(const navmap::NavMap & nm); /** * @brief Reconstruct a core `navmap::NavMap` from the ROS transport message. * * @param[in] msg Input `navmap_ros_interfaces::msg::NavMap` message. + * @param[out] header Header extracted from the message. * @return A core `navmap::NavMap` equivalent to the content of @p msg. * * @details @@ -105,6 +115,13 @@ navmap_ros_interfaces::msg::NavMap to_msg( * * @throw std::runtime_error If the message describes inconsistent geometry or layer sizes. */ +navmap::NavMap from_msg( + const navmap_ros_interfaces::msg::NavMap & msg, + std_msgs::msg::Header & header); + +/** + * @brief Backward-compatible overload (ignores message header). + */ navmap::NavMap from_msg(const navmap_ros_interfaces::msg::NavMap & msg); /** @@ -112,6 +129,7 @@ navmap::NavMap from_msg(const navmap_ros_interfaces::msg::NavMap & msg); * * @param[in] nm Input NavMap. * @param[in] layer Name of the layer to export. + * @param[in] header Header to assign to the resulting message. * @return A NavMapLayer message containing the layer values and metadata. * * @details @@ -121,6 +139,14 @@ navmap::NavMap from_msg(const navmap_ros_interfaces::msg::NavMap & msg); * * @throw std::runtime_error If the layer does not exist or has an unsupported type. */ +navmap_ros_interfaces::msg::NavMapLayer to_msg( + const navmap::NavMap & nm, + const std::string & layer, + const std_msgs::msg::Header & header); + +/** + * @brief Backward-compatible overload (no header provided). + */ navmap_ros_interfaces::msg::NavMapLayer to_msg( const navmap::NavMap & nm, const std::string & layer); @@ -133,6 +159,7 @@ navmap_ros_interfaces::msg::NavMapLayer to_msg( * * @param[in] msg Input NavMapLayer message. * @param[in,out] nm Destination NavMap (must already have navcels sized correctly). + * @param[in] header Header to assign to the resulting message. * * @details * - The function verifies that the length of the populated data array matches @@ -141,6 +168,14 @@ navmap_ros_interfaces::msg::NavMapLayer to_msg( * * @throw std::runtime_error If sizes are inconsistent or the message is ill-formed. */ +void from_msg( + const navmap_ros_interfaces::msg::NavMapLayer & msg, + navmap::NavMap & nm, + std_msgs::msg::Header & header); + +/** + * @brief Backward-compatible overload (ignores message header). + */ void from_msg( const navmap_ros_interfaces::msg::NavMapLayer & msg, navmap::NavMap & nm); @@ -152,6 +187,7 @@ void from_msg( * using a regular triangular surface with shared vertices. * * @param[in] grid Input ROS OccupancyGrid (row-major, width×height, resolution and origin). + * @param[in] header Header to assign to the resulting message. * @return A core `navmap::NavMap` with: * - Vertices: `(W+1) * (H+1)` laid on the grid plane, with `Z = grid.info.origin.position.z`. * - Triangles: `2 * W * H` (two per cell), using diagonal pattern = 0. @@ -167,6 +203,13 @@ void from_msg( * @note The grid origin pose may contain a rotation. The vertex Z is taken from the origin Z; * handling of non-zero yaw/roll/pitch (if any) is implementation-defined in the builder. */ +navmap::NavMap from_occupancy_grid( + const nav_msgs::msg::OccupancyGrid & grid, + std_msgs::msg::Header & header); + +/** + * @brief Backward-compatible overload (ignores grid header). + */ navmap::NavMap from_occupancy_grid(const nav_msgs::msg::OccupancyGrid & grid); /** @@ -192,6 +235,13 @@ navmap::NavMap from_occupancy_grid(const nav_msgs::msg::OccupancyGrid & grid); * @warning If the map does not carry grid metadata or the `"occupancy"` layer is missing, * the result may be incomplete or implementation-defined. */ +nav_msgs::msg::OccupancyGrid to_occupancy_grid( + const navmap::NavMap & nm, + const std_msgs::msg::Header & header); + +/** + * @brief Backward-compatible overload. + */ nav_msgs::msg::OccupancyGrid to_occupancy_grid(const navmap::NavMap & nm); /** diff --git a/navmap_ros/src/navmap_ros/conversions.cpp b/navmap_ros/src/navmap_ros/conversions.cpp index 276e1dd..e77d552 100644 --- a/navmap_ros/src/navmap_ros/conversions.cpp +++ b/navmap_ros/src/navmap_ros/conversions.cpp @@ -67,9 +67,10 @@ static inline navmap::NavCelId tri_index_for_cell(uint32_t i, uint32_t j, uint32 // ----------------- NavMap <-> ROS message ----------------- -NavMap to_msg(const navmap::NavMap & nm) +NavMap to_msg(const navmap::NavMap & nm, const std_msgs::msg::Header & header) { NavMap out; + out.header = header; // positions out.positions_x.assign(nm.positions.x.begin(), nm.positions.x.end()); @@ -135,8 +136,14 @@ NavMap to_msg(const navmap::NavMap & nm) return out; } -navmap::NavMap from_msg(const NavMap & msg) +NavMap to_msg(const navmap::NavMap & nm) { + return to_msg(nm, std_msgs::msg::Header()); +} + +navmap::NavMap from_msg(const NavMap & msg, std_msgs::msg::Header & header) +{ + header = msg.header; navmap::NavMap nm; // positions @@ -203,11 +210,19 @@ navmap::NavMap from_msg(const NavMap & msg) return nm; } +navmap::NavMap from_msg(const NavMap & msg) +{ + std_msgs::msg::Header unused; + return from_msg(msg, unused); +} + navmap_ros_interfaces::msg::NavMapLayer to_msg( const navmap::NavMap & nm, - const std::string & layer_name) + const std::string & layer_name, + const std_msgs::msg::Header & header) { navmap_ros_interfaces::msg::NavMapLayer msg; + msg.header = header; msg.name = layer_name; auto base = nm.layers.get(layer_name); @@ -241,10 +256,19 @@ navmap_ros_interfaces::msg::NavMapLayer to_msg( return msg; } +navmap_ros_interfaces::msg::NavMapLayer to_msg( + const navmap::NavMap & nm, + const std::string & layer_name) +{ + return to_msg(nm, layer_name, std_msgs::msg::Header()); +} + void from_msg( const navmap_ros_interfaces::msg::NavMapLayer & msg, - navmap::NavMap & nm) + navmap::NavMap & nm, + std_msgs::msg::Header & header) { + header = msg.header; switch (msg.type) { case navmap_ros_interfaces::msg::NavMapLayer::U8: { auto dst = nm.add_layer(msg.name, /*desc*/"", /*unit*/"", uint8_t{}); @@ -276,10 +300,21 @@ void from_msg( } } +void from_msg( + const navmap_ros_interfaces::msg::NavMapLayer & msg, + navmap::NavMap & nm) +{ + std_msgs::msg::Header unused; + from_msg(msg, nm, unused); +} + // ----------------- OccupancyGrid <-> NavMap ----------------- -navmap::NavMap from_occupancy_grid(const nav_msgs::msg::OccupancyGrid & grid) +navmap::NavMap from_occupancy_grid( + const nav_msgs::msg::OccupancyGrid & grid, + std_msgs::msg::Header & header) { + header = grid.header; navmap::NavMap nm; const uint32_t W = grid.info.width; @@ -349,10 +384,21 @@ navmap::NavMap from_occupancy_grid(const nav_msgs::msg::OccupancyGrid & grid) return nm; } -nav_msgs::msg::OccupancyGrid to_occupancy_grid(const navmap::NavMap & nm) +navmap::NavMap from_occupancy_grid(const nav_msgs::msg::OccupancyGrid & grid) +{ + std_msgs::msg::Header unused; + return from_occupancy_grid(grid, unused); +} + +nav_msgs::msg::OccupancyGrid to_occupancy_grid( + const navmap::NavMap & nm, + const std_msgs::msg::Header & header) { nav_msgs::msg::OccupancyGrid g; - g.header.frame_id = (nm.surfaces.empty() ? std::string() : nm.surfaces[0].frame_id); + g.header = header; + if (g.header.frame_id.empty() && !nm.surfaces.empty()) { + g.header.frame_id = nm.surfaces[0].frame_id; + } auto base = nm.layers.get("occupancy"); if (!base || base->type() != navmap::LayerType::U8) { @@ -449,6 +495,13 @@ nav_msgs::msg::OccupancyGrid to_occupancy_grid(const navmap::NavMap & nm) return g; } +nav_msgs::msg::OccupancyGrid to_occupancy_grid(const navmap::NavMap & nm) +{ + std_msgs::msg::Header h; + h.frame_id = (nm.surfaces.empty() ? std::string() : nm.surfaces[0].frame_id); + return to_occupancy_grid(nm, h); +} + bool build_navmap_from_mesh( const pcl::PointCloud & cloud, const std::vector & triangles, diff --git a/navmap_ros/tests/test_conversions.cpp b/navmap_ros/tests/test_conversions.cpp index 8d0d021..266fe6e 100644 --- a/navmap_ros/tests/test_conversions.cpp +++ b/navmap_ros/tests/test_conversions.cpp @@ -17,6 +17,7 @@ #include #include "navmap_ros_interfaces/msg/nav_map_layer.hpp" +#include "std_msgs/msg/header.hpp" #include "navmap_ros/conversions.hpp" #include "navmap_core/NavMap.hpp" @@ -124,8 +125,17 @@ static inline navmap::NavCelId tri_index_for_cell(uint32_t i, uint32_t j, uint32 TEST(NavMap_FullConversions, RoundTrip_All) { navmap::NavMap a; build_square_with_layers(a); - auto msg = navmap_ros::to_msg(a); - navmap::NavMap b = navmap_ros::from_msg(msg); + std_msgs::msg::Header h; + h.frame_id = "map"; + h.stamp.sec = 123; + h.stamp.nanosec = 456; + auto msg = navmap_ros::to_msg(a, h); + std_msgs::msg::Header h2; + navmap::NavMap b = navmap_ros::from_msg(msg, h2); + + EXPECT_EQ(h2.frame_id, h.frame_id); + EXPECT_EQ(h2.stamp.sec, h.stamp.sec); + EXPECT_EQ(h2.stamp.nanosec, h.stamp.nanosec); ASSERT_EQ(a.positions.size(), b.positions.size()); for (size_t i = 0; i < a.positions.size(); ++i) { @@ -170,8 +180,16 @@ TEST(NavMap_FullConversions, RoundTrip_All) TEST(NavMap_FullConversions, EmptyMap_RoundTrip) { navmap::NavMap a; - auto msg = navmap_ros::to_msg(a); - navmap::NavMap b = navmap_ros::from_msg(msg); + std_msgs::msg::Header h; + h.frame_id = "map"; + h.stamp.sec = 1; + h.stamp.nanosec = 2; + auto msg = navmap_ros::to_msg(a, h); + std_msgs::msg::Header h2; + navmap::NavMap b = navmap_ros::from_msg(msg, h2); + EXPECT_EQ(h2.frame_id, h.frame_id); + EXPECT_EQ(h2.stamp.sec, h.stamp.sec); + EXPECT_EQ(h2.stamp.nanosec, h.stamp.nanosec); EXPECT_EQ(b.positions.size(), 0u); EXPECT_EQ(b.navcels.size(), 0u); EXPECT_EQ(b.surfaces.size(), 0u); @@ -184,7 +202,14 @@ TEST(NavMap_LayerConversions, U8_RoundTrip) auto occ = nm.add_layer("occupancy", "occ", "", uint8_t(0)); occ->data()[0] = 10u; occ->data()[1] = 250u; - auto msg = navmap_ros::to_msg(nm, "occupancy"); + std_msgs::msg::Header h; + h.frame_id = "map"; + h.stamp.sec = 10; + h.stamp.nanosec = 20; + auto msg = navmap_ros::to_msg(nm, "occupancy", h); + EXPECT_EQ(msg.header.frame_id, h.frame_id); + EXPECT_EQ(msg.header.stamp.sec, h.stamp.sec); + EXPECT_EQ(msg.header.stamp.nanosec, h.stamp.nanosec); EXPECT_EQ(msg.name, "occupancy"); EXPECT_EQ(msg.type, 0u); // 0=U8 ASSERT_EQ(msg.data_u8.size(), 2u); @@ -192,7 +217,11 @@ TEST(NavMap_LayerConversions, U8_RoundTrip) EXPECT_EQ(msg.data_u8[1], 250u); navmap::NavMap nm2; make_flat_square(nm2); - navmap_ros::from_msg(msg, nm2); + std_msgs::msg::Header h2; + navmap_ros::from_msg(msg, nm2, h2); + EXPECT_EQ(h2.frame_id, h.frame_id); + EXPECT_EQ(h2.stamp.sec, h.stamp.sec); + EXPECT_EQ(h2.stamp.nanosec, h.stamp.nanosec); ASSERT_TRUE(nm2.has_layer("occupancy")); EXPECT_NEAR(nm2.layer_get("occupancy", 0), 10.0, 1e-6); EXPECT_NEAR(nm2.layer_get("occupancy", 1), 250.0, 1e-6); @@ -204,14 +233,25 @@ TEST(NavMap_LayerConversions, F32_RoundTrip) auto cost = nm.add_layer("cost", "cost", "", 0.0f); cost->data()[0] = 1.25f; cost->data()[1] = 9.5f; - auto msg = navmap_ros::to_msg(nm, "cost"); + std_msgs::msg::Header h; + h.frame_id = "map"; + h.stamp.sec = 11; + h.stamp.nanosec = 22; + auto msg = navmap_ros::to_msg(nm, "cost", h); + EXPECT_EQ(msg.header.frame_id, h.frame_id); + EXPECT_EQ(msg.header.stamp.sec, h.stamp.sec); + EXPECT_EQ(msg.header.stamp.nanosec, h.stamp.nanosec); EXPECT_EQ(msg.type, 1u); // 1=F32 ASSERT_EQ(msg.data_f32.size(), 2u); EXPECT_NEAR(msg.data_f32[0], 1.25f, 1e-6); EXPECT_NEAR(msg.data_f32[1], 9.5f, 1e-6); navmap::NavMap nm2; make_flat_square(nm2); - navmap_ros::from_msg(msg, nm2); + std_msgs::msg::Header h2; + navmap_ros::from_msg(msg, nm2, h2); + EXPECT_EQ(h2.frame_id, h.frame_id); + EXPECT_EQ(h2.stamp.sec, h.stamp.sec); + EXPECT_EQ(h2.stamp.nanosec, h.stamp.nanosec); ASSERT_TRUE(nm2.has_layer("cost")); EXPECT_NEAR(nm2.layer_get("cost", 0), 1.25, 1e-6); EXPECT_NEAR(nm2.layer_get("cost", 1), 9.5, 1e-6); @@ -224,14 +264,25 @@ TEST(NavMap_LayerConversions, F64_RoundTrip) elev->data()[0] = 12.345; elev->data()[1] = -2.5; - auto msg = navmap_ros::to_msg(nm, "elevation"); + std_msgs::msg::Header h; + h.frame_id = "map"; + h.stamp.sec = 12; + h.stamp.nanosec = 24; + auto msg = navmap_ros::to_msg(nm, "elevation", h); + EXPECT_EQ(msg.header.frame_id, h.frame_id); + EXPECT_EQ(msg.header.stamp.sec, h.stamp.sec); + EXPECT_EQ(msg.header.stamp.nanosec, h.stamp.nanosec); EXPECT_EQ(msg.type, 2u); // 2=F64 ASSERT_EQ(msg.data_f64.size(), 2u); EXPECT_NEAR(msg.data_f64[0], 12.345, 1e-9); EXPECT_NEAR(msg.data_f64[1], -2.5, 1e-9); navmap::NavMap nm2; make_flat_square(nm2); - navmap_ros::from_msg(msg, nm2); + std_msgs::msg::Header h2; + navmap_ros::from_msg(msg, nm2, h2); + EXPECT_EQ(h2.frame_id, h.frame_id); + EXPECT_EQ(h2.stamp.sec, h.stamp.sec); + EXPECT_EQ(h2.stamp.nanosec, h.stamp.nanosec); ASSERT_TRUE(nm2.has_layer("elevation")); EXPECT_NEAR(nm2.layer_get("elevation", 0), 12.345, 1e-9); EXPECT_NEAR(nm2.layer_get("elevation", 1), -2.5, 1e-9); @@ -247,9 +298,23 @@ TEST(TestConversions, RoundTrip_ExactEquality_4m_0p1) { const int W = 40, H = 40; auto g = make_grid_4m_0p1(); - - auto nm = from_occupancy_grid(g); - auto gout = to_occupancy_grid(nm); + g.header.stamp.sec = 111; + g.header.stamp.nanosec = 222; + + std_msgs::msg::Header h_in; + auto nm = from_occupancy_grid(g, h_in); + EXPECT_EQ(h_in.frame_id, g.header.frame_id); + EXPECT_EQ(h_in.stamp.sec, g.header.stamp.sec); + EXPECT_EQ(h_in.stamp.nanosec, g.header.stamp.nanosec); + + std_msgs::msg::Header h_out; + h_out.frame_id = "map"; + h_out.stamp.sec = 333; + h_out.stamp.nanosec = 444; + auto gout = to_occupancy_grid(nm, h_out); + EXPECT_EQ(gout.header.frame_id, h_out.frame_id); + EXPECT_EQ(gout.header.stamp.sec, h_out.stamp.sec); + EXPECT_EQ(gout.header.stamp.nanosec, h_out.stamp.nanosec); ASSERT_EQ(gout.info.width, g.info.width); ASSERT_EQ(gout.info.height, g.info.height); @@ -293,7 +358,8 @@ TEST(TestConversions, TriangleIndicesFollowPattern0) { const int W = 40; auto g = make_grid_4m_0p1(); - auto nm = from_occupancy_grid(g); + std_msgs::msg::Header unused; + auto nm = from_occupancy_grid(g, unused); // Pick a cell and verify its triangles reference the expected 4 vertices. auto v_id = [W](uint32_t i, uint32_t j) -> navmap::PointId { diff --git a/navmap_ros/tests/test_navmap_io.cpp b/navmap_ros/tests/test_navmap_io.cpp index c6674cb..8168125 100644 --- a/navmap_ros/tests/test_navmap_io.cpp +++ b/navmap_ros/tests/test_navmap_io.cpp @@ -23,6 +23,8 @@ #include #include +#include "std_msgs/msg/header.hpp" + #include "navmap_core/NavMap.hpp" #include "navmap_ros/conversions.hpp" #include "navmap_ros/navmap_io.hpp" @@ -290,7 +292,9 @@ TEST(NavMapIoCore, RoundtripViaCoreAndMsgCompare) msg.layers = {u8, f32}; // msg -> core -navmap::NavMap core = navmap_ros::from_msg(msg); +std_msgs::msg::Header h_in; +navmap::NavMap core = navmap_ros::from_msg(msg, h_in); +EXPECT_EQ(h_in.frame_id, msg.header.frame_id); // save(core) -> load(core) std::string path = (std::filesystem::temp_directory_path() / @@ -302,8 +306,10 @@ navmap::NavMap core_loaded; ASSERT_TRUE(navmap_ros::io::load_from_file(path, core_loaded, &ec)) << ec.message(); // core -> msg -auto msg_from_core = navmap_ros::to_msg(core); -auto msg_from_core_loaded = navmap_ros::to_msg(core_loaded); +std_msgs::msg::Header h_out; +h_out.frame_id = msg.header.frame_id; +auto msg_from_core = navmap_ros::to_msg(core, h_out); +auto msg_from_core_loaded = navmap_ros::to_msg(core_loaded, h_out); // Semantic comparison (order and FP tolerant) ExpectNavMapMsgEqualSemantic(msg_from_core, msg_from_core_loaded); From 04139b2c9d63665ee0174e21938101faaac16dc4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 10 Jan 2026 08:44:01 +0100 Subject: [PATCH 2/2] Fix doc in header MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- navmap_ros/include/navmap_ros/conversions.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/navmap_ros/include/navmap_ros/conversions.hpp b/navmap_ros/include/navmap_ros/conversions.hpp index f073f08..b5843a2 100644 --- a/navmap_ros/include/navmap_ros/conversions.hpp +++ b/navmap_ros/include/navmap_ros/conversions.hpp @@ -159,7 +159,7 @@ navmap_ros_interfaces::msg::NavMapLayer to_msg( * * @param[in] msg Input NavMapLayer message. * @param[in,out] nm Destination NavMap (must already have navcels sized correctly). - * @param[in] header Header to assign to the resulting message. + * @param[out] header Header extracted from the message. * * @details * - The function verifies that the length of the populated data array matches @@ -187,7 +187,7 @@ void from_msg( * using a regular triangular surface with shared vertices. * * @param[in] grid Input ROS OccupancyGrid (row-major, width×height, resolution and origin). - * @param[in] header Header to assign to the resulting message. + * @param[out] header Header to assign to the resulting message. * @return A core `navmap::NavMap` with: * - Vertices: `(W+1) * (H+1)` laid on the grid plane, with `Z = grid.info.origin.position.z`. * - Triangles: `2 * W * H` (two per cell), using diagonal pattern = 0.