Skip to content

Add optional ROS 1/2 bridge utilities #131

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

Open
Ikhyeon-Cho wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Add optional ROS 1/2 bridge utilities #131

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
37 changes: 37 additions & 0 deletions include/small_gicp/ros/ros1.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,37 @@
// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
// SPDX-FileCopyrightText: Copyright 2025 Ikhyeon Cho
// SPDX-License-Identifier: MIT
#pragma once

#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointField.h>

#include <small_gicp/ros/ros_impl.hpp>

namespace small_gicp {

/// @brief Convert ROS1 PointCloud2 message to small_gicp::PointCloud
/// @note Only XYZ coordinates are extracted. Invalid points (NaN/Inf) are skipped.
/// @warning Normals and covariances are UNINITIALIZED (contain garbage values).
/// You MUST call estimate_normals_covariances() before using this cloud for registration.
/// @param msg Input ROS1 PointCloud2 message
/// @return Shared pointer to PointCloud. Returns empty cloud if message is invalid.
inline PointCloud::Ptr from_ros_msg(const sensor_msgs::PointCloud2& msg) {
return detail::from_impl<sensor_msgs::PointCloud2, sensor_msgs::PointField>(msg);
}

/// @brief Convert small_gicp::PointCloud to ROS1 PointCloud2 message
/// @note By default, only XYZ coordinates are exported.
/// @param cloud Input point cloud
/// @param frame_id Frame ID for message header
/// @param stamp Timestamp for message header (default: ros::Time::now())
/// @param with_normals If true, include normal_x/y/z fields (default: false)
/// @warning If with_normals is true, caller must ensure normals are valid
/// (i.e., estimate_normals_covariances() was called beforehand)
/// @return ROS1 PointCloud2 message
inline sensor_msgs::PointCloud2 to_ros_msg(const PointCloud& cloud, const std::string& frame_id, const ros::Time& stamp = ros::Time::now(), bool with_normals = false) {
//
return detail::to_impl<sensor_msgs::PointCloud2, sensor_msgs::PointField>(cloud, frame_id, stamp, with_normals);
}

} // namespace small_gicp
38 changes: 38 additions & 0 deletions include/small_gicp/ros/ros2.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,38 @@
// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
// SPDX-FileCopyrightText: Copyright 2025 Ikhyeon Cho
// SPDX-License-Identifier: MIT
#pragma once

#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/msg/point_field.hpp>

#include <small_gicp/ros/ros_impl.hpp>

namespace small_gicp {

/// @brief Convert ROS2 PointCloud2 message to small_gicp::PointCloud
/// @note Only XYZ coordinates are extracted. Invalid points (NaN/Inf) are skipped.
/// @warning Normals and covariances are UNINITIALIZED (contain garbage values).
/// You MUST call estimate_normals_covariances() before using this cloud for registration.
/// @param msg Input ROS2 PointCloud2 message
/// @return Shared pointer to PointCloud. Returns empty cloud if message is invalid.
inline PointCloud::Ptr from_ros_msg(const sensor_msgs::msg::PointCloud2& msg) {
return detail::from_impl<sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointField>(msg);
}

/// @brief Convert small_gicp::PointCloud to ROS2 PointCloud2 message
/// @note By default, only XYZ coordinates are exported.
/// @param cloud Input point cloud
/// @param frame_id Frame ID for message header
/// @param stamp Timestamp for message header
/// @param with_normals If true, include normal_x/y/z fields (default: false)
/// @warning If with_normals is true, caller must ensure normals are valid
/// (i.e., estimate_normals_covariances() was called beforehand)
/// @return ROS2 PointCloud2 message
inline sensor_msgs::msg::PointCloud2
to_ros_msg(const PointCloud& cloud, const std::string& frame_id, const builtin_interfaces::msg::Time& stamp = builtin_interfaces::msg::Time(), bool with_normals = false) {
//
return detail::to_impl<sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointField>(cloud, frame_id, stamp, with_normals);
}

} // namespace small_gicp
175 changes: 175 additions & 0 deletions include/small_gicp/ros/ros_impl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,175 @@
// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
// SPDX-FileCopyrightText: Copyright 2025 Ikhyeon Cho
// SPDX-License-Identifier: MIT
#pragma once

#include <cmath>
#include <iostream>

#include <small_gicp/points/point_cloud.hpp>

namespace small_gicp {

namespace detail {

/// @brief Parse XYZ field offsets from PointCloud2 fields
/// @tparam PointFieldT PointField type (sensor_msgs::PointField or sensor_msgs::msg::PointField)
template <typename PointFieldT>
struct FieldOffsets {
int x = -1;
int y = -1;
int z = -1;

bool has_xyz() const { return x >= 0 && y >= 0 && z >= 0; }

template <typename FieldsT>
static FieldOffsets parse(const FieldsT& fields) {
FieldOffsets offsets;
for (const auto& field : fields) {
if (field.name == "x") {
offsets.x = field.offset;
} else if (field.name == "y") {
offsets.y = field.offset;
} else if (field.name == "z") {
offsets.z = field.offset;
}
}
return offsets;
}
};

/// @brief Convert PointCloud2 message to small_gicp::PointCloud (implementation)
/// @tparam PointCloud2T PointCloud2 message type
/// @tparam PointFieldT PointField type
template <typename PointCloud2T, typename PointFieldT>
PointCloud::Ptr from_impl(const PointCloud2T& msg) {
auto cloud = std::make_shared<PointCloud>();

const size_t num_points = static_cast<size_t>(msg.width) * msg.height;
if (num_points == 0) {
return cloud;
}

const auto offsets = FieldOffsets<PointFieldT>::parse(msg.fields);
if (!offsets.has_xyz()) {
std::cerr << "warning: PointCloud2 message does not have XYZ fields" << std::endl;
return cloud;
}

// Reserve memory without initialization (avoid unnecessary default construction)
cloud->points.reserve(num_points);

const uint8_t* data_ptr = msg.data.data();
const size_t point_step = msg.point_step;

for (size_t i = 0; i < num_points; i++) {
const uint8_t* pt = data_ptr + i * point_step;

const float x = *reinterpret_cast<const float*>(pt + offsets.x);
const float y = *reinterpret_cast<const float*>(pt + offsets.y);
const float z = *reinterpret_cast<const float*>(pt + offsets.z);

// Skip invalid points (NaN or Inf)
if (!std::isfinite(x) || !std::isfinite(y) || !std::isfinite(z)) {
continue;
}

cloud->points.emplace_back(x, y, z, 1.0);
}

// Resize normals and covs to match points size
cloud->normals.resize(cloud->points.size());
cloud->covs.resize(cloud->points.size());

return cloud;
}

/// @brief Convert small_gicp::PointCloud to PointCloud2 message (implementation)
/// @tparam PointCloud2T PointCloud2 message type
/// @tparam PointFieldT PointField type
/// @tparam TimeT Timestamp type
template <typename PointCloud2T, typename PointFieldT, typename TimeT>
PointCloud2T to_impl(const PointCloud& cloud, const std::string& frame_id, const TimeT& stamp, bool with_normals) {
PointCloud2T msg;

msg.header.frame_id = frame_id;
msg.header.stamp = stamp;
msg.height = 1;
msg.width = static_cast<uint32_t>(cloud.size());
msg.is_bigendian = false;
msg.is_dense = true;

if (cloud.empty()) {
msg.point_step = 0;
msg.row_step = 0;
return msg;
}

// Build fields
PointFieldT field;
field.count = 1;
field.datatype = PointFieldT::FLOAT32;

uint32_t offset = 0;

field.name = "x";
field.offset = offset;
msg.fields.push_back(field);
offset += sizeof(float);

field.name = "y";
field.offset = offset;
msg.fields.push_back(field);
offset += sizeof(float);

field.name = "z";
field.offset = offset;
msg.fields.push_back(field);
offset += sizeof(float);

if (with_normals) {
field.name = "normal_x";
field.offset = offset;
msg.fields.push_back(field);
offset += sizeof(float);

field.name = "normal_y";
field.offset = offset;
msg.fields.push_back(field);
offset += sizeof(float);

field.name = "normal_z";
field.offset = offset;
msg.fields.push_back(field);
offset += sizeof(float);
}

msg.point_step = offset;
msg.row_step = msg.point_step * msg.width;

// Fill data
msg.data.resize(msg.row_step);
uint8_t* data_ptr = msg.data.data();

for (size_t i = 0; i < cloud.size(); i++) {
uint8_t* pt = data_ptr + i * msg.point_step;

const Eigen::Vector4d& point = cloud.point(i);
*reinterpret_cast<float*>(pt + 0) = static_cast<float>(point.x());
*reinterpret_cast<float*>(pt + 4) = static_cast<float>(point.y());
*reinterpret_cast<float*>(pt + 8) = static_cast<float>(point.z());

if (with_normals) {
const Eigen::Vector4d& normal = cloud.normal(i);
*reinterpret_cast<float*>(pt + 12) = static_cast<float>(normal.x());
*reinterpret_cast<float*>(pt + 16) = static_cast<float>(normal.y());
*reinterpret_cast<float*>(pt + 20) = static_cast<float>(normal.z());
}
}

return msg;
}

} // namespace detail

} // namespace small_gicp