[ROS2]How to write data to rosbag2 in ros2 foxy

edit

Does somebody knows a way of doing this using the rosbag2_cpp::converter_interfaces::SerializationFormatSerializer instead of the rclcpp::Serialization<msgtype> ?

I am trying to make generic functions to save complex parameters as rosbag messages. rclcpp::Serialization<msgtype> is not an option as I must specify the message type. The idea is to get the message type using the rosbag2_cpp::SerializationFormatConverterFactory.

My code looks really similar to the one on the post. The reader function already works but I can't get the writer function to work. I am getting the same error:

terminate called after throwing an instance of 'std::logic_error' what(): basic_string::_M_construct null not valid

Code:

bool save_config(void* msg, std::string field, std::string type){

  // Check the config directory exists, create it otherwise
  const std::string package_install_directory = ament_index_cpp::get_package_prefix("storage_manager");
  rcpputils::fs::path dir(package_install_directory + "/config/");
  if(!dir.exists()){
    rcpputils::fs::create_directories(dir);
  }

  // Check the specific config directory exists, create it otherwise
  rcpputils::fs::path path(dir.string() + field);
  rcpputils::fs::remove_all(path);
  if(!path.exists()){
    rcpputils::fs::create_directories(path);
  }

  // Create rosbag writter
  const rosbag2_cpp::StorageOptions storage_options({path.string(), "sqlite3"});
  const rosbag2_cpp::ConverterOptions converter_options({rmw_get_serialization_format(), rmw_get_serialization_format()});
  std::unique_ptr<rosbag2_cpp::writers::SequentialWriter> writer_;
  writer_ = std::make_unique<rosbag2_cpp::writers::SequentialWriter>();
  writer_->open(storage_options, converter_options);

  // Topics would be fields of the config file
  writer_->create_topic({field, type, rmw_get_serialization_format(),""});

  // Serializer
  auto type_library = rosbag2_cpp::get_typesupport_library(type, "rosidl_typesupport_cpp");
  auto type_support = rosbag2_cpp::get_typesupport_handle(type, "rosidl_typesupport_cpp", type_library);
  rosbag2_cpp::SerializationFormatConverterFactory factory;
  std::unique_ptr<rosbag2_cpp::converter_interfaces::SerializationFormatSerializer> cdr_serializer_;
  cdr_serializer_ = factory.load_serializer("cdr");

  // Instrospection message
  auto ros_message = std::make_shared<rosbag2_cpp::rosbag2_introspection_message_t>();
  ros_message->time_stamp = 0;
  ros_message->message = nullptr;
  ros_message->allocator = rcutils_get_default_allocator();
  ros_message->message = msg;

  auto bag_message = std::make_shared<rosbag2_storage::SerializedBagMessage>();
  bag_message->time_stamp = 0;
  bag_message->topic_name = field;
  bag_message->serialized_data = rosbag2_storage::make_empty_serialized_message(0);
  cdr_serializer_->serialize(ros_message, type_support, bag_message);

  writer_->write(bag_message);
  return true;
} 

bool load_data(void* msg, std::string field){
  // Check the config directory exists, create it otherwise
  const std::string package_install_directory = ament_index_cpp::get_package_prefix("storage_manager");
  rcpputils::fs::path path(package_install_directory + "/config/" + field);
  if(!path.exists()){
    std::cout << "Config file for field " << field << " not found.\n" << std::endl;
    return false;
  }

  // Create rosbag reader
  const rosbag2_cpp::StorageOptions storage_options({path.string(), "sqlite3"});
  const rosbag2_cpp::ConverterOptions converter_options({rmw_get_serialization_format(), rmw_get_serialization_format()});

  rosbag2_cpp::readers::SequentialReader reader;
  reader.open(storage_options, converter_options);

  auto topics = reader.get_all_topics_and_types();
  std::string topic_type;
  bool topic_found = false;

  // Fetch config field as topic
  for (auto t:topics){
    if(t.name == field){
      topic_type = t.type;
      topic_found = true;
    }
  }
  if(!topic_found){
    printf("Config field not found. Topic not exists on rosbag file.\n");
    return false;
  }

  // Filter topic
  rosbag2_storage::StorageFilter filter;
  filter.topics.push_back(field);
  reader.set_filter(filter);

  // Read and deserialize "serialized data"
  if(reader.has_next()){

    // Serialized data
    auto serialized_message = reader.read_next();

    // Deserialization and conversion to ros message
    auto ros_message = std::make_shared<rosbag2_cpp::rosbag2_introspection_message_t>();
    ros_message->time_stamp = 0;
    ros_message->message = nullptr;
    ros_message->allocator = rcutils_get_default_allocator();
    ros_message->message = msg;
    auto type_library = rosbag2_cpp::get_typesupport_library(topic_type, "rosidl_typesupport_cpp");
    auto type_support = rosbag2_cpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", type_library);

    rosbag2_cpp::SerializationFormatConverterFactory factory;
    std::unique_ptr<rosbag2_cpp::converter_interfaces::SerializationFormatDeserializer> cdr_deserializer_;
    cdr_deserializer_ = factory.load_deserializer("cdr");
    cdr_deserializer_->deserialize(serialized_message, type_support, ros_message);
  }

  return true;

}

Please apologize the late response. This post really just now came to my attention. We've recently added a test which should illustrate the use of the C++ API to write data to a rosbag. https://github.com/ros2/rosbag2/blob/...

We're working on making this more user-friendly in the near future and to enhance the functionality further to allow writing non-serialized messages directly.

Looking at the above code snippet, there might be a confusion on when to use the converter API. The converters should really only be used when dealing with different middlewares, aka rmw implementations, which each have different serialization formats. A bit of context: ROS2 is designed to support multiple middlewares and we can't guarantee that the format in which data was recorded is the same as the format used when replaying this data. Therefore, we introduced plugins to convert data on-the-fly to different serialization formats. This shouldn't be necessary though when dealing with only one middleware.

There's some discussion about this here: https://github.com/ros2/rosbag2/issue...