Store IDL definitions in MCAP (#53)

Adds logic to detect IDL-defined message types and store the IDL.

Signed-off-by: James Smith <james@foxglove.dev>

rosbag2_storage_mcap/CHANGELOG.rst

@@ -2,6 +2,12 @@
 Changelog for package rosbag2_storage_mcap
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.3.0 (2022-09-09)
+------------------
+* Store IDL message definitions in Schema records when no MSG definition is available
+  (`#43 <https://github.com/ros-tooling/rosbag2_storage_mcap/issues/43>`_)
+* Contributors: James Smith
+
 0.2.0 (2022-09-08)
 ------------------
 * Support timestamp-ordered playback (`#50 <https://github.com/ros-tooling/rosbag2_storage_mcap/issues/50>`_)

rosbag2_storage_mcap/CMakeLists.txt

@@ -29,6 +29,10 @@ add_library(${PROJECT_NAME} SHARED
   src/mcap_storage.cpp
   src/message_definition_cache.cpp
 )
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
+)
 target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17)
 ament_target_dependencies(${PROJECT_NAME}
   mcap_vendor
@@ -87,6 +91,9 @@ if(BUILD_TESTING)
   target_link_libraries(test_mcap_storage ${PROJECT_NAME})
   ament_target_dependencies(test_mcap_storage rosbag2_storage rosbag2_cpp rosbag2_test_common std_msgs)
   target_compile_definitions(test_mcap_storage PRIVATE ${MCAP_COMPILE_DEFS})
+
+  ament_add_gmock(test_message_definition_cache test/rosbag2_storage_mcap/test_message_definition_cache.cpp)
+  target_link_libraries(test_message_definition_cache ${PROJECT_NAME})
 endif()
 
 

rosbag2_storage_mcap/include/rosbag2_storage_mcap/message_definition_cache.hpp

@@ -0,0 +1,94 @@
+// Copyright 2022, Foxglove Technologies. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ROSBAG2_STORAGE_MCAP__MESSAGE_DEFINITION_CACHE_HPP_
+#define ROSBAG2_STORAGE_MCAP__MESSAGE_DEFINITION_CACHE_HPP_
+
+#include <set>
+#include <string>
+#include <unordered_map>
+#include <unordered_set>
+#include <utility>
+
+namespace rosbag2_storage_mcap::internal {
+
+enum struct Format {
+  IDL,
+  MSG,
+};
+
+struct MessageSpec {
+  MessageSpec(Format format, std::string text, const std::string& package_context);
+  const std::set<std::string> dependencies;
+  const std::string text;
+  Format format;
+};
+
+struct DefinitionIdentifier {
+  Format format;
+  std::string package_resource_name;
+
+  bool operator==(const DefinitionIdentifier& di) const {
+    return (format == di.format) && (package_resource_name == di.package_resource_name);
+  }
+};
+
+class DefinitionNotFoundError : public std::exception {
+private:
+  std::string name_;
+
+public:
+  explicit DefinitionNotFoundError(const std::string& name)
+      : name_(name) {}
+  const char* what() const throw() {
+    return name_.c_str();
+  }
+};
+
+class MessageDefinitionCache final {
+public:
+  /**
+   * Concatenate the message definition with its dependencies into a self-contained schema.
+   * The format is different for MSG and IDL definitions, and is described fully here:
+   * [MSG](https://mcap.dev/specification/appendix.html#ros2msg-data-format)
+   * [IDL](https://mcap.dev/specification/appendix.html#ros2idl-data-format)
+   * Throws DefinitionNotFoundError if one or more definition files are missing for the given
+   * package resource name.
+   */
+  std::pair<Format, std::string> get_full_text(const std::string& package_resource_name);
+
+private:
+  struct DefinitionIdentifierHash {
+    std::size_t operator()(const DefinitionIdentifier& di) const {
+      std::size_t h1 = std::hash<Format>()(di.format);
+      std::size_t h2 = std::hash<std::string>()(di.package_resource_name);
+      return h1 ^ h2;
+    }
+  };
+  /**
+   * 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);
+
+  std::unordered_map<DefinitionIdentifier, MessageSpec, DefinitionIdentifierHash>
+    msg_specs_by_definition_identifier_;
+};
+
+std::set<std::string> parse_dependencies(Format format, const std::string& text,
+                                         const std::string& package_context);
+
+}  // namespace rosbag2_storage_mcap::internal
+
+#endif  // ROSBAG2_STORAGE_MCAP__MESSAGE_DEFINITION_CACHE_HPP_

rosbag2_storage_mcap/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rosbag2_storage_mcap</name>
-  <version>0.2.0</version>
+  <version>0.3.0</version>
   <description>rosbag2 storage plugin using the MCAP file format</description>
   <maintainer email="ros-tooling@foxglove.dev">Foxglove</maintainer>
   <license>Apache-2.0</license>
@@ -23,6 +23,7 @@
   <test_depend>rosbag2_cpp</test_depend>
   <test_depend>rosbag2_test_common</test_depend>
   <test_depend>std_msgs</test_depend>
+  <test_depend>rosbag2_storage_mcap_test_fixture_interfaces</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

rosbag2_storage_mcap/src/mcap_storage.cpp

@@ -12,11 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "message_definition_cache.hpp"
 #include "rcutils/logging_macros.h"
 #include "rosbag2_storage/metadata_io.hpp"
 #include "rosbag2_storage/ros_helper.hpp"
 #include "rosbag2_storage/storage_interfaces/read_write_interface.hpp"
+#include "rosbag2_storage_mcap/message_definition_cache.hpp"
 
 #ifdef ROSBAG2_STORAGE_MCAP_HAS_YAML_HPP
 #include "rosbag2_storage/yaml.hpp"
@@ -520,12 +520,23 @@ void MCAPStorage::write(std::shared_ptr<const rosbag2_storage::SerializedBagMess
   const auto& datatype = topic_info.topic_metadata.type;
   const auto schema_it = schema_ids.find(datatype);
   if (schema_it == schema_ids.end()) {
-    std::string full_text = msgdef_cache_.get_full_text(datatype);
     mcap::Schema schema;
     schema.name = datatype;
-    schema.encoding = "ros2msg";
-    schema.data.assign(reinterpret_cast<const std::byte*>(full_text.data()),
-                       reinterpret_cast<const std::byte*>(full_text.data() + full_text.size()));
+    try {
+      auto [format, full_text] = msgdef_cache_.get_full_text(datatype);
+      if (format == rosbag2_storage_mcap::internal::Format::MSG) {
+        schema.encoding = "ros2msg";
+      } else {
+        schema.encoding = "ros2idl";
+      }
+      schema.data.assign(reinterpret_cast<const std::byte*>(full_text.data()),
+                         reinterpret_cast<const std::byte*>(full_text.data() + full_text.size()));
+    } catch (rosbag2_storage_mcap::internal::DefinitionNotFoundError& err) {
+      RCUTILS_LOG_ERROR_NAMED("rosbag2_storage_mcap",
+                              "definition file(s) missing for %s: missing %s", datatype.c_str(),
+                              err.what());
+      schema.encoding = "";
+    }
     mcap_writer_->addSchema(schema);
     schema_ids.emplace(datatype, schema.id);
     schema_id = schema.id;

rosbag2_storage_mcap/src/message_definition_cache.cpp

@@ -12,13 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "message_definition_cache.hpp"
+#include "rosbag2_storage_mcap/message_definition_cache.hpp"
 
 #include <ament_index_cpp/get_package_share_directory.hpp>
 #include <ament_index_cpp/get_resource.hpp>
 #include <ament_index_cpp/get_resources.hpp>
+#include <rcutils/logging_macros.h>
 
 #include <fstream>
+#include <optional>
 #include <regex>
 #include <set>
 #include <string>
@@ -28,19 +30,24 @@
 namespace rosbag2_storage_mcap::internal {
 
 // Match datatype names (foo_msgs/Bar or foo_msgs/msg/Bar)
-static const std::regex MSG_DATATYPE_REGEX{R"(^([a-zA-Z0-9_]+)/(?:msg/)?([a-zA-Z0-9_]+)$)"};
+static const std::regex PACKAGE_TYPENAME_REGEX{R"(^([a-zA-Z0-9_]+)/(?:msg/)?([a-zA-Z0-9_]+)$)"};
 
 // Match field types from .msg definitions ("foo_msgs/Bar" in "foo_msgs/Bar[] bar")
-static const std::regex FIELD_TYPE_REGEX{R"((?:^|\n)\s*([a-zA-Z0-9_/]+)(?:\[[^\]]*\])?\s+)"};
+static const std::regex MSG_FIELD_TYPE_REGEX{R"((?:^|\n)\s*([a-zA-Z0-9_/]+)(?:\[[^\]]*\])?\s+)"};
+
+// match field types from `.idl` definitions ("foo_msgs/msg/bar" in #include <foo_msgs/msg/Bar.idl>)
+static const std::regex IDL_FIELD_TYPE_REGEX{
+  R"((?:^|\n)#include\s+(?:"|<)([a-zA-Z0-9_/]+)\.idl(?:"|>))"};
 
 static const std::unordered_set<std::string> PRIMITIVE_TYPES{
   "bool",  "byte",   "char",  "float32", "float64", "int8",   "uint8",
   "int16", "uint16", "int32", "uint32",  "int64",   "uint64", "string"};
 
-static std::set<std::string> parse_dependencies(const std::string& text,
-                                                const std::string& package_context) {
+static std::set<std::string> parse_msg_dependencies(const std::string& text,
+                                                    const std::string& package_context) {
   std::set<std::string> dependencies;
-  for (std::sregex_iterator iter(text.begin(), text.end(), FIELD_TYPE_REGEX);
+
+  for (std::sregex_iterator iter(text.begin(), text.end(), MSG_FIELD_TYPE_REGEX);
        iter != std::sregex_iterator(); ++iter) {
     std::string type = (*iter)[1];
     if (PRIMITIVE_TYPES.find(type) != PRIMITIVE_TYPES.end()) {
@@ -55,54 +62,127 @@ static std::set<std::string> parse_dependencies(const std::string& text,
   return dependencies;
 }
 
-MessageSpec::MessageSpec(std::string text, const std::string& package_context)
-    : dependencies(parse_dependencies(text, package_context))
-    , text(std::move(text)) {}
+static std::set<std::string> parse_idl_dependencies(const std::string& text) {
+  std::set<std::string> dependencies;
 
-const MessageSpec& MessageDefinitionCache::load_message_spec(const std::string& datatype) {
-  if (auto it = msg_specs_by_datatype_.find(datatype); it != msg_specs_by_datatype_.end()) {
-    return it->second;
+  for (std::sregex_iterator iter(text.begin(), text.end(), IDL_FIELD_TYPE_REGEX);
+       iter != std::sregex_iterator(); ++iter) {
+    dependencies.insert(std::move((*iter)[1]));
   }
+  return dependencies;
+}
 
+std::set<std::string> 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);
+    case Format::IDL:
+      return parse_idl_dependencies(text);
+    default:
+      throw std::runtime_error("switch is not exhaustive");
+  }
+}
+
+static const char* extension_for_format(Format format) {
+  switch (format) {
+    case Format::MSG:
+      return ".msg";
+    case Format::IDL:
+      return ".idl";
+    default:
+      throw std::runtime_error("switch is not exhaustive");
+  }
+}
+
+static std::string delimiter(const DefinitionIdentifier& definition_identifier) {
+  std::string result =
+    "================================================================================\n";
+  switch (definition_identifier.format) {
+    case Format::MSG:
+      result += "MSG: ";
+      break;
+    case Format::IDL:
+      result += "IDL: ";
+      break;
+    default:
+      throw std::runtime_error("switch is not exhaustive");
+  }
+  result += definition_identifier.package_resource_name;
+  result += "\n";
+  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) {}
+
+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()) {
+    return it->second;
+  }
   std::smatch match;
-  if (!std::regex_match(datatype, match, MSG_DATATYPE_REGEX)) {
-    throw std::invalid_argument("Invalid datatype name: " + datatype);
+  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() + ".msg"};
+  std::ifstream file{share_dir + "/msg/" + match[2].str() +
+                     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 =
-    msg_specs_by_datatype_.emplace(datatype, MessageSpec(std::move(contents), package))
+    msg_specs_by_definition_identifier_
+      .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."
   return spec;
 }
 
-std::string MessageDefinitionCache::get_full_text(const std::string& root_datatype) {
-  std::string result;
-  std::unordered_set<std::string> seen_deps = {root_datatype};
-  std::function<void(const std::string&)> append_recursive = [&](const std::string& datatype) {
-    const MessageSpec& spec = load_message_spec(datatype);
-    if (!result.empty()) {
-      result +=
-        "\n================================================================================\nMSG: ";
-      result += datatype;
-      result += '\n';
-    }
-    result += spec.text;
-    for (const auto& dep : spec.dependencies) {
-      bool inserted = seen_deps.insert(dep).second;
-      if (inserted) {
-        append_recursive(dep);
+std::pair<Format, std::string> MessageDefinitionCache::get_full_text(
+  const std::string& root_package_resource_name) {
+  std::unordered_set<DefinitionIdentifier, DefinitionIdentifierHash> seen_deps;
+
+  std::function<std::string(const DefinitionIdentifier&)> 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) {
+        DefinitionIdentifier dep{definition_identifier.format, dep_name};
+        bool inserted = seen_deps.insert(dep).second;
+        if (inserted) {
+          result += "\n";
+          result += delimiter(dep);
+          result += append_recursive(dep);
+        }
       }
-    }
-  };
-  append_recursive(root_datatype);
-  return result;
+      return result;
+    };
+
+  std::string result;
+  Format format = Format::MSG;
+  try {
+    result = append_recursive(DefinitionIdentifier{format, root_package_resource_name});
+  } 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());
+    format = Format::IDL;
+    DefinitionIdentifier root_definition_identifier{format, root_package_resource_name};
+    result = delimiter(root_definition_identifier) + append_recursive(root_definition_identifier);
+  }
+  return std::make_pair(format, result);
 }
 
 }  // namespace rosbag2_storage_mcap::internal