Skip to content

perf: read header.stamp directly from CDR bytes without deserialization #89

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
otenim merged 1 commit into from
Apr 9, 2026
Merged

perf: read header.stamp directly from CDR bytes without deserialization #89

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
9 changes: 5 additions & 4 deletions src/bag_converter/include/header_stamp.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,11 +56,12 @@ inline const std::set<std::string> kTypesWithHeader = {
};

/**
* @brief Extract header.stamp by deserializing the message.
* @brief Extract header.stamp directly from CDR-serialized bytes.
*
* For types in kTypesWithHeader, deserializes to the concrete message type and
* returns header.stamp. Returns std::nullopt if the type is unsupported or
* deserialization fails.
* For types in kTypesWithHeader (all have Header as first field), reads
* stamp.sec and stamp.nanosec from fixed CDR byte offsets without full
* deserialization. Returns std::nullopt if the type is unsupported, the
* buffer is too small, or the CDR encoding is not little-endian.
*/
std::optional<rclcpp::Time> extract_header_stamp(
const rcutils_uint8_array_t & serialized_data, const std::string & topic_type);
Expand Down
112 changes: 31 additions & 81 deletions src/bag_converter/src/header_stamp.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,100 +8,50 @@

#include "header_stamp.hpp"

#include <rclcpp/serialization.hpp>
#include <seyond/msg/seyond_scan.hpp>

#include <can_msgs/msg/frame.hpp>
#include <geometry_msgs/msg/accel_stamped.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <geometry_msgs/msg/wrench_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <nebula_msgs/msg/nebula_packets.hpp>
#include <oxts_msgs/msg/imu_bias.hpp>
#include <oxts_msgs/msg/lever_arm.hpp>
#include <oxts_msgs/msg/nav_sat_ref.hpp>
#include <oxts_msgs/msg/ncom.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/compressed_image.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <sensor_msgs/msg/nav_sat_fix.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <cstring>

namespace bag_converter
{
namespace
{

template <typename MsgT>
std::optional<rclcpp::Time> deserialize_header_stamp(rclcpp::SerializedMessage & serialized_msg)
{
MsgT msg;
rclcpp::Serialization<MsgT> ser;
ser.deserialize_message(&serialized_msg, &msg);
return rclcpp::Time(msg.header.stamp);
}

} // namespace
// CDR layout for messages where std_msgs/msg/Header is the first field:
// Bytes 0-1: CDR representation identifier (0x00 0x01 = CDR little-endian)
// Bytes 2-3: CDR options (usually 0x00 0x00)
// Bytes 4-7: header.stamp.sec (int32_t)
// Bytes 8-11: header.stamp.nanosec (uint32_t)
//
// All types in kTypesWithHeader have Header as their first field, so this
// fixed-offset read is valid for all of them. This avoids full message
// deserialization, which is critical for large messages (e.g., Image).

static constexpr size_t kCdrHeaderSize = 4;
static constexpr size_t kStampSecOffset = kCdrHeaderSize;
static constexpr size_t kStampNanosecOffset = kCdrHeaderSize + sizeof(int32_t);
static constexpr size_t kMinBufferSize = kStampNanosecOffset + sizeof(uint32_t); // 12 bytes

std::optional<rclcpp::Time> extract_header_stamp(
const rcutils_uint8_array_t & serialized_data, const std::string & topic_type)
{
if (kTypesWithHeader.find(topic_type) == kTypesWithHeader.end()) {
return std::nullopt;
}
try {
rclcpp::SerializedMessage serialized_msg(serialized_data);
// sensor_msgs
if (topic_type == "sensor_msgs/msg/CameraInfo")
return deserialize_header_stamp<sensor_msgs::msg::CameraInfo>(serialized_msg);
if (topic_type == "sensor_msgs/msg/CompressedImage")
return deserialize_header_stamp<sensor_msgs::msg::CompressedImage>(serialized_msg);
if (topic_type == "sensor_msgs/msg/Imu")
return deserialize_header_stamp<sensor_msgs::msg::Imu>(serialized_msg);
if (topic_type == "sensor_msgs/msg/Image")
return deserialize_header_stamp<sensor_msgs::msg::Image>(serialized_msg);
if (topic_type == "sensor_msgs/msg/LaserScan")
return deserialize_header_stamp<sensor_msgs::msg::LaserScan>(serialized_msg);
if (topic_type == "sensor_msgs/msg/NavSatFix")
return deserialize_header_stamp<sensor_msgs::msg::NavSatFix>(serialized_msg);
if (topic_type == "sensor_msgs/msg/PointCloud2")
return deserialize_header_stamp<sensor_msgs::msg::PointCloud2>(serialized_msg);
// geometry_msgs
if (topic_type == "geometry_msgs/msg/AccelStamped")
return deserialize_header_stamp<geometry_msgs::msg::AccelStamped>(serialized_msg);
if (topic_type == "geometry_msgs/msg/PoseStamped")
return deserialize_header_stamp<geometry_msgs::msg::PoseStamped>(serialized_msg);
if (topic_type == "geometry_msgs/msg/TwistStamped")
return deserialize_header_stamp<geometry_msgs::msg::TwistStamped>(serialized_msg);
if (topic_type == "geometry_msgs/msg/WrenchStamped")
return deserialize_header_stamp<geometry_msgs::msg::WrenchStamped>(serialized_msg);
// nav_msgs
if (topic_type == "nav_msgs/msg/Odometry")
return deserialize_header_stamp<nav_msgs::msg::Odometry>(serialized_msg);
// can_msgs
if (topic_type == "can_msgs/msg/Frame")
return deserialize_header_stamp<can_msgs::msg::Frame>(serialized_msg);
// oxts_msgs
if (topic_type == "oxts_msgs/msg/ImuBias")
return deserialize_header_stamp<oxts_msgs::msg::ImuBias>(serialized_msg);
if (topic_type == "oxts_msgs/msg/LeverArm")
return deserialize_header_stamp<oxts_msgs::msg::LeverArm>(serialized_msg);
if (topic_type == "oxts_msgs/msg/NavSatRef")
return deserialize_header_stamp<oxts_msgs::msg::NavSatRef>(serialized_msg);
if (topic_type == "oxts_msgs/msg/Ncom")
return deserialize_header_stamp<oxts_msgs::msg::Ncom>(serialized_msg);
// LiDAR packet types
if (topic_type == "nebula_msgs/msg/NebulaPackets")
return deserialize_header_stamp<nebula_msgs::msg::NebulaPackets>(serialized_msg);
if (topic_type == "seyond/msg/SeyondScan")
return deserialize_header_stamp<seyond::msg::SeyondScan>(serialized_msg);
} catch (...) {

if (serialized_data.buffer_length < kMinBufferSize) {
return std::nullopt;
}

const uint8_t * buf = serialized_data.buffer;

// Verify CDR little-endian encoding (0x00 0x01)
if (buf[0] != 0x00 || buf[1] != 0x01) {
return std::nullopt;
}
return std::nullopt;

int32_t sec;
uint32_t nanosec;
std::memcpy(&sec, buf + kStampSecOffset, sizeof(sec));
std::memcpy(&nanosec, buf + kStampNanosecOffset, sizeof(nanosec));

return rclcpp::Time(sec, nanosec);
}

} // namespace bag_converter
Loading