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;
}