From c5354bb9c681903e98eb004abeadd75f39b83c1e Mon Sep 17 00:00:00 2001 From: James Smith Date: Thu, 17 Nov 2022 15:41:50 +1100 Subject: [PATCH] rosbag2_storage_mcap: satisfy uncrustify Signed-off-by: James Smith --- .../message_definition_cache.hpp | 45 +- rosbag2_storage_mcap/src/mcap_storage.cpp | 394 ++++++++++-------- .../src/message_definition_cache.cpp | 90 ++-- .../test_mcap_storage.cpp | 8 +- .../test_message_definition_cache.cpp | 33 +- 5 files changed, 329 insertions(+), 241 deletions(-) diff --git a/rosbag2_storage_mcap/include/rosbag2_storage_mcap/message_definition_cache.hpp b/rosbag2_storage_mcap/include/rosbag2_storage_mcap/message_definition_cache.hpp index 64fa8f2719..a8e532a56d 100644 --- a/rosbag2_storage_mcap/include/rosbag2_storage_mcap/message_definition_cache.hpp +++ b/rosbag2_storage_mcap/include/rosbag2_storage_mcap/message_definition_cache.hpp @@ -23,43 +23,51 @@ #include #include -namespace rosbag2_storage_mcap::internal { +namespace rosbag2_storage_mcap::internal +{ -enum struct Format { +enum struct Format +{ IDL, MSG, }; -struct MessageSpec { - MessageSpec(Format format, std::string text, const std::string& package_context); +struct MessageSpec +{ + MessageSpec(Format format, std::string text, const std::string & package_context); const std::set dependencies; const std::string text; Format format; }; -struct DefinitionIdentifier { +struct DefinitionIdentifier +{ Format format; std::string package_resource_name; - bool operator==(const DefinitionIdentifier& di) const { + bool operator==(const DefinitionIdentifier & di) const + { return (format == di.format) && (package_resource_name == di.package_resource_name); } }; -class DefinitionNotFoundError : public std::exception { +class DefinitionNotFoundError : public std::exception +{ private: std::string name_; public: explicit DefinitionNotFoundError(std::string name) - : name_(std::move(name)) {} + : name_(std::move(name)) {} - const char* what() const noexcept override { + const char * what() const noexcept override + { return name_.c_str(); } }; -class MessageDefinitionCache final { +class MessageDefinitionCache final +{ public: /** * Concatenate the message definition with its dependencies into a self-contained schema. @@ -70,11 +78,13 @@ class MessageDefinitionCache final { * package resource name. */ ROSBAG2_STORAGE_MCAP_PUBLIC - std::pair get_full_text(const std::string& package_resource_name); + std::pair get_full_text(const std::string & package_resource_name); private: - struct DefinitionIdentifierHash { - std::size_t operator()(const DefinitionIdentifier& di) const { + struct DefinitionIdentifierHash + { + std::size_t operator()(const DefinitionIdentifier & di) const + { std::size_t h1 = std::hash()(di.format); std::size_t h2 = std::hash()(di.package_resource_name); return h1 ^ h2; @@ -84,15 +94,16 @@ class MessageDefinitionCache final { * Load and parse the message file referenced by the given datatype, or return it from * msg_specs_by_datatype */ - const MessageSpec& load_message_spec(const DefinitionIdentifier& definition_identifier); + const MessageSpec & load_message_spec(const DefinitionIdentifier & definition_identifier); std::unordered_map - msg_specs_by_definition_identifier_; + msg_specs_by_definition_identifier_; }; ROSBAG2_STORAGE_MCAP_PUBLIC -std::set parse_dependencies(Format format, const std::string& text, - const std::string& package_context); +std::set parse_dependencies( + Format format, const std::string & text, + const std::string & package_context); } // namespace rosbag2_storage_mcap::internal diff --git a/rosbag2_storage_mcap/src/mcap_storage.cpp b/rosbag2_storage_mcap/src/mcap_storage.cpp index 51223e2052..75c28fafd6 100644 --- a/rosbag2_storage_mcap/src/mcap_storage.cpp +++ b/rosbag2_storage_mcap/src/mcap_storage.cpp @@ -21,11 +21,11 @@ #ifdef ROSBAG2_STORAGE_MCAP_HAS_YAML_HPP #include "rosbag2_storage/yaml.hpp" #else - // COMPATIBILITY(foxy, galactic) - this block is available in rosbag2_storage/yaml.hpp in H +// COMPATIBILITY(foxy, galactic) - this block is available in rosbag2_storage/yaml.hpp in H #ifdef _WIN32 - // This is necessary because of a bug in yaml-cpp's cmake +// This is necessary because of a bug in yaml-cpp's cmake #define YAML_CPP_DLL - // This is necessary because yaml-cpp does not always use dllimport/dllexport consistently +// This is necessary because yaml-cpp does not always use dllimport/dllexport consistently #pragma warning(push) #pragma warning(disable : 4251) #pragma warning(disable : 4275) @@ -50,69 +50,77 @@ #include #endif -#define DECLARE_YAML_VALUE_MAP(KEY_TYPE, VALUE_TYPE, ...) \ - template <> \ - struct convert { \ - static Node encode(const KEY_TYPE& e) { \ +#define DECLARE_YAML_VALUE_MAP(KEY_TYPE, VALUE_TYPE, ...) \ + template<> \ + struct convert { \ + static Node encode(const KEY_TYPE & e) { \ static const std::pair mapping[] = __VA_ARGS__; \ - for (const auto& m : mapping) { \ - if (m.first == e) { \ - return Node(m.second); \ - } \ - } \ - return Node(""); \ - } \ - \ - static bool decode(const Node& node, KEY_TYPE& e) { \ + for (const auto & m : mapping) { \ + if (m.first == e) { \ + return Node(m.second); \ + } \ + } \ + return Node(""); \ + } \ + \ + static bool decode(const Node & node, KEY_TYPE & e) { \ static const std::pair mapping[] = __VA_ARGS__; \ - const auto val = node.as(); \ - for (const auto& m : mapping) { \ - if (m.second == val) { \ - e = m.first; \ - return true; \ - } \ - } \ - return false; \ - } \ + const auto val = node.as(); \ + for (const auto & m : mapping) { \ + if (m.second == val) { \ + e = m.first; \ + return true; \ + } \ + } \ + return false; \ + } \ } -namespace { +namespace +{ // Simple wrapper with default constructor for use by YAML -struct McapWriterOptions : mcap::McapWriterOptions { +struct McapWriterOptions : mcap::McapWriterOptions +{ McapWriterOptions() - : mcap::McapWriterOptions("ros2") {} + : mcap::McapWriterOptions("ros2") {} }; } // namespace -namespace YAML { +namespace YAML +{ #ifndef ROSBAG2_STORAGE_MCAP_HAS_YAML_HPP -template -void optional_assign(const Node& node, std::string field, T& assign_to) { +template +void optional_assign(const Node & node, std::string field, T & assign_to) +{ if (node[field]) { assign_to = node[field].as(); } } #endif -DECLARE_YAML_VALUE_MAP(mcap::Compression, std::string, - {{mcap::Compression::None, "None"}, - {mcap::Compression::Lz4, "Lz4"}, - {mcap::Compression::Zstd, "Zstd"}}); - -DECLARE_YAML_VALUE_MAP(mcap::CompressionLevel, std::string, - {{mcap::CompressionLevel::Fastest, "Fastest"}, - {mcap::CompressionLevel::Fast, "Fast"}, - {mcap::CompressionLevel::Default, "Default"}, - {mcap::CompressionLevel::Slow, "Slow"}, - {mcap::CompressionLevel::Slowest, "Slowest"}}); - -template <> -struct convert { +DECLARE_YAML_VALUE_MAP( + mcap::Compression, std::string, + {{mcap::Compression::None, "None"}, + {mcap::Compression::Lz4, "Lz4"}, + {mcap::Compression::Zstd, "Zstd"}}); + +DECLARE_YAML_VALUE_MAP( + mcap::CompressionLevel, std::string, + {{mcap::CompressionLevel::Fastest, "Fastest"}, + {mcap::CompressionLevel::Fast, "Fast"}, + {mcap::CompressionLevel::Default, "Default"}, + {mcap::CompressionLevel::Slow, "Slow"}, + {mcap::CompressionLevel::Slowest, "Slowest"}}); + +template<> +struct convert +{ // NOTE: when updating this struct, also update documentation in README.md - static bool decode(const Node& node, McapWriterOptions& o) { + static bool decode(const Node & node, McapWriterOptions & o) + { optional_assign(node, "noChunkCRC", o.noChunkCRC); optional_assign(node, "noAttachmentCRC", o.noAttachmentCRC); optional_assign(node, "enableDataCRC", o.enableDataCRC); @@ -138,36 +146,42 @@ struct convert { } // namespace YAML -namespace rosbag2_storage_plugins { +namespace rosbag2_storage_plugins +{ using mcap::ByteOffset; using time_point = std::chrono::time_point; static const char FILE_EXTENSION[] = ".mcap"; static const char LOG_NAME[] = "rosbag2_storage_mcap"; -static void OnProblem(const mcap::Status& status) { +static void OnProblem(const mcap::Status & status) +{ RCUTILS_LOG_ERROR_NAMED(LOG_NAME, "%s", status.message.c_str()); } /** * A storage implementation for the MCAP file format. */ -class MCAPStorage : public rosbag2_storage::storage_interfaces::ReadWriteInterface { +class MCAPStorage : public rosbag2_storage::storage_interfaces::ReadWriteInterface +{ public: MCAPStorage(); ~MCAPStorage() override; /** BaseIOInterface **/ #ifdef ROSBAG2_STORAGE_MCAP_HAS_STORAGE_OPTIONS - void open(const rosbag2_storage::StorageOptions& storage_options, - rosbag2_storage::storage_interfaces::IOFlag io_flag = - rosbag2_storage::storage_interfaces::IOFlag::READ_WRITE) override; - void open(const std::string& uri, rosbag2_storage::storage_interfaces::IOFlag io_flag = - rosbag2_storage::storage_interfaces::IOFlag::READ_WRITE); + void open( + const rosbag2_storage::StorageOptions & storage_options, + rosbag2_storage::storage_interfaces::IOFlag io_flag = + rosbag2_storage::storage_interfaces::IOFlag::READ_WRITE) override; + void open( + const std::string & uri, rosbag2_storage::storage_interfaces::IOFlag io_flag = + rosbag2_storage::storage_interfaces::IOFlag::READ_WRITE); #else - void open(const std::string& uri, - rosbag2_storage::storage_interfaces::IOFlag io_flag = - rosbag2_storage::storage_interfaces::IOFlag::READ_WRITE) override; + void open( + const std::string & uri, + rosbag2_storage::storage_interfaces::IOFlag io_flag = + rosbag2_storage::storage_interfaces::IOFlag::READ_WRITE) override; #endif /** BaseInfoInterface **/ @@ -178,19 +192,19 @@ class MCAPStorage : public rosbag2_storage::storage_interfaces::ReadWriteInterfa /** BaseReadInterface **/ #ifdef ROSBAG2_STORAGE_MCAP_HAS_SET_READ_ORDER - void set_read_order(const rosbag2_storage::ReadOrder&) override; + void set_read_order(const rosbag2_storage::ReadOrder &) override; #endif bool has_next() override; std::shared_ptr read_next() override; std::vector get_all_topics_and_types() override; /** ReadOnlyInterface **/ - void set_filter(const rosbag2_storage::StorageFilter& storage_filter) override; + void set_filter(const rosbag2_storage::StorageFilter & storage_filter) override; void reset_filter() override; #ifdef ROSBAG2_STORAGE_MCAP_OVERRIDE_SEEK_METHOD - void seek(const rcutils_time_point_value_t& time_stamp) override; + void seek(const rcutils_time_point_value_t & time_stamp) override; #else - void seek(const rcutils_time_point_value_t& timestamp); + void seek(const rcutils_time_point_value_t & timestamp); #endif /** ReadWriteInterface **/ @@ -199,14 +213,15 @@ class MCAPStorage : public rosbag2_storage::storage_interfaces::ReadWriteInterfa /** BaseWriteInterface **/ void write(std::shared_ptr msg) override; void write( - const std::vector>& msg) override; - void create_topic(const rosbag2_storage::TopicMetadata& topic) override; - void remove_topic(const rosbag2_storage::TopicMetadata& topic) override; + const std::vector> & msg) override; + void create_topic(const rosbag2_storage::TopicMetadata & topic) override; + void remove_topic(const rosbag2_storage::TopicMetadata & topic) override; private: - void open_impl(const std::string& uri, const std::string& preset_profile, - rosbag2_storage::storage_interfaces::IOFlag io_flag, - const std::string& storage_config_uri); + void open_impl( + const std::string & uri, const std::string & preset_profile, + rosbag2_storage::storage_interfaces::IOFlag io_flag, + const std::string & storage_config_uri); void reset_iterator(rcutils_time_point_value_t start_time = 0); bool read_and_enqueue_message(); @@ -237,12 +252,14 @@ class MCAPStorage : public rosbag2_storage::storage_interfaces::ReadWriteInterfa bool has_read_summary_ = false; }; -MCAPStorage::MCAPStorage() { +MCAPStorage::MCAPStorage() +{ metadata_.storage_identifier = get_storage_identifier(); metadata_.message_count = 0; } -MCAPStorage::~MCAPStorage() { +MCAPStorage::~MCAPStorage() +{ if (mcap_reader_) { mcap_reader_->close(); } @@ -256,19 +273,25 @@ MCAPStorage::~MCAPStorage() { /** BaseIOInterface **/ #ifdef ROSBAG2_STORAGE_MCAP_HAS_STORAGE_OPTIONS -void MCAPStorage::open(const rosbag2_storage::StorageOptions& storage_options, - rosbag2_storage::storage_interfaces::IOFlag io_flag) { - open_impl(storage_options.uri, storage_options.storage_preset_profile, io_flag, - storage_options.storage_config_uri); +void MCAPStorage::open( + const rosbag2_storage::StorageOptions & storage_options, + rosbag2_storage::storage_interfaces::IOFlag io_flag) +{ + open_impl( + storage_options.uri, storage_options.storage_preset_profile, io_flag, + storage_options.storage_config_uri); } #endif -void MCAPStorage::open(const std::string& uri, - rosbag2_storage::storage_interfaces::IOFlag io_flag) { +void MCAPStorage::open( + const std::string & uri, + rosbag2_storage::storage_interfaces::IOFlag io_flag) +{ open_impl(uri, "", io_flag, ""); } -static void SetOptionsForPreset(const std::string& preset_profile, McapWriterOptions& options) { +static void SetOptionsForPreset(const std::string & preset_profile, McapWriterOptions & options) +{ if (preset_profile == "fastwrite") { options.noChunking = true; options.noSummaryCRC = true; @@ -282,63 +305,66 @@ static void SetOptionsForPreset(const std::string& preset_profile, McapWriterOpt options.chunkSize = 4 * 1024 * 1024; } else { throw std::runtime_error( - "unknown MCAP storage preset profile " - "(valid options are 'fastwrite', 'zstd_fast', 'zstd_small'): " + - preset_profile); + "unknown MCAP storage preset profile " + "(valid options are 'fastwrite', 'zstd_fast', 'zstd_small'): " + + preset_profile); } } -void MCAPStorage::open_impl(const std::string& uri, const std::string& preset_profile, - rosbag2_storage::storage_interfaces::IOFlag io_flag, - const std::string& storage_config_uri) { +void MCAPStorage::open_impl( + const std::string & uri, const std::string & preset_profile, + rosbag2_storage::storage_interfaces::IOFlag io_flag, + const std::string & storage_config_uri) +{ switch (io_flag) { case rosbag2_storage::storage_interfaces::IOFlag::READ_ONLY: { - relative_path_ = uri; - input_ = std::make_unique(relative_path_, std::ios::binary); - data_source_ = std::make_unique(*input_); - mcap_reader_ = std::make_unique(); - auto status = mcap_reader_->open(*data_source_); - if (!status.ok()) { - throw std::runtime_error(status.message); + relative_path_ = uri; + input_ = std::make_unique(relative_path_, std::ios::binary); + data_source_ = std::make_unique(*input_); + mcap_reader_ = std::make_unique(); + auto status = mcap_reader_->open(*data_source_); + if (!status.ok()) { + throw std::runtime_error(status.message); + } + reset_iterator(); + break; } - reset_iterator(); - break; - } case rosbag2_storage::storage_interfaces::IOFlag::READ_WRITE: case rosbag2_storage::storage_interfaces::IOFlag::APPEND: { - // APPEND does not seem to be used; treat it the same as READ_WRITE - io_flag = rosbag2_storage::storage_interfaces::IOFlag::READ_WRITE; - relative_path_ = uri + FILE_EXTENSION; - - mcap_writer_ = std::make_unique(); - McapWriterOptions options; - // Set defaults for the rosbag2 storage plugin specifically. - options.noChunkCRC = true; - options.compression = mcap::Compression::None; - // Set options from preset profile first - if (!preset_profile.empty()) { - SetOptionsForPreset(preset_profile, options); - } - // If both preset profile and storage config are specified, - // options from the storage config are overlaid on the options from the preset profile. - if (!storage_config_uri.empty()) { - YAML::Node yaml_node = YAML::LoadFile(storage_config_uri); - YAML::convert::decode(yaml_node, options); - } + // APPEND does not seem to be used; treat it the same as READ_WRITE + io_flag = rosbag2_storage::storage_interfaces::IOFlag::READ_WRITE; + relative_path_ = uri + FILE_EXTENSION; + + mcap_writer_ = std::make_unique(); + McapWriterOptions options; + // Set defaults for the rosbag2 storage plugin specifically. + options.noChunkCRC = true; + options.compression = mcap::Compression::None; + // Set options from preset profile first + if (!preset_profile.empty()) { + SetOptionsForPreset(preset_profile, options); + } + // If both preset profile and storage config are specified, + // options from the storage config are overlaid on the options from the preset profile. + if (!storage_config_uri.empty()) { + YAML::Node yaml_node = YAML::LoadFile(storage_config_uri); + YAML::convert::decode(yaml_node, options); + } - auto status = mcap_writer_->open(relative_path_, options); - if (!status.ok()) { - throw std::runtime_error(status.message); + auto status = mcap_writer_->open(relative_path_, options); + if (!status.ok()) { + throw std::runtime_error(status.message); + } + break; } - break; - } } opened_as_ = io_flag; metadata_.relative_file_paths = {get_relative_file_path()}; } /** BaseInfoInterface **/ -rosbag2_storage::BagMetadata MCAPStorage::get_metadata() { +rosbag2_storage::BagMetadata MCAPStorage::get_metadata() +{ ensure_summary_read(); metadata_.version = 2; @@ -347,15 +373,15 @@ rosbag2_storage::BagMetadata MCAPStorage::get_metadata() { metadata_.relative_file_paths = {get_relative_file_path()}; // Fill out summary metadata from the Statistics record - const mcap::Statistics& stats = mcap_reader_->statistics().value(); + const mcap::Statistics & stats = mcap_reader_->statistics().value(); metadata_.message_count = stats.messageCount; metadata_.duration = std::chrono::nanoseconds(stats.messageEndTime - stats.messageStartTime); metadata_.starting_time = time_point(std::chrono::nanoseconds(stats.messageStartTime)); // Build a list of topic information along with per-topic message counts metadata_.topics_with_message_count.clear(); - for (const auto& [channel_id, channel_ptr] : mcap_reader_->channels()) { - const mcap::Channel& channel = *channel_ptr; + for (const auto & [channel_id, channel_ptr] : mcap_reader_->channels()) { + const mcap::Channel & channel = *channel_ptr; // Look up the Schema for this topic const auto schema_ptr = mcap_reader_->schema(channel.schemaId); @@ -389,28 +415,32 @@ rosbag2_storage::BagMetadata MCAPStorage::get_metadata() { return metadata_; } -std::string MCAPStorage::get_relative_file_path() const { +std::string MCAPStorage::get_relative_file_path() const +{ return relative_path_; } -uint64_t MCAPStorage::get_bagfile_size() const { +uint64_t MCAPStorage::get_bagfile_size() const +{ if (opened_as_ == rosbag2_storage::storage_interfaces::IOFlag::READ_ONLY) { return data_source_ ? data_source_->size() : 0; } else { if (!mcap_writer_) { return 0; } - const auto* data_sink = mcap_writer_->dataSink(); + const auto * data_sink = mcap_writer_->dataSink(); return data_sink ? data_sink->size() : 0; } } -std::string MCAPStorage::get_storage_identifier() const { +std::string MCAPStorage::get_storage_identifier() const +{ return "mcap"; } /** BaseReadInterface **/ -bool MCAPStorage::read_and_enqueue_message() { +bool MCAPStorage::read_and_enqueue_message() +{ // The recording has not been opened. if (!linear_iterator_) { return false; @@ -420,19 +450,20 @@ bool MCAPStorage::read_and_enqueue_message() { return true; } - auto& it = *linear_iterator_; + auto & it = *linear_iterator_; // At the end of the recording if (it == linear_view_->end()) { return false; } - const auto& messageView = *it; + const auto & messageView = *it; auto msg = std::make_shared(); msg->time_stamp = rcutils_time_point_value_t(messageView.message.logTime); msg->topic_name = messageView.channel->topic; - msg->serialized_data = rosbag2_storage::make_serialized_message(messageView.message.data, - messageView.message.dataSize); + msg->serialized_data = rosbag2_storage::make_serialized_message( + messageView.message.data, + messageView.message.dataSize); // enqueue this message to be used next_ = msg; @@ -441,29 +472,30 @@ bool MCAPStorage::read_and_enqueue_message() { return true; } -void MCAPStorage::reset_iterator(rcutils_time_point_value_t start_time) { +void MCAPStorage::reset_iterator(rcutils_time_point_value_t start_time) +{ ensure_summary_read(); mcap::ReadMessageOptions options; options.startTime = mcap::Timestamp(start_time); options.readOrder = read_order_; if (!storage_filter_.topics.empty()) { options.topicFilter = [this](std::string_view topic) { - for (const auto& match_topic : storage_filter_.topics) { - if (match_topic == topic) { - return true; + for (const auto & match_topic : storage_filter_.topics) { + if (match_topic == topic) { + return true; + } } - } - return false; - }; + return false; + }; } #ifdef ROSBAG2_STORAGE_MCAP_HAS_STORAGE_FILTER_TOPIC_REGEX if (!storage_filter_.topics_regex.empty()) { options.topicFilter = [this](std::string_view topic) { - std::smatch m; - std::string topic_string(topic); - std::regex re(storage_filter_.topics_regex); - return std::regex_match(topic_string, m, re); - }; + std::smatch m; + std::string topic_string(topic); + std::regex re(storage_filter_.topics_regex); + return std::regex_match(topic_string, m, re); + }; } #endif linear_view_ = @@ -471,7 +503,8 @@ void MCAPStorage::reset_iterator(rcutils_time_point_value_t start_time) { linear_iterator_ = std::make_unique(linear_view_->begin()); } -void MCAPStorage::ensure_summary_read() { +void MCAPStorage::ensure_summary_read() +{ if (!has_read_summary_) { const auto status = mcap_reader_->readSummary(mcap::ReadSummaryMethod::AllowFallbackScan); @@ -480,15 +513,16 @@ void MCAPStorage::ensure_summary_read() { } // check if message indexes are present, if not, read in file order. bool message_indexes_found = false; - for (const auto& ci : mcap_reader_->chunkIndexes()) { + for (const auto & ci : mcap_reader_->chunkIndexes()) { if (ci.messageIndexLength > 0) { message_indexes_found = true; break; } } if (!message_indexes_found) { - RCUTILS_LOG_WARN_NAMED(LOG_NAME, - "no message indices found, falling back to reading in file order"); + RCUTILS_LOG_WARN_NAMED( + LOG_NAME, + "no message indices found, falling back to reading in file order"); read_order_ = mcap::ReadMessageOptions::ReadOrder::FileOrder; } has_read_summary_ = true; @@ -496,7 +530,8 @@ void MCAPStorage::ensure_summary_read() { } #ifdef ROSBAG2_STORAGE_MCAP_HAS_SET_READ_ORDER -void MCAPStorage::set_read_order(const rosbag2_storage::ReadOrder& read_order) { +void MCAPStorage::set_read_order(const rosbag2_storage::ReadOrder & read_order) +{ auto next_read_order = read_order_; switch (read_order.sort_by) { case rosbag2_storage::ReadOrder::ReceivedTimestamp: @@ -524,7 +559,8 @@ void MCAPStorage::set_read_order(const rosbag2_storage::ReadOrder& read_order) { } #endif -bool MCAPStorage::has_next() { +bool MCAPStorage::has_next() +{ if (!linear_iterator_) { return false; } @@ -536,7 +572,8 @@ bool MCAPStorage::has_next() { return read_and_enqueue_message(); } -std::shared_ptr MCAPStorage::read_next() { +std::shared_ptr MCAPStorage::read_next() +{ if (!has_next()) { throw std::runtime_error{"No next message is available."}; } @@ -544,36 +581,42 @@ std::shared_ptr MCAPStorage::read_next() return std::move(next_); } -std::vector MCAPStorage::get_all_topics_and_types() { +std::vector MCAPStorage::get_all_topics_and_types() +{ auto metadata = get_metadata(); std::vector out; - for (const auto& topic : metadata.topics_with_message_count) { + for (const auto & topic : metadata.topics_with_message_count) { out.push_back(topic.topic_metadata); } return out; } /** ReadOnlyInterface **/ -void MCAPStorage::set_filter(const rosbag2_storage::StorageFilter& storage_filter) { +void MCAPStorage::set_filter(const rosbag2_storage::StorageFilter & storage_filter) +{ storage_filter_ = storage_filter; reset_iterator(); } -void MCAPStorage::reset_filter() { +void MCAPStorage::reset_filter() +{ set_filter(rosbag2_storage::StorageFilter()); } -void MCAPStorage::seek(const rcutils_time_point_value_t& time_stamp) { +void MCAPStorage::seek(const rcutils_time_point_value_t & time_stamp) +{ reset_iterator(time_stamp); } /** ReadWriteInterface **/ -uint64_t MCAPStorage::get_minimum_split_file_size() const { +uint64_t MCAPStorage::get_minimum_split_file_size() const +{ return 1024; } /** BaseWriteInterface **/ -void MCAPStorage::write(std::shared_ptr msg) { +void MCAPStorage::write(std::shared_ptr msg) +{ const auto topic_it = topics_.find(msg->topic_name); if (topic_it == topics_.end()) { throw std::runtime_error{"Unknown message topic \"" + msg->topic_name + "\""}; @@ -595,12 +638,12 @@ void MCAPStorage::write(std::shared_ptrtime_stamp); mcap_msg.publishTime = mcap_msg.logTime; mcap_msg.dataSize = msg->serialized_data->buffer_length; - mcap_msg.data = reinterpret_cast(msg->serialized_data->buffer); + mcap_msg.data = reinterpret_cast(msg->serialized_data->buffer); const auto status = mcap_writer_->write(mcap_msg); if (!status.ok()) { throw std::runtime_error{std::string{"Failed to write "} + - std::to_string(msg->serialized_data->buffer_length) + - " byte message to MCAP file: " + status.message}; + std::to_string(msg->serialized_data->buffer_length) + + " byte message to MCAP file: " + status.message}; } /// Update metadata @@ -614,13 +657,15 @@ void MCAPStorage::write(std::shared_ptr>& msgs) { - for (const auto& msg : msgs) { + const std::vector> & msgs) +{ + for (const auto & msg : msgs) { write(msg); } } -void MCAPStorage::create_topic(const rosbag2_storage::TopicMetadata& topic) { +void MCAPStorage::create_topic(const rosbag2_storage::TopicMetadata & topic) +{ auto topic_info = rosbag2_storage::TopicInformation{topic, 0}; const auto topic_it = topics_.find(topic.name); if (topic_it == topics_.end()) { @@ -631,7 +676,7 @@ void MCAPStorage::create_topic(const rosbag2_storage::TopicMetadata& topic) { } // Create Schema for topic if it doesn't exist yet - const auto& datatype = topic_info.topic_metadata.type; + const auto & datatype = topic_info.topic_metadata.type; const auto schema_it = schema_ids_.find(datatype); mcap::SchemaId schema_id; if (schema_it == schema_ids_.end()) { @@ -644,11 +689,13 @@ void MCAPStorage::create_topic(const rosbag2_storage::TopicMetadata& topic) { } else { schema.encoding = "ros2idl"; } - schema.data.assign(reinterpret_cast(full_text.data()), - reinterpret_cast(full_text.data() + full_text.size())); - } catch (rosbag2_storage_mcap::internal::DefinitionNotFoundError& err) { - RCUTILS_LOG_ERROR_NAMED(LOG_NAME, "definition file(s) missing for %s: missing %s", - datatype.c_str(), err.what()); + schema.data.assign( + reinterpret_cast(full_text.data()), + reinterpret_cast(full_text.data() + full_text.size())); + } catch (rosbag2_storage_mcap::internal::DefinitionNotFoundError & err) { + RCUTILS_LOG_ERROR_NAMED( + LOG_NAME, "definition file(s) missing for %s: missing %s", + datatype.c_str(), err.what()); schema.encoding = ""; } mcap_writer_->addSchema(schema); @@ -665,19 +712,22 @@ void MCAPStorage::create_topic(const rosbag2_storage::TopicMetadata& topic) { channel.topic = topic.name; channel.messageEncoding = topic_info.topic_metadata.serialization_format; channel.schemaId = schema_id; - channel.metadata.emplace("offered_qos_profiles", - topic_info.topic_metadata.offered_qos_profiles); + channel.metadata.emplace( + "offered_qos_profiles", + topic_info.topic_metadata.offered_qos_profiles); mcap_writer_->addChannel(channel); channel_ids_.emplace(topic.name, channel.id); } } -void MCAPStorage::remove_topic(const rosbag2_storage::TopicMetadata& topic) { +void MCAPStorage::remove_topic(const rosbag2_storage::TopicMetadata & topic) +{ topics_.erase(topic.name); } } // namespace rosbag2_storage_plugins #include "pluginlib/class_list_macros.hpp" // NOLINT -PLUGINLIB_EXPORT_CLASS(rosbag2_storage_plugins::MCAPStorage, - rosbag2_storage::storage_interfaces::ReadWriteInterface) +PLUGINLIB_EXPORT_CLASS( + rosbag2_storage_plugins::MCAPStorage, + rosbag2_storage::storage_interfaces::ReadWriteInterface) diff --git a/rosbag2_storage_mcap/src/message_definition_cache.cpp b/rosbag2_storage_mcap/src/message_definition_cache.cpp index 0d08d0922a..7fd3faed76 100644 --- a/rosbag2_storage_mcap/src/message_definition_cache.cpp +++ b/rosbag2_storage_mcap/src/message_definition_cache.cpp @@ -28,7 +28,8 @@ #include #include -namespace rosbag2_storage_mcap::internal { +namespace rosbag2_storage_mcap::internal +{ // Match datatype names (foo_msgs/Bar or foo_msgs/msg/Bar) static const std::regex PACKAGE_TYPENAME_REGEX{R"(^([a-zA-Z0-9_]+)/(?:msg/)?([a-zA-Z0-9_]+)$)"}; @@ -41,15 +42,18 @@ static const std::regex IDL_FIELD_TYPE_REGEX{ R"((?:^|\n)#include\s+(?:"|<)([a-zA-Z0-9_/]+)\.idl(?:"|>))"}; static const std::unordered_set PRIMITIVE_TYPES{ - "bool", "byte", "char", "float32", "float64", "int8", "uint8", - "int16", "uint16", "int32", "uint32", "int64", "uint64", "string"}; + "bool", "byte", "char", "float32", "float64", "int8", "uint8", + "int16", "uint16", "int32", "uint32", "int64", "uint64", "string"}; -static std::set parse_msg_dependencies(const std::string& text, - const std::string& package_context) { +static std::set parse_msg_dependencies( + const std::string & text, + const std::string & package_context) +{ std::set dependencies; for (std::sregex_iterator iter(text.begin(), text.end(), MSG_FIELD_TYPE_REGEX); - iter != std::sregex_iterator(); ++iter) { + iter != std::sregex_iterator(); ++iter) + { std::string type = (*iter)[1]; if (PRIMITIVE_TYPES.find(type) != PRIMITIVE_TYPES.end()) { continue; @@ -63,18 +67,22 @@ static std::set parse_msg_dependencies(const std::string& text, return dependencies; } -static std::set parse_idl_dependencies(const std::string& text) { +static std::set parse_idl_dependencies(const std::string & text) +{ std::set dependencies; for (std::sregex_iterator iter(text.begin(), text.end(), IDL_FIELD_TYPE_REGEX); - iter != std::sregex_iterator(); ++iter) { + iter != std::sregex_iterator(); ++iter) + { dependencies.insert((*iter)[1]); } return dependencies; } -std::set parse_dependencies(Format format, const std::string& text, - const std::string& package_context) { +std::set parse_dependencies( + Format format, const std::string & text, + const std::string & package_context) +{ switch (format) { case Format::MSG: return parse_msg_dependencies(text, package_context); @@ -85,7 +93,8 @@ std::set parse_dependencies(Format format, const std::string& text, } } -static const char* extension_for_format(Format format) { +static const char * extension_for_format(Format format) +{ switch (format) { case Format::MSG: return ".msg"; @@ -96,7 +105,8 @@ static const char* extension_for_format(Format format) { } } -static std::string delimiter(const DefinitionIdentifier& definition_identifier) { +static std::string delimiter(const DefinitionIdentifier & definition_identifier) +{ std::string result = "================================================================================\n"; switch (definition_identifier.format) { @@ -114,37 +124,43 @@ static std::string delimiter(const DefinitionIdentifier& definition_identifier) return result; } -MessageSpec::MessageSpec(Format format, std::string text, const std::string& package_context) - : dependencies(parse_dependencies(format, text, package_context)) - , text(std::move(text)) - , format(format) {} +MessageSpec::MessageSpec(Format format, std::string text, const std::string & package_context) +: dependencies(parse_dependencies(format, text, package_context)) + , text(std::move(text)) + , format(format) {} -const MessageSpec& MessageDefinitionCache::load_message_spec( - const DefinitionIdentifier& definition_identifier) { +const MessageSpec & MessageDefinitionCache::load_message_spec( + const DefinitionIdentifier & definition_identifier) +{ if (auto it = msg_specs_by_definition_identifier_.find(definition_identifier); - it != msg_specs_by_definition_identifier_.end()) { + it != msg_specs_by_definition_identifier_.end()) + { return it->second; } std::smatch match; - if (!std::regex_match(definition_identifier.package_resource_name, match, - PACKAGE_TYPENAME_REGEX)) { - throw std::invalid_argument("Invalid package resource name: " + - definition_identifier.package_resource_name); + if (!std::regex_match( + definition_identifier.package_resource_name, match, + PACKAGE_TYPENAME_REGEX)) + { + throw std::invalid_argument( + "Invalid package resource name: " + + definition_identifier.package_resource_name); } std::string package = match[1]; std::string share_dir = ament_index_cpp::get_package_share_directory(package); std::ifstream file{share_dir + "/msg/" + match[2].str() + - extension_for_format(definition_identifier.format)}; + extension_for_format(definition_identifier.format)}; if (!file.good()) { throw DefinitionNotFoundError(definition_identifier.package_resource_name); } std::string contents{std::istreambuf_iterator(file), {}}; - const MessageSpec& spec = + const MessageSpec & spec = msg_specs_by_definition_identifier_ - .emplace(definition_identifier, - MessageSpec(definition_identifier.format, std::move(contents), package)) - .first->second; + .emplace( + definition_identifier, + MessageSpec(definition_identifier.format, std::move(contents), package)) + .first->second; // "References and pointers to data stored in the container are only invalidated by erasing that // element, even when the corresponding iterator is invalidated." @@ -152,14 +168,15 @@ const MessageSpec& MessageDefinitionCache::load_message_spec( } std::pair MessageDefinitionCache::get_full_text( - const std::string& root_package_resource_name) { + const std::string & root_package_resource_name) +{ std::unordered_set seen_deps; - std::function append_recursive = - [&](const DefinitionIdentifier& definition_identifier) { - const MessageSpec& spec = load_message_spec(definition_identifier); + std::function append_recursive = + [&](const DefinitionIdentifier & definition_identifier) { + const MessageSpec & spec = load_message_spec(definition_identifier); std::string result = spec.text; - for (const auto& dep_name : spec.dependencies) { + for (const auto & dep_name : spec.dependencies) { DefinitionIdentifier dep{definition_identifier.format, dep_name}; bool inserted = seen_deps.insert(dep).second; if (inserted) { @@ -175,10 +192,11 @@ std::pair MessageDefinitionCache::get_full_text( Format format = Format::MSG; try { result = append_recursive(DefinitionIdentifier{format, root_package_resource_name}); - } catch (const DefinitionNotFoundError& err) { + } catch (const DefinitionNotFoundError & err) { // log that we've fallen back - RCUTILS_LOG_WARN_NAMED("rosbag2_storage_mcap", "no .msg definition for %s, falling back to IDL", - err.what()); + RCUTILS_LOG_WARN_NAMED( + "rosbag2_storage_mcap", "no .msg definition for %s, falling back to IDL", + err.what()); format = Format::IDL; DefinitionIdentifier root_definition_identifier{format, root_package_resource_name}; result = delimiter(root_definition_identifier) + append_recursive(root_definition_identifier); diff --git a/rosbag2_storage_mcap/test/rosbag2_storage_mcap/test_mcap_storage.cpp b/rosbag2_storage_mcap/test/rosbag2_storage_mcap/test_mcap_storage.cpp index 94d5dd205f..7fa59aac61 100644 --- a/rosbag2_storage_mcap/test/rosbag2_storage_mcap/test_mcap_storage.cpp +++ b/rosbag2_storage_mcap/test/rosbag2_storage_mcap/test_mcap_storage.cpp @@ -76,8 +76,8 @@ TEST_F(TemporaryDirectoryFixture, can_write_and_read_basic_mcap_file) { // For this example it wouldn't matter, but in case anybody extends this test, it's for safety. auto serialized_bag_msg = std::make_shared(); serialized_bag_msg->serialized_data = std::shared_ptr( - const_cast(&serialized_msg->get_rcl_serialized_message()), - [](rcutils_uint8_array_t* /* data */) {}); + const_cast(&serialized_msg->get_rcl_serialized_message()), + [](rcutils_uint8_array_t * /* data */) {}); serialized_bag_msg->time_stamp = time_stamp; serialized_bag_msg->topic_name = topic_name; writer.write(serialized_bag_msg); @@ -143,8 +143,8 @@ TEST_F(TemporaryDirectoryFixture, can_write_mcap_with_zstd_configured_from_yaml) // For this example it wouldn't matter, but in case anybody extends this test, it's for safety. auto serialized_bag_msg = std::make_shared(); serialized_bag_msg->serialized_data = std::shared_ptr( - const_cast(&serialized_msg->get_rcl_serialized_message()), - [](rcutils_uint8_array_t* /* data */) {}); + const_cast(&serialized_msg->get_rcl_serialized_message()), + [](rcutils_uint8_array_t * /* data */) {}); serialized_bag_msg->time_stamp = time_stamp; serialized_bag_msg->topic_name = topic_name; writer.write(serialized_bag_msg); diff --git a/rosbag2_storage_mcap/test/rosbag2_storage_mcap/test_message_definition_cache.cpp b/rosbag2_storage_mcap/test/rosbag2_storage_mcap/test_message_definition_cache.cpp index 04596ef31f..cccd39f1f9 100644 --- a/rosbag2_storage_mcap/test/rosbag2_storage_mcap/test_message_definition_cache.cpp +++ b/rosbag2_storage_mcap/test/rosbag2_storage_mcap/test_message_definition_cache.cpp @@ -24,7 +24,8 @@ using rosbag2_storage_mcap::internal::parse_dependencies; using ::testing::UnorderedElementsAre; TEST(test_message_definition_cache, can_find_idl_includes) { - const char sample[] = R"r( + const char sample[] = + R"r( #include "rosbag2_storage_mcap_testdata/msg/BasicIdlA.idl" #include @@ -40,29 +41,34 @@ module rosbag2_storage_mcap_testdata { )r"; std::set dependencies = parse_dependencies(Format::IDL, sample, ""); - ASSERT_THAT(dependencies, UnorderedElementsAre("rosbag2_storage_mcap_testdata/msg/BasicIdlA", - "rosbag2_storage_mcap_testdata/msg/BasicIdlB")); + ASSERT_THAT( + dependencies, UnorderedElementsAre( + "rosbag2_storage_mcap_testdata/msg/BasicIdlA", + "rosbag2_storage_mcap_testdata/msg/BasicIdlB")); } TEST(test_message_definition_cache, can_find_msg_deps) { MessageDefinitionCache cache; auto [format, content] = cache.get_full_text("rosbag2_storage_mcap_testdata/ComplexMsg"); ASSERT_EQ(format, Format::MSG); - ASSERT_EQ(content, - R"r(rosbag2_storage_mcap_testdata/BasicMsg b + ASSERT_EQ( + content, + R"r(rosbag2_storage_mcap_testdata/BasicMsg b ================================================================================ MSG: rosbag2_storage_mcap_testdata/BasicMsg float32 c -)r"); +)r" + ); } TEST(test_message_definition_cache, can_find_idl_deps) { MessageDefinitionCache cache; auto [format, content] = cache.get_full_text("rosbag2_storage_mcap_testdata/msg/ComplexIdl"); EXPECT_EQ(format, Format::IDL); - EXPECT_EQ(content, - R"r(================================================================================ + EXPECT_EQ( + content, + R"r(================================================================================ IDL: rosbag2_storage_mcap_testdata/msg/ComplexIdl #include "rosbag2_storage_mcap_testdata/msg/BasicIdl.idl" @@ -83,7 +89,8 @@ module rosbag2_storage_mcap_testdata { }; }; }; -)r"); +)r" + ); } TEST(test_message_definition_cache, can_resolve_msg_with_idl_deps) { @@ -91,8 +98,9 @@ TEST(test_message_definition_cache, can_resolve_msg_with_idl_deps) { auto [format, content] = cache.get_full_text("rosbag2_storage_mcap_testdata/msg/ComplexMsgDependsOnIdl"); EXPECT_EQ(format, Format::IDL); - EXPECT_EQ(content, - R"r(================================================================================ + EXPECT_EQ( + content, + R"r(================================================================================ IDL: rosbag2_storage_mcap_testdata/msg/ComplexMsgDependsOnIdl // generated from rosidl_adapter/resource/msg.idl.em // with input from rosbag2_storage_mcap_testdata/msg/ComplexMsgDependsOnIdl.msg @@ -117,5 +125,6 @@ module rosbag2_storage_mcap_testdata { }; }; }; -)r"); +)r" + ); }