diff --git a/src/bag_converter/include/header_stamp.hpp b/src/bag_converter/include/header_stamp.hpp index 7ea4749..d63636a 100644 --- a/src/bag_converter/include/header_stamp.hpp +++ b/src/bag_converter/include/header_stamp.hpp @@ -56,11 +56,12 @@ inline const std::set 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 extract_header_stamp( const rcutils_uint8_array_t & serialized_data, const std::string & topic_type); diff --git a/src/bag_converter/src/header_stamp.cpp b/src/bag_converter/src/header_stamp.cpp index 5a808d9..74950a6 100644 --- a/src/bag_converter/src/header_stamp.cpp +++ b/src/bag_converter/src/header_stamp.cpp @@ -8,43 +8,25 @@ #include "header_stamp.hpp" -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include namespace bag_converter { -namespace -{ - -template -std::optional deserialize_header_stamp(rclcpp::SerializedMessage & serialized_msg) -{ - MsgT msg; - rclcpp::Serialization 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 extract_header_stamp( const rcutils_uint8_array_t & serialized_data, const std::string & topic_type) @@ -52,56 +34,24 @@ std::optional extract_header_stamp( 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(serialized_msg); - if (topic_type == "sensor_msgs/msg/CompressedImage") - return deserialize_header_stamp(serialized_msg); - if (topic_type == "sensor_msgs/msg/Imu") - return deserialize_header_stamp(serialized_msg); - if (topic_type == "sensor_msgs/msg/Image") - return deserialize_header_stamp(serialized_msg); - if (topic_type == "sensor_msgs/msg/LaserScan") - return deserialize_header_stamp(serialized_msg); - if (topic_type == "sensor_msgs/msg/NavSatFix") - return deserialize_header_stamp(serialized_msg); - if (topic_type == "sensor_msgs/msg/PointCloud2") - return deserialize_header_stamp(serialized_msg); - // geometry_msgs - if (topic_type == "geometry_msgs/msg/AccelStamped") - return deserialize_header_stamp(serialized_msg); - if (topic_type == "geometry_msgs/msg/PoseStamped") - return deserialize_header_stamp(serialized_msg); - if (topic_type == "geometry_msgs/msg/TwistStamped") - return deserialize_header_stamp(serialized_msg); - if (topic_type == "geometry_msgs/msg/WrenchStamped") - return deserialize_header_stamp(serialized_msg); - // nav_msgs - if (topic_type == "nav_msgs/msg/Odometry") - return deserialize_header_stamp(serialized_msg); - // can_msgs - if (topic_type == "can_msgs/msg/Frame") - return deserialize_header_stamp(serialized_msg); - // oxts_msgs - if (topic_type == "oxts_msgs/msg/ImuBias") - return deserialize_header_stamp(serialized_msg); - if (topic_type == "oxts_msgs/msg/LeverArm") - return deserialize_header_stamp(serialized_msg); - if (topic_type == "oxts_msgs/msg/NavSatRef") - return deserialize_header_stamp(serialized_msg); - if (topic_type == "oxts_msgs/msg/Ncom") - return deserialize_header_stamp(serialized_msg); - // LiDAR packet types - if (topic_type == "nebula_msgs/msg/NebulaPackets") - return deserialize_header_stamp(serialized_msg); - if (topic_type == "seyond/msg/SeyondScan") - return deserialize_header_stamp(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