ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

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

asked 2020-06-25 01:36:35 -0500

ryohei sasaki gravatar image

I am writing a program to write data directly to the rosbag2 in ros2 foxy.
The sample code snippet is as follows.

        #include <rosbag2_cpp/writers/sequential_writer.hpp>
        #include <osbag2_cpp/typesupport_helpers.hpp>

        nav_msgs::msg::Path path;
        path = ...

        rosbag2_cpp::writers::SequentialWriter writer;
        rosbag2_cpp::StorageOptions storage_options{};

        storage_options.uri = "aaaa";
        storage_options.storage_id = "sqlite3";

        rosbag2_cpp::ConverterOptions converter_options{};
        converter_options.input_serialization_format = "cdr";
        converter_options.output_serialization_format = "cdr";
        writer.open(storage_options, converter_options);

        auto ros_message = std::make_shared<rosbag2_cpp::rosbag2_introspection_message_t>();
        ros_message->message = &path;

        rosbag2_cpp::SerializationFormatConverterFactory factory;
        std::unique_ptr<rosbag2_cpp::converter_interfaces::SerializationFormatSerializer> cdr_serializer;
        cdr_serializer = factory.load_serializer("cdr");
        auto library = rosbag2_cpp::get_typesupport_library("nav_msgs/Odometry", "rosidl_typesupport_cpp");
        auto type_support = rosbag2_cpp::get_typesupport_handle("nav_msgs/Odometry", "rosidl_typesupport_cpp", library);

        auto message = std::make_shared<rosbag2_storage::SerializedBagMessage>();
        cdr_serializer->serialize(ros_message, type_support, message);

        writer.create_topic({"path_gt", "nav_msgs/Odometry", "", ""});
        writer.write(message);

It was compiled, but an error occurs while writing data.

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

I want to know how to write properly.
Thanks.

Reference
・rosbag2
https://github.com/ros2/rosbag2
・How to read data from rosbag2 in ros2
https://stackoverflow.com/questions/6...

edit retag flag offensive close merge delete

3 Answers

Sort by » oldest newest most voted
0

answered 2022-05-09 11:52:52 -0500

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;

}

edit flag offensive delete link more
0

answered 2020-07-23 12:59:19 -0500

Karsten gravatar image

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.

edit flag offensive delete link more

Comments

That minimal writer example is a lifesaver; it's pretty clear from that code how to set up a simple helper class to get back to one line open and write statements (at least until the improved API is available :-).

Jeffrey Kane Johnson gravatar image Jeffrey Kane Johnson  ( 2020-07-23 13:51:03 -0500 )edit

Thanks for the reply!

ryohei sasaki gravatar image ryohei sasaki  ( 2020-07-24 05:40:48 -0500 )edit
0

answered 2020-07-10 15:27:50 -0500

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

edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2020-06-25 01:15:34 -0500

Seen: 2,609 times

Last updated: Jul 24 '20