Skip to content

refactor: extract header stamp extraction into separate library #87

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

refactor: extract header stamp extraction into separate library #87

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
1 change: 1 addition & 0 deletions src/bag_converter/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,7 @@ endif()
# Build unified bag_converter executable
add_executable(bag_converter_bin
src/bag_converter.cpp
src/header_stamp.cpp
src/memory_management.cpp
src/merge.cpp
src/seyond_decoder.cpp
Expand Down
36 changes: 1 addition & 35 deletions src/bag_converter/include/bag_converter.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
#ifndef BAG_CONVERTER__BAG_CONVERTER_HPP
#define BAG_CONVERTER__BAG_CONVERTER_HPP

#include "header_stamp.hpp"
#include "memory_management.hpp"
#include "nebula_decoder.hpp"
#include "point_types.hpp"
Expand Down Expand Up @@ -57,41 +58,6 @@ inline constexpr int64_t header_stamp_min_epoch_sec = 946'684'800;

} // namespace defaults

/**
* @brief Set of ROS 2 message types that have std_msgs/msg/Header (used for log_time override).
*
* For these types, header.stamp is extracted by deserializing the message and
* reading the header field, so log_time can be overridden from header.stamp when
* --use-header-stamp-as-log-time is enabled.
*/
inline const std::set<std::string> kTypesWithHeader = {
// LiDAR packet types (for log_time override when keeping original topics)
"nebula_msgs/msg/NebulaPackets",
"seyond/msg/SeyondScan",
// sensor_msgs
"sensor_msgs/msg/CameraInfo",
"sensor_msgs/msg/CompressedImage",
"sensor_msgs/msg/Imu",
"sensor_msgs/msg/Image",
"sensor_msgs/msg/LaserScan",
"sensor_msgs/msg/NavSatFix",
"sensor_msgs/msg/PointCloud2",
// geometry_msgs
"geometry_msgs/msg/AccelStamped",
"geometry_msgs/msg/PoseStamped",
"geometry_msgs/msg/TwistStamped",
"geometry_msgs/msg/WrenchStamped",
// nav_msgs
"nav_msgs/msg/Odometry",
// can_msgs
"can_msgs/msg/Frame",
// oxts_msgs
"oxts_msgs/msg/ImuBias",
"oxts_msgs/msg/LeverArm",
"oxts_msgs/msg/NavSatRef",
"oxts_msgs/msg/Ncom",
};

/**
* @brief Return code for conversion functions
*/
Expand Down
70 changes: 70 additions & 0 deletions src/bag_converter/include/header_stamp.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,70 @@
/*
* Copyright (C) 2025 TierIV Inc.
*
* License: Apache License
*
* Header stamp extraction utilities for ROS 2 message types
*/

#ifndef BAG_CONVERTER__HEADER_STAMP_HPP
#define BAG_CONVERTER__HEADER_STAMP_HPP

#include <rclcpp/rclcpp.hpp>

#include <rcutils/types/uint8_array.h>

#include <optional>
#include <set>
#include <string>

namespace bag_converter
{

/**
* @brief Set of ROS 2 message types that have std_msgs/msg/Header (used for log_time override).
*
* For these types, header.stamp is extracted by deserializing the message and
* reading the header field, so log_time can be overridden from header.stamp when
* --use-header-stamp-as-log-time is enabled.
*/
inline const std::set<std::string> kTypesWithHeader = {
// LiDAR packet types (for log_time override when keeping original topics)
"nebula_msgs/msg/NebulaPackets",
"seyond/msg/SeyondScan",
// sensor_msgs
"sensor_msgs/msg/CameraInfo",
"sensor_msgs/msg/CompressedImage",
"sensor_msgs/msg/Imu",
"sensor_msgs/msg/Image",
"sensor_msgs/msg/LaserScan",
"sensor_msgs/msg/NavSatFix",
"sensor_msgs/msg/PointCloud2",
// geometry_msgs
"geometry_msgs/msg/AccelStamped",
"geometry_msgs/msg/PoseStamped",
"geometry_msgs/msg/TwistStamped",
"geometry_msgs/msg/WrenchStamped",
// nav_msgs
"nav_msgs/msg/Odometry",
// can_msgs
"can_msgs/msg/Frame",
// oxts_msgs
"oxts_msgs/msg/ImuBias",
"oxts_msgs/msg/LeverArm",
"oxts_msgs/msg/NavSatRef",
"oxts_msgs/msg/Ncom",
};

/**
* @brief Extract header.stamp by deserializing the message.
*
* 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.
*/
std::optional<rclcpp::Time> extract_header_stamp(
const rcutils_uint8_array_t & serialized_data, const std::string & topic_type);

} // namespace bag_converter

#endif // BAG_CONVERTER__HEADER_STAMP_HPP
167 changes: 1 addition & 166 deletions src/bag_converter/src/bag_converter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,28 +13,8 @@
#include "merge.hpp"
#include "tf_transformer.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 <algorithm>
#include <cctype>
#include <cstring>
#include <filesystem>
#include <fstream>
Expand All @@ -50,147 +30,6 @@ static const rclcpp::Logger g_logger = rclcpp::get_logger("bag_converter");
namespace bag_converter
{

/**
* @brief Extract header.stamp by deserializing the message.
*
* 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.
*/
static 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") {
sensor_msgs::msg::CameraInfo msg;
rclcpp::Serialization<sensor_msgs::msg::CameraInfo> ser;
ser.deserialize_message(&serialized_msg, &msg);
return rclcpp::Time(msg.header.stamp);
}
if (topic_type == "sensor_msgs/msg/CompressedImage") {
sensor_msgs::msg::CompressedImage msg;
rclcpp::Serialization<sensor_msgs::msg::CompressedImage> ser;
ser.deserialize_message(&serialized_msg, &msg);
return rclcpp::Time(msg.header.stamp);
}
if (topic_type == "sensor_msgs/msg/Imu") {
sensor_msgs::msg::Imu msg;
rclcpp::Serialization<sensor_msgs::msg::Imu> ser;
ser.deserialize_message(&serialized_msg, &msg);
return rclcpp::Time(msg.header.stamp);
}
if (topic_type == "sensor_msgs/msg/Image") {
sensor_msgs::msg::Image msg;
rclcpp::Serialization<sensor_msgs::msg::Image> ser;
ser.deserialize_message(&serialized_msg, &msg);
return rclcpp::Time(msg.header.stamp);
}
if (topic_type == "sensor_msgs/msg/LaserScan") {
sensor_msgs::msg::LaserScan msg;
rclcpp::Serialization<sensor_msgs::msg::LaserScan> ser;
ser.deserialize_message(&serialized_msg, &msg);
return rclcpp::Time(msg.header.stamp);
}
if (topic_type == "sensor_msgs/msg/NavSatFix") {
sensor_msgs::msg::NavSatFix msg;
rclcpp::Serialization<sensor_msgs::msg::NavSatFix> ser;
ser.deserialize_message(&serialized_msg, &msg);
return rclcpp::Time(msg.header.stamp);
}
if (topic_type == "sensor_msgs/msg/PointCloud2") {
sensor_msgs::msg::PointCloud2 msg;
rclcpp::Serialization<sensor_msgs::msg::PointCloud2> ser;
ser.deserialize_message(&serialized_msg, &msg);
return rclcpp::Time(msg.header.stamp);
}
// geometry_msgs
if (topic_type == "geometry_msgs/msg/AccelStamped") {
geometry_msgs::msg::AccelStamped msg;
rclcpp::Serialization<geometry_msgs::msg::AccelStamped> ser;
ser.deserialize_message(&serialized_msg, &msg);
return rclcpp::Time(msg.header.stamp);
}
if (topic_type == "geometry_msgs/msg/PoseStamped") {
geometry_msgs::msg::PoseStamped msg;
rclcpp::Serialization<geometry_msgs::msg::PoseStamped> ser;
ser.deserialize_message(&serialized_msg, &msg);
return rclcpp::Time(msg.header.stamp);
}
if (topic_type == "geometry_msgs/msg/TwistStamped") {
geometry_msgs::msg::TwistStamped msg;
rclcpp::Serialization<geometry_msgs::msg::TwistStamped> ser;
ser.deserialize_message(&serialized_msg, &msg);
return rclcpp::Time(msg.header.stamp);
}
if (topic_type == "geometry_msgs/msg/WrenchStamped") {
geometry_msgs::msg::WrenchStamped msg;
rclcpp::Serialization<geometry_msgs::msg::WrenchStamped> ser;
ser.deserialize_message(&serialized_msg, &msg);
return rclcpp::Time(msg.header.stamp);
}
// nav_msgs
if (topic_type == "nav_msgs/msg/Odometry") {
nav_msgs::msg::Odometry msg;
rclcpp::Serialization<nav_msgs::msg::Odometry> ser;
ser.deserialize_message(&serialized_msg, &msg);
return rclcpp::Time(msg.header.stamp);
}
// can_msgs
if (topic_type == "can_msgs/msg/Frame") {
can_msgs::msg::Frame msg;
rclcpp::Serialization<can_msgs::msg::Frame> ser;
ser.deserialize_message(&serialized_msg, &msg);
return rclcpp::Time(msg.header.stamp);
}
// oxts_msgs
if (topic_type == "oxts_msgs/msg/ImuBias") {
oxts_msgs::msg::ImuBias msg;
rclcpp::Serialization<oxts_msgs::msg::ImuBias> ser;
ser.deserialize_message(&serialized_msg, &msg);
return rclcpp::Time(msg.header.stamp);
}
if (topic_type == "oxts_msgs/msg/LeverArm") {
oxts_msgs::msg::LeverArm msg;
rclcpp::Serialization<oxts_msgs::msg::LeverArm> ser;
ser.deserialize_message(&serialized_msg, &msg);
return rclcpp::Time(msg.header.stamp);
}
if (topic_type == "oxts_msgs/msg/NavSatRef") {
oxts_msgs::msg::NavSatRef msg;
rclcpp::Serialization<oxts_msgs::msg::NavSatRef> ser;
ser.deserialize_message(&serialized_msg, &msg);
return rclcpp::Time(msg.header.stamp);
}
if (topic_type == "oxts_msgs/msg/Ncom") {
oxts_msgs::msg::Ncom msg;
rclcpp::Serialization<oxts_msgs::msg::Ncom> ser;
ser.deserialize_message(&serialized_msg, &msg);
return rclcpp::Time(msg.header.stamp);
}
// LiDAR packet types
if (topic_type == "nebula_msgs/msg/NebulaPackets") {
nebula_msgs::msg::NebulaPackets msg;
rclcpp::Serialization<nebula_msgs::msg::NebulaPackets> ser;
ser.deserialize_message(&serialized_msg, &msg);
return rclcpp::Time(msg.header.stamp);
}
if (topic_type == "seyond/msg/SeyondScan") {
seyond::msg::SeyondScan msg;
rclcpp::Serialization<seyond::msg::SeyondScan> ser;
ser.deserialize_message(&serialized_msg, &msg);
return rclcpp::Time(msg.header.stamp);
}
} catch (...) {
return std::nullopt;
}
return std::nullopt;
}

/**
* @brief Override log_time with header.stamp for a pass-through message.
*
Expand Down Expand Up @@ -792,7 +631,6 @@ BagConverterResultStatus run_impl(const BagConverterConfig & config)
return BagConverterResultStatus::kError;
}
// Create output topics based on input metadata
std::set<std::string> created_topics;
for (const auto & topic_info : bag_metadata.topics_with_message_count) {
const auto & topic_metadata = topic_info.topic_metadata;
const auto & topic_type = topic_metadata.type;
Expand All @@ -816,15 +654,12 @@ BagConverterResultStatus run_impl(const BagConverterConfig & config)
pc_topic_meta.type = "sensor_msgs/msg/PointCloud2";
pc_topic_meta.serialization_format = "cdr";
writer.create_topic(pc_topic_meta);
created_topics.insert(output_topic);

if (config.keep_original_topics) {
writer.create_topic(topic_metadata);
created_topics.insert(topic_metadata.name);
}
} else {
writer.create_topic(topic_metadata);
created_topics.insert(topic_metadata.name);
}
}

Expand Down
Loading
Loading