1
$\begingroup$

As known, we can record a mcap in ros2. Now I'm trying to read messages in this mcap file and then publish it with rclcpp interfaces. There are some examples and demos in the mcap source code, but none is what I want. Then ros2 version is foxy.

$\endgroup$

2 Answers 2

1
$\begingroup$

Unfortunately, there are no solutions for Foxy because there is no support for MCAP file format in the Foxy distro and it has already reached the EOL.

We added support for MCAP file format starting from Humble.
Starting from Humble distro it will be something like this:

  rosbag2_storage::StorageOptions storage_options;
  storage_options.uri = "path/to/the/bag/file/or/folder";
  storage_options.storage_id = "mcap";
  auto reader = rosbag2_transport::ReaderWriterFactory::make_reader(storage_options);
  reader->open(storage_options);
  auto filter = rosbag2_storage::StorageFilter{};
  std::string topic_name = "topic_name_to_publish";
  filter.topics.emplace_back(topic_name);
  reader->set_filter(filter);

  const auto metadata = reader->get_metadata();

  std::string topic_type;
  rclcpp::QoS topic_qos_profile{1};

  for (const auto & topic : metadata.topics_with_message_count) {
    if (topic.topic_metadata.name == topic_name) {
      topic_type = topic.topic_metadata.type;
      topic_qos_profile = topic.topic_metadata.offered_qos_profiles[0];
    }
  }

  std::shared_ptr<rclcpp::GenericPublisher> pub =
    node_->create_generic_publisher(topic_name, topic_type, topic_qos_profile);

  while (reader->has_next()) {
    auto msg = reader->read_next();
    // TODO Add sleep for delay between two msg->time_stamp
    pub->publish(rclcpp::SerializedMessage((*msg->serialized_data)));
  }
```
$\endgroup$
0
$\begingroup$

This may work for you in foxy, you'll need to install the mcap C++ headers:

https://github.com/lucasw/ros_one2z/blob/zenoh_cpp_ros_comm/ros2_example_cpp/src/play_mcap.cpp

If not it should serve as an answer to how to publish already serialized messages in at least the rolling of right now (where I tested it).

It opens an mcap through the mcap api (rather than going through rosbag2 storage which doesn't support mcap in foxy as mentioned), and passes on the serialized bytes to be published. I just got this working so not sure if there's a better way to create a SerializedMessage from a buffer pointer and length (of uint8_t I think)- having to manipulate the buffer_length of the underlying rcl_msg seems suspect, the chances of memory leaking is non-zero given my understanding- but I think what is reserved should be deallocated when the wrapping SerializedMessage is destroyed upon going out of scope.

The QoS settings could be made to match what is stored in the mcap as well, I could use some help there but will probably figure it out eventually.

#define MCAP_IMPLEMENTATION
#include <mcap/reader.hpp>
...
#include <rclcpp/rclcpp.hpp>
#include <rcutils/types.h>

...
    // TODO(lucasw) this can't handle multiple publishers on same topic of different types
    std::map<std::string, std::shared_ptr<rclcpp::GenericPublisher>> pubs;

...
      std::ifstream input(inputFile, std::ios::binary);
      mcap::FileStreamReader dataSource{input};

      mcap::McapReader reader;
      const auto status0 = reader.open(dataSource);
...

      for (const auto& it : reader.channels()) {
        const auto channel = it.second;
        const auto schema = reader.schema(channel->schemaId);
        // TODO(lucasw) 'reliability' is in here but not sure about the mapping
        // 'offered_qos_profiles' can have multiple profiles
        for (const auto& m_it : channel->metadata) {
          std::cout << "  " << m_it.first << ":\n"<< m_it.second << "\n";
        }
        const auto qos_profile = rclcpp::QoS(1);
        pubs[channel->topic] = this->create_generic_publisher(channel->topic, schema->name, qos_profile);
      }

      auto messages = reader.readMessages(onProblem);
...
        for (const auto& msgView : messages) {
          const mcap::Channel& channel = *msgView.channel;
          auto& pub = pubs[channel.topic];

          const auto msg_size = msgView.message.dataSize;
          rclcpp::SerializedMessage serialized_msg;
          serialized_msg.reserve(msg_size);
          auto& rcl_msg = serialized_msg.get_rcl_serialized_message();
          // TODO(lucasw) is this copy avoidable?
          std::memcpy(rcl_msg.buffer, msgView.message.data, msg_size);
          rcl_msg.buffer_length = msg_size;

          pub->publish(serialized_msg);
        }
...

If anyone has something more elegant for the CMakeLists.txt I'd appreciate it but this works for me:

# these are needed for mcap
find_package(PkgConfig)
pkg_check_modules(LZ4 REQUIRED)
find_library(LZ4_LIBRARY
  NAMES lz4 liblz4
)

find_package(zstd REQUIRED)
...

add_executable(play_mcap src/play_mcap.cpp)
ament_target_dependencies(play_mcap rclcpp)
# these can't go into ament macro
target_link_libraries(play_mcap
  ${LZ4_LIBRARY}
  zstd::libzstd_shared
)
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.