Merge pull request #18 from fmrico/headers_in_conversions

Add headers in conversions

Author: fmrico

navmap_ros/include/navmap_ros/conversions.hpp

@@ -41,6 +41,7 @@
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 
+#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,13 +115,21 @@ 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);
 
 /**
  * @brief Convert a single layer from a NavMap into a ROS message.
  *
  * @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[out] header Header extracted from the 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[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.
@@ -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);
 
 /**

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<uint8_t>(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<pcl::PointXYZ> & cloud,
   const std::vector<Eigen::Vector3i> & triangles,