0. 简介
对弈ros/ros2的bag而言,真的是非常好用一个产物,通过bag我们可以保证数据的多次的重复的复现,直至发现问题。而对弈ros的bag的二次开发也一直是大家关注的重点。下面我们将从ROS1开始,带领大家了解一下ROS2的内容。
1. ROS1 BAG
对于ROS1的bag,网上有比较多的例子了,比如说下面的https://zhuanlan.zhihu.com/p/150290102
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <ros/ros.h>
#include <novatel_msgs/INSPVAX.h>
#include <vector>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH
int main(){
rosbag::Bag bag;
bag.open("/home/liuxiao/bagfiles/novatel.bag", rosbag::bagmode::Read);
std::vector<std::string> topics;
topics.push_back(std::string("/novatel_data/inspvax"));
rosbag::View view(bag, rosbag::TopicQuery(topics));
foreach(rosbag::MessageInstance const m, view)
{
novatel_msgs::INSPVAX::ConstPtr s = m.instantiate<novatel_msgs::INSPVAX>();
if (s != NULL)
std::cout << s->latitude << std::endl;
}
bag.close();
}
这些都是比较简单的例子,比较详细的内容可以参考ROS WIKI。
rosbag C++ API工作的前提是使用 "查询 (queries)"创建一个或多个包的 “views(视图)”。查询是一个抽象的类,它定义了一个函数来过滤是否包括来自连接的消息。这个函数可以访问topic_name、数据类型、md5sum、消息定义以及连接头。此外,每个查询可以为它包括的时间范围指定一个开始和结束时间。
多个查询可以被添加到一个视图中,包括来自不同包的查询。然后,该视图提供一个跨包的迭代器接口,根据时间排序。
上面一段文字比较详细的介绍了ROS1当中比较重要的两个内容,分别是:
-
rosbag::Bag - 在磁盘上序列化到/从一个bag文件。
-
rosbag::View - 指定一个bag文件的视图,以允许在特间范围内查询特定连接的消息。
-
2. ROS2 BAG
对于ROS2而言,我们知道ROS2包的形式和ROS1不太一样,但rosbag2也不只是提供了ros2 bag命令行工具。它还提供了一个C++ API,用于从你自己的源代码中读取和写入一个包。这允许你订阅一个主题,并在对该数据进行任何其他处理的同时,将收到的数据保存到一个包中。
首先需要安装了rosbag2软件包
sudo apt install ros-foxy-rosbag2
然后创建的ros2_ws目录。导航到ros2_ws/src目录并创建一个新的包
ros2 pkg create --build-type ament_cmake bag_recorder_nodes --dependencies example_interfaces rclcpp rosbag2_cpp std_msgs
你的终端将返回一条信息,验证你的包bag_recorder_nodes及其所有必要的文件和文件夹的创建。–dependencies参数将自动在package.xml和CMakeLists.txt中添加必要的依赖关系行。在这种情况下,该包将使用rosbag2_cpp包以及rclcpp包。本教程的后面部分也需要对example_interfaces包的依赖。
然后就是创建新文件simple_bag_recorder.cpp
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <rosbag2_cpp/typesupport_helpers.hpp>
#include <rosbag2_cpp/writer.hpp>
#include <rosbag2_cpp/writers/sequential_writer.hpp>
#include <rosbag2_storage/serialized_bag_message.hpp>
using std::placeholders::_1;
class SimpleBagRecorder : public rclcpp::Node
{
public:
SimpleBagRecorder()
: Node("simple_bag_recorder")
{
const rosbag2_cpp::StorageOptions storage_options({"my_bag", "sqlite3"});//设置可选的参数,包括bag文件名和数据库类型
const rosbag2_cpp::ConverterOptions converter_options(
{rmw_get_serialization_format(),
rmw_get_serialization_format()});//设置可选的参数,包括序列化格式和压缩格式
writer_ = std::make_unique<rosbag2_cpp::writers::SequentialWriter>();//创建SequentialWriter对象,并制定序列化格式
writer_->open(storage_options, converter_options);//打开writer,当中就包含了打开数据库的操作和设置序列化格式的操作
writer_->create_topic(
{"chatter",
"std_msgs/msg/String",
rmw_get_serialization_format(),
""});//创建topic
subscription_ = create_subscription<std_msgs::msg::String>(
"chatter", 10, std::bind(&SimpleBagRecorder::topic_callback, this, _1));//创建订阅者
}
private:
void topic_callback(std::shared_ptr<rclcpp::SerializedMessage> msg) const//设置回调函数
{
auto bag_message = std::make_shared<rosbag2_storage::SerializedBagMessage>();//创建SerializedBagMessage对象,并设置bag信息的类型
bag_message->serialized_data = std::shared_ptr<rcutils_uint8_array_t>(
new rcutils_uint8_array_t,//创建rcutils_uint8_array_t对象
[this](rcutils_uint8_array_t *msg) {//设置析构函数
auto fini_return = rcutils_uint8_array_fini(msg);//释放内存
delete msg;//删除对象
if (fini_return != RCUTILS_RET_OK) {//判断是否释放成功
RCLCPP_ERROR(get_logger(),
"Failed to destroy serialized message %s", rcutils_get_error_string().str);//打印错误信息
}
});//将bag_message中的serialized_data设置为rcutils_uint8_array_t类型
*bag_message->serialized_data = msg->release_rcl_serialized_message();//然后将msg信息中的rcl_serialized_message赋值给bag_message中的serialized_data
bag_message->topic_name = "chatter";//设置topic名称
if (rcutils_system_time_now(&bag_message->time_stamp) != RCUTILS_RET_OK) {//设置时间戳
RCLCPP_ERROR(get_logger(), "Error getting current time: %s",
rcutils_get_error_string().str);//打印错误信息
}
writer_->write(bag_message);//写入数据
}
rclcpp::Subscription<rclcpp::SerializedMessage>::SharedPtr subscription_;//创建订阅者
std::unique_ptr<rosbag2_cpp::writers::SequentialWriter> writer_;//创建SequentialWriter对象
};
这里面只是最简单的订阅并发布写入bag当中的操作,当然也有一些比较高级的操作,这里作者一一列举
void read_and_print_bag(std::string uri)
{
std::unique_ptr<rosbag2_cpp::readers::SequentialReader> reader_impl =
std::make_unique<rosbag2_cpp::readers::SequentialReader>(); // 设置ros2bag的读取器,并制定序列化格式
const rosbag2_cpp::StorageOptions storage_options({uri, "sqlite3"});// 设置ros2bag的存储格式
const rosbag2_cpp::ConverterOptions converter_options(
{rmw_get_serialization_format(),
rmw_get_serialization_format()});// 设置ros2bag的转换格式
reader_impl->open(storage_options, converter_options);// 打开ros2bag
rosbag2_cpp::Reader reader(std::move(reader_impl));// 读取ros2bag
auto serializer = rclcpp::Serialization<example_interfaces::msg::Int32>();// 设置序列化格式
while (reader.has_next())// 读取ros2bag的数据,并判断是否有下一个数据
{
auto message = reader.read_next();// 读取下一个数据
example_interfaces::msg::Int32 data;//设置输出的格式类型
rcutils_uint8_array_t raw_data = *message->serialized_data;//将bag包中的数据转换为rcutils_uint8_array_t类型
auto serialized_message = rclcpp::SerializedMessage(raw_data);//然后将raw_data数据转成数据格式
serializer.deserialize_message(&serialized_message, &data);//并将数据格式输出成data的信息,以供显示
// The data is owned by the original shared pointer in the SerializedBagMessage object, not the
// SerializedMessage object
std::cout << "Topic: " << message->topic_name << "\tData: " << data.data << "\tTime stamp: " << message->time_stamp << '\n';
}
}
或者像这样,可以暂停数据包