ROS 系列学习教程(总目录)
本文目录
- 一、rosbag::Bag
- 1.1 常用接口
- 1.2 其他接口
- 二、rosbag::View
- 2.1 常用接口
- 2.1.1 代码示例
rosbag 的 C++ API 主要有两个类,用于写bag文件的Bag
类,和用于读bag文件的View
类。
一、rosbag::Bag
用于写bag文件。
头文件:bag.h
1.1 常用接口
# 打开bag文件
Bag (std::string const &filename, uint32_t mode=bagmode::Read)# 打开bag文件
void open (std::string const &filename, uint32_t mode=bagmode::Read)# 关闭bag文件
void close()# 写bag文件
template<class T >
void write (std::string const &topic, ros::MessageEvent< T > const &event)template<class T >
void write (std::string const &topic, ros::Time const &time, boost::shared_ptr< T > const &msg, boost::shared_ptr< ros::M_string > connection_header=boost::shared_ptr< ros::M_string >())template<class T >
void write (std::string const &topic, ros::Time const &time, boost::shared_ptr< T const > const &msg, boost::shared_ptr< ros::M_string > connection_header=boost::shared_ptr< ros::M_string >())template<class T >
void write (std::string const &topic, ros::Time const &time, T const &msg, boost::shared_ptr< ros::M_string > connection_header=boost::shared_ptr< ros::M_string >())
示例代码:
#include <ros/ros.h>
#include <rosbag/bag.h>
#include <ros/package.h>
#include <std_msgs/String.h>int main(int argc, char **argv)
{ros::init(argc, argv, "bag_write");std::string packagePath = ros::package::getPath("rosbag_learning");std::string bagsPath = packagePath + "/bags";ros::NodeHandle nh;// 创建bag对象rosbag::Bag bag;// 打开文件bag.open(bagsPath+"/test.bag", rosbag::BagMode::Write);// 写文件std_msgs::String msg;msg.data = "hello world";// 写入4帧for (size_t i = 0; i < 4; i++){bag.write("/chatter", ros::Time::now(), msg);}// 关闭文件bag.close();return 0;
}
编译运行生成 test.bag
文件,查看该文件信息,结果如下:
可以看到成功写入了4帧数据。
1.2 其他接口
bag文件是否被打开
bool isOpen() const;
获取文件名
std::string getFileName() const;
获取文件的打开模式,枚举如下
BagMode getMode() const;
enum BagMode
{Write = 1,Read = 2,Append = 4
};
获取文件的主版本号和次版本号
ROS bag格式有很多版本,官方不保证不同版本之间的兼容性
bag文件第一行记录了当前的版本号,格式如:#ROSBAG VX.Y
旧版本使用#ROSRECORD或#ROSLOG前缀
其中,X是主版本号,Y是次版本号
uint32_t getMajorVersion() const;
uint32_t getMinorVersion() const;
获取文件大小
uint64_t getSize() const;
设置/获取用于写入块的压缩方法,枚举如下
void setCompression(CompressionType compression);
CompressionType getCompression() const;
enum CompressionType
{Uncompressed = 0, // 不压缩BZ2 = 1, // BZ2格式LZ4 = 2, // LZ4格式
};
设置/获取 Bag 文件中每个块的最大消息数量
在 ROS Bag 文件中,消息数据被划分为多个“块”(chunks)。每个块可能包含多个消息,并且块的大小是固定的。块的大小决定了 Bag 文件的读写效率和磁盘空间使用。
当向 Bag 文件中写入消息时,ROS 会尝试将消息放入当前的块。如果当前块中的消息数量达到或超过 chunk_threshold,则 ROS 会开始一个新的块来存储后续的消息。
通过调整 chunk_threshold,你可以控制 Bag 文件的读写效率和磁盘空间使用。较小的 chunk_threshold 会导致更多的块,这可能会降低读写效率但可能会节省磁盘空间(因为每个块都有自己的元数据)。而较大的 chunk_threshold 则会提高读写效率,但可能会使用更多的磁盘空间。
void setChunkThreshold(uint32_t chunk_threshold);
uint32_t getChunkThreshold() const;
使用指定的加密插件加密bag文件
void setEncryptorPlugin(const std::string& plugin_name, const std::string& plugin_param = std::string());
交换当前bag对象与参数bag的内容
void swap(Bag&);
二、rosbag::View
用于读bag文件。
头文件:view.h
2.1 常用接口
创建一个bag文件视图
将bag中的msg存到vector中,按时间升序排序
bag:bag文件
query:查询条件函数
start_time:查询时间范围的开始时间
end_time:查询时间范围的结束时间
reduce_overlap:如果返回多个相同的消息,将它们合并为一条消息
View(bool const& reduce_overlap = false);
View(Bag const& bag, ros::Time const& start_time = ros::TIME_MIN, ros::Time const& end_time = ros::TIME_MAX, bool const& reduce_overlap = false);
View(Bag const& bag, boost::function<bool(ConnectionInfo const*)> query, ros::Time const& start_time = ros::TIME_MIN, ros::Time const& end_time = ros::TIME_MAX, bool const& reduce_overlap = false);
查询时间范围的开始/结束时间
ros::Time getBeginTime();
ros::Time getEndTime();
用于遍历vector中的msg
iterator begin();
iterator end();
获取vector大小(msg个数)
uint32_t size();
添加查询时间范围
bag:bag文件
query:查询条件函数
start_time:查询时间范围的开始时间
end_time:查询时间范围的结束时间
void addQuery(Bag const& bag, ros::Time const& start_time = ros::TIME_MIN, ros::Time const& end_time = ros::TIME_MAX);
void addQuery(Bag const& bag, boost::function<bool(ConnectionInfo const*)> query, ros::Time const& start_time = ros::TIME_MIN, ros::Time const& end_time = ros::TIME_MAX);
获取bag中topic的连接信息,每个topic一个结构体
其中返回结构体定义如下:
std::vector<const ConnectionInfo*> getConnections();
struct ROSBAG_STORAGE_DECL ConnectionInfo
{ConnectionInfo() : id(-1) { }uint32_t id; // topic idstd::string topic; // topic名字std::string datatype; // topic数据类型,即topic的msgstd::string md5sum; // topic的MD5值std::string msg_def; // msg的数据类型boost::shared_ptr<ros::M_string> header;
};
2.1.1 代码示例
#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <std_msgs/String.h>int main(int argc, char **argv)
{ros::init(argc, argv, "bag_read");std::string packagePath = ros::package::getPath("rosbag_learning");std::string bagsPath = packagePath + "/bags";rosbag::Bag bag;bag.open(bagsPath+"/test.bag"); // BagMode is Read by defaultfor (rosbag::MessageInstance const m : rosbag::View(bag)){std_msgs::String::ConstPtr i = m.instantiate<std_msgs::String>();if (i != nullptr){ROS_INFO("%s", i->data.c_str());}}bag.close();return 0;
}
编译运行,读取上文生成的test.bag
文件,结果如下:
对于 getConnections()
函数,示例如下:
#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>int main(int argc, char **argv)
{ros::init(argc, argv, "bag_read");std::string packagePath = ros::package::getPath("rosbag_learning");std::string bagsPath = packagePath + "/bags";rosbag::Bag bag;bag.open(bagsPath+"/test.bag"); // BagMode is Read by defaultrosbag::View view(bag);std::vector<const rosbag::ConnectionInfo*> cInfo = view.getConnections();for (size_t i = 0; i < cInfo.size(); i++){ROS_INFO("id: %d, topic: %s, dataType: %s, md5: %s, msg_def: %s", cInfo[i]->id, cInfo[i]->topic.c_str(), cInfo[i]->datatype.c_str(),cInfo[i]->md5sum.c_str(), cInfo[i]->msg_def.c_str());}bag.close();return 0;
}
编译运行,结果如下: