代码实现ROS2bag录制数据和回放数据(C++)

代码实现ROS2bag录制和回放数据功能(C++)

ROS2的ROS2bag功能可以将指定Topic中的数据以.db3的格式(不同于ROS）保存到本地文件中。读取文件数据可以实现数据回放。
ROS2版本为humble。

添加运行包

在项目的CMakeList文件中添加

find_package(rosbag2_cpp REQUIRED)
ament_target_dependencies(${PROJECT_NAME}rosbag2_cpp)

录制数据功能

以录制摄像头数据sensor_msgs::msg::Image为例

//stdpath是stdstring类型，为文件的保存路径
rosbag2_storage::StorageOptions storage_options({stdPath,"sqlite3"}); 
        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);
//image_data 为Topic Name,sensor_msgs::msg::Image为Topics数据类型
 writer_->create_topic(
              {"image_data",
               "sensor_msgs::msg::Image",
               rmw_get_serialization_format(),
               ""});

把ROS2标准类型序列化操作(下面操作一般在循环中进行）

//sensor_msgs::msg::Image类型序列化
sensor_msgs::msg::Image image_msg;
auto serializer = rclcpp::Serialization<sensor_msgs::msg::Image>();
auto serialized_message = rclcpp::SerializedMessage();
//image_msg是要序列化的数据，serialized_message是序列化后的数据
serializer.serialize_message(&image_msg, &serialized_message);
auto bag_message = std::make_shared<rosbag2_storage::SerializedBagMessage>();

bag_message->serialized_data = std::shared_ptr<rcutils_uint8_array_t>(
                  new rcutils_uint8_array_t,
                  [this](rcutils_uint8_array_t *msg) {
                    auto fini_return = rcutils_uint8_array_fini(msg);
                    delete msg;
                  });
*bag_message->serialized_data = serialized_message.release_rcl_serialized_message();

写入

 bag_message->topic_name = "image_data";
                writer_->write(bag_message);

回放数据功能

//同样stdPath为读取文件的路径
rosbag2_storage::StorageOptions storage_options({stdPath,"sqlite3"});
    rosbag2_cpp::ConverterOptions converter_options(
         {rmw_get_serialization_format(),
          rmw_get_serialization_format()});
std::unique_ptr<rosbag2_cpp::readers::SequentialReader> reader_;
reader_ = std::make_unique<rosbag2_cpp::readers::SequentialReader>();
reader_->open(storage_options, converter_options);

在循环中读取

//判断是否有下一条数据
while(reader_->has_next())
    {
        std::shared_ptr<rosbag2_storage::SerializedBagMessage> 		     image_SerializedBagMessage = reader_->read_next();
        auto serialized_message = rclcpp::SerializedMessage(*image_SerializedBagMessage->serialized_data);
        auto serializer = rclcpp::Serialization<sensor_msgs::msg::Image>();
        sensor_msgs::msg::Image image_msg;
        //数据反序列化
        serializer.deserialize_message(&serialized_message,&image_msg);
        msleep(30);
    }