Skip to content

Add headers in conversions #18

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
fmrico merged 2 commits into from
Jan 10, 2026
+200 −25
Merged

Add headers in conversions #18

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
f18f705
Add headers in conversions
fmrico Jan 10, 2026
04139b2
Fix doc in header
fmrico Jan 10, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
52 changes: 51 additions & 1 deletion navmap_ros/include/navmap_ros/conversions.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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.
*
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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);
Expand All @@ -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
Expand All @@ -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);
Expand All @@ -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.
Expand All @@ -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);

/**
Expand All @@ -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);

/**
Expand Down
67 changes: 60 additions & 7 deletions navmap_ros/src/navmap_ros/conversions.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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());
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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{});
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -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,
Expand Down
Loading
Loading