Skip to content

Commit 5ea2a37

Browse files
otenimotenim
authored
refactor: extract header stamp extraction into separate library (#87)
Move kTypesWithHeader, extract_header_stamp, and the deserialization logic into header_stamp.hpp/cpp. This consolidates 18 repetitive deserialize-and-return blocks into a single template helper, making it easier to add new message types. Also removes unused created_topics variable and adds missing <cctype> include for std::toupper.
1 parent 6d45479 commit 5ea2a37

5 files changed

Lines changed: 180 additions & 201 deletions

File tree

‎src/bag_converter/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -57,6 +57,7 @@ endif()
5757
# Build unified bag_converter executable
5858
add_executable(bag_converter_bin
5959
src/bag_converter.cpp
60+
src/header_stamp.cpp
6061
src/memory_management.cpp
6162
src/merge.cpp
6263
src/seyond_decoder.cpp

‎src/bag_converter/include/bag_converter.hpp‎

Lines changed: 1 addition & 35 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@
1010
#ifndef BAG_CONVERTER__BAG_CONVERTER_HPP
1111
#define BAG_CONVERTER__BAG_CONVERTER_HPP
1212

13+
#include "header_stamp.hpp"
1314
#include "memory_management.hpp"
1415
#include "nebula_decoder.hpp"
1516
#include "point_types.hpp"
@@ -57,41 +58,6 @@ inline constexpr int64_t header_stamp_min_epoch_sec = 946'684'800;
5758

5859
} // namespace defaults
5960

60-
/**
61-
* @brief Set of ROS 2 message types that have std_msgs/msg/Header (used for log_time override).
62-
*
63-
* For these types, header.stamp is extracted by deserializing the message and
64-
* reading the header field, so log_time can be overridden from header.stamp when
65-
* --use-header-stamp-as-log-time is enabled.
66-
*/
67-
inline const std::set<std::string> kTypesWithHeader = {
68-
// LiDAR packet types (for log_time override when keeping original topics)
69-
"nebula_msgs/msg/NebulaPackets",
70-
"seyond/msg/SeyondScan",
71-
// sensor_msgs
72-
"sensor_msgs/msg/CameraInfo",
73-
"sensor_msgs/msg/CompressedImage",
74-
"sensor_msgs/msg/Imu",
75-
"sensor_msgs/msg/Image",
76-
"sensor_msgs/msg/LaserScan",
77-
"sensor_msgs/msg/NavSatFix",
78-
"sensor_msgs/msg/PointCloud2",
79-
// geometry_msgs
80-
"geometry_msgs/msg/AccelStamped",
81-
"geometry_msgs/msg/PoseStamped",
82-
"geometry_msgs/msg/TwistStamped",
83-
"geometry_msgs/msg/WrenchStamped",
84-
// nav_msgs
85-
"nav_msgs/msg/Odometry",
86-
// can_msgs
87-
"can_msgs/msg/Frame",
88-
// oxts_msgs
89-
"oxts_msgs/msg/ImuBias",
90-
"oxts_msgs/msg/LeverArm",
91-
"oxts_msgs/msg/NavSatRef",
92-
"oxts_msgs/msg/Ncom",
93-
};
94-
9561
/**
9662
* @brief Return code for conversion functions
9763
*/

‎src/bag_converter/include/header_stamp.hpp‎

Lines changed: 70 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,70 @@
1+
/*
2+
* Copyright (C) 2025 TierIV Inc.
3+
*
4+
* License: Apache License
5+
*
6+
* Header stamp extraction utilities for ROS 2 message types
7+
*/
8+
9+
#ifndef BAG_CONVERTER__HEADER_STAMP_HPP
10+
#define BAG_CONVERTER__HEADER_STAMP_HPP
11+
12+
#include <rclcpp/rclcpp.hpp>
13+
14+
#include <rcutils/types/uint8_array.h>
15+
16+
#include <optional>
17+
#include <set>
18+
#include <string>
19+
20+
namespace bag_converter
21+
{
22+
23+
/**
24+
* @brief Set of ROS 2 message types that have std_msgs/msg/Header (used for log_time override).
25+
*
26+
* For these types, header.stamp is extracted by deserializing the message and
27+
* reading the header field, so log_time can be overridden from header.stamp when
28+
* --use-header-stamp-as-log-time is enabled.
29+
*/
30+
inline const std::set<std::string> kTypesWithHeader = {
31+
// LiDAR packet types (for log_time override when keeping original topics)
32+
"nebula_msgs/msg/NebulaPackets",
33+
"seyond/msg/SeyondScan",
34+
// sensor_msgs
35+
"sensor_msgs/msg/CameraInfo",
36+
"sensor_msgs/msg/CompressedImage",
37+
"sensor_msgs/msg/Imu",
38+
"sensor_msgs/msg/Image",
39+
"sensor_msgs/msg/LaserScan",
40+
"sensor_msgs/msg/NavSatFix",
41+
"sensor_msgs/msg/PointCloud2",
42+
// geometry_msgs
43+
"geometry_msgs/msg/AccelStamped",
44+
"geometry_msgs/msg/PoseStamped",
45+
"geometry_msgs/msg/TwistStamped",
46+
"geometry_msgs/msg/WrenchStamped",
47+
// nav_msgs
48+
"nav_msgs/msg/Odometry",
49+
// can_msgs
50+
"can_msgs/msg/Frame",
51+
// oxts_msgs
52+
"oxts_msgs/msg/ImuBias",
53+
"oxts_msgs/msg/LeverArm",
54+
"oxts_msgs/msg/NavSatRef",
55+
"oxts_msgs/msg/Ncom",
56+
};
57+
58+
/**
59+
* @brief Extract header.stamp by deserializing the message.
60+
*
61+
* For types in kTypesWithHeader, deserializes to the concrete message type and
62+
* returns header.stamp. Returns std::nullopt if the type is unsupported or
63+
* deserialization fails.
64+
*/
65+
std::optional<rclcpp::Time> extract_header_stamp(
66+
const rcutils_uint8_array_t & serialized_data, const std::string & topic_type);
67+
68+
} // namespace bag_converter
69+
70+
#endif // BAG_CONVERTER__HEADER_STAMP_HPP

‎src/bag_converter/src/bag_converter.cpp‎

Lines changed: 1 addition & 166 deletions
Original file line numberDiff line numberDiff line change
@@ -13,28 +13,8 @@
1313
#include "merge.hpp"
1414
#include "tf_transformer.hpp"
1515

16-
#include <seyond/msg/seyond_scan.hpp>
17-
18-
#include <can_msgs/msg/frame.hpp>
19-
#include <geometry_msgs/msg/accel_stamped.hpp>
20-
#include <geometry_msgs/msg/pose_stamped.hpp>
21-
#include <geometry_msgs/msg/twist_stamped.hpp>
22-
#include <geometry_msgs/msg/wrench_stamped.hpp>
23-
#include <nav_msgs/msg/odometry.hpp>
24-
#include <nebula_msgs/msg/nebula_packets.hpp>
25-
#include <oxts_msgs/msg/imu_bias.hpp>
26-
#include <oxts_msgs/msg/lever_arm.hpp>
27-
#include <oxts_msgs/msg/nav_sat_ref.hpp>
28-
#include <oxts_msgs/msg/ncom.hpp>
29-
#include <sensor_msgs/msg/camera_info.hpp>
30-
#include <sensor_msgs/msg/compressed_image.hpp>
31-
#include <sensor_msgs/msg/image.hpp>
32-
#include <sensor_msgs/msg/imu.hpp>
33-
#include <sensor_msgs/msg/laser_scan.hpp>
34-
#include <sensor_msgs/msg/nav_sat_fix.hpp>
35-
#include <sensor_msgs/msg/point_cloud2.hpp>
36-
3716
#include <algorithm>
17+
#include <cctype>
3818
#include <cstring>
3919
#include <filesystem>
4020
#include <fstream>
@@ -50,147 +30,6 @@ static const rclcpp::Logger g_logger = rclcpp::get_logger("bag_converter");
5030
namespace bag_converter
5131
{
5232

53-
/**
54-
* @brief Extract header.stamp by deserializing the message.
55-
*
56-
* For types in kTypesWithHeader, deserializes to the concrete message type and
57-
* returns header.stamp. Returns std::nullopt if the type is unsupported or
58-
* deserialization fails.
59-
*/
60-
static std::optional<rclcpp::Time> extract_header_stamp(
61-
const rcutils_uint8_array_t & serialized_data, const std::string & topic_type)
62-
{
63-
if (kTypesWithHeader.find(topic_type) == kTypesWithHeader.end()) {
64-
return std::nullopt;
65-
}
66-
try {
67-
rclcpp::SerializedMessage serialized_msg(serialized_data);
68-
// sensor_msgs
69-
if (topic_type == "sensor_msgs/msg/CameraInfo") {
70-
sensor_msgs::msg::CameraInfo msg;
71-
rclcpp::Serialization<sensor_msgs::msg::CameraInfo> ser;
72-
ser.deserialize_message(&serialized_msg, &msg);
73-
return rclcpp::Time(msg.header.stamp);
74-
}
75-
if (topic_type == "sensor_msgs/msg/CompressedImage") {
76-
sensor_msgs::msg::CompressedImage msg;
77-
rclcpp::Serialization<sensor_msgs::msg::CompressedImage> ser;
78-
ser.deserialize_message(&serialized_msg, &msg);
79-
return rclcpp::Time(msg.header.stamp);
80-
}
81-
if (topic_type == "sensor_msgs/msg/Imu") {
82-
sensor_msgs::msg::Imu msg;
83-
rclcpp::Serialization<sensor_msgs::msg::Imu> ser;
84-
ser.deserialize_message(&serialized_msg, &msg);
85-
return rclcpp::Time(msg.header.stamp);
86-
}
87-
if (topic_type == "sensor_msgs/msg/Image") {
88-
sensor_msgs::msg::Image msg;
89-
rclcpp::Serialization<sensor_msgs::msg::Image> ser;
90-
ser.deserialize_message(&serialized_msg, &msg);
91-
return rclcpp::Time(msg.header.stamp);
92-
}
93-
if (topic_type == "sensor_msgs/msg/LaserScan") {
94-
sensor_msgs::msg::LaserScan msg;
95-
rclcpp::Serialization<sensor_msgs::msg::LaserScan> ser;
96-
ser.deserialize_message(&serialized_msg, &msg);
97-
return rclcpp::Time(msg.header.stamp);
98-
}
99-
if (topic_type == "sensor_msgs/msg/NavSatFix") {
100-
sensor_msgs::msg::NavSatFix msg;
101-
rclcpp::Serialization<sensor_msgs::msg::NavSatFix> ser;
102-
ser.deserialize_message(&serialized_msg, &msg);
103-
return rclcpp::Time(msg.header.stamp);
104-
}
105-
if (topic_type == "sensor_msgs/msg/PointCloud2") {
106-
sensor_msgs::msg::PointCloud2 msg;
107-
rclcpp::Serialization<sensor_msgs::msg::PointCloud2> ser;
108-
ser.deserialize_message(&serialized_msg, &msg);
109-
return rclcpp::Time(msg.header.stamp);
110-
}
111-
// geometry_msgs
112-
if (topic_type == "geometry_msgs/msg/AccelStamped") {
113-
geometry_msgs::msg::AccelStamped msg;
114-
rclcpp::Serialization<geometry_msgs::msg::AccelStamped> ser;
115-
ser.deserialize_message(&serialized_msg, &msg);
116-
return rclcpp::Time(msg.header.stamp);
117-
}
118-
if (topic_type == "geometry_msgs/msg/PoseStamped") {
119-
geometry_msgs::msg::PoseStamped msg;
120-
rclcpp::Serialization<geometry_msgs::msg::PoseStamped> ser;
121-
ser.deserialize_message(&serialized_msg, &msg);
122-
return rclcpp::Time(msg.header.stamp);
123-
}
124-
if (topic_type == "geometry_msgs/msg/TwistStamped") {
125-
geometry_msgs::msg::TwistStamped msg;
126-
rclcpp::Serialization<geometry_msgs::msg::TwistStamped> ser;
127-
ser.deserialize_message(&serialized_msg, &msg);
128-
return rclcpp::Time(msg.header.stamp);
129-
}
130-
if (topic_type == "geometry_msgs/msg/WrenchStamped") {
131-
geometry_msgs::msg::WrenchStamped msg;
132-
rclcpp::Serialization<geometry_msgs::msg::WrenchStamped> ser;
133-
ser.deserialize_message(&serialized_msg, &msg);
134-
return rclcpp::Time(msg.header.stamp);
135-
}
136-
// nav_msgs
137-
if (topic_type == "nav_msgs/msg/Odometry") {
138-
nav_msgs::msg::Odometry msg;
139-
rclcpp::Serialization<nav_msgs::msg::Odometry> ser;
140-
ser.deserialize_message(&serialized_msg, &msg);
141-
return rclcpp::Time(msg.header.stamp);
142-
}
143-
// can_msgs
144-
if (topic_type == "can_msgs/msg/Frame") {
145-
can_msgs::msg::Frame msg;
146-
rclcpp::Serialization<can_msgs::msg::Frame> ser;
147-
ser.deserialize_message(&serialized_msg, &msg);
148-
return rclcpp::Time(msg.header.stamp);
149-
}
150-
// oxts_msgs
151-
if (topic_type == "oxts_msgs/msg/ImuBias") {
152-
oxts_msgs::msg::ImuBias msg;
153-
rclcpp::Serialization<oxts_msgs::msg::ImuBias> ser;
154-
ser.deserialize_message(&serialized_msg, &msg);
155-
return rclcpp::Time(msg.header.stamp);
156-
}
157-
if (topic_type == "oxts_msgs/msg/LeverArm") {
158-
oxts_msgs::msg::LeverArm msg;
159-
rclcpp::Serialization<oxts_msgs::msg::LeverArm> ser;
160-
ser.deserialize_message(&serialized_msg, &msg);
161-
return rclcpp::Time(msg.header.stamp);
162-
}
163-
if (topic_type == "oxts_msgs/msg/NavSatRef") {
164-
oxts_msgs::msg::NavSatRef msg;
165-
rclcpp::Serialization<oxts_msgs::msg::NavSatRef> ser;
166-
ser.deserialize_message(&serialized_msg, &msg);
167-
return rclcpp::Time(msg.header.stamp);
168-
}
169-
if (topic_type == "oxts_msgs/msg/Ncom") {
170-
oxts_msgs::msg::Ncom msg;
171-
rclcpp::Serialization<oxts_msgs::msg::Ncom> ser;
172-
ser.deserialize_message(&serialized_msg, &msg);
173-
return rclcpp::Time(msg.header.stamp);
174-
}
175-
// LiDAR packet types
176-
if (topic_type == "nebula_msgs/msg/NebulaPackets") {
177-
nebula_msgs::msg::NebulaPackets msg;
178-
rclcpp::Serialization<nebula_msgs::msg::NebulaPackets> ser;
179-
ser.deserialize_message(&serialized_msg, &msg);
180-
return rclcpp::Time(msg.header.stamp);
181-
}
182-
if (topic_type == "seyond/msg/SeyondScan") {
183-
seyond::msg::SeyondScan msg;
184-
rclcpp::Serialization<seyond::msg::SeyondScan> ser;
185-
ser.deserialize_message(&serialized_msg, &msg);
186-
return rclcpp::Time(msg.header.stamp);
187-
}
188-
} catch (...) {
189-
return std::nullopt;
190-
}
191-
return std::nullopt;
192-
}
193-
19433
/**
19534
* @brief Override log_time with header.stamp for a pass-through message.
19635
*
@@ -792,7 +631,6 @@ BagConverterResultStatus run_impl(const BagConverterConfig & config)
792631
return BagConverterResultStatus::kError;
793632
}
794633
// Create output topics based on input metadata
795-
std::set<std::string> created_topics;
796634
for (const auto & topic_info : bag_metadata.topics_with_message_count) {
797635
const auto & topic_metadata = topic_info.topic_metadata;
798636
const auto & topic_type = topic_metadata.type;
@@ -816,15 +654,12 @@ BagConverterResultStatus run_impl(const BagConverterConfig & config)
816654
pc_topic_meta.type = "sensor_msgs/msg/PointCloud2";
817655
pc_topic_meta.serialization_format = "cdr";
818656
writer.create_topic(pc_topic_meta);
819-
created_topics.insert(output_topic);
820657

821658
if (config.keep_original_topics) {
822659
writer.create_topic(topic_metadata);
823-
created_topics.insert(topic_metadata.name);
824660
}
825661
} else {
826662
writer.create_topic(topic_metadata);
827-
created_topics.insert(topic_metadata.name);
828663
}
829664
}
830665

0 commit comments

Comments
 (0)