ROS - C++实现RosBag包录制

1. 录制原理

#include <ros/ros.h>
#include <rosbag/bag.h>
#include <std_msgs/String.h>int main(int argc, char** argv)
{// 初始化ROS节点ros::init(argc, argv, "record_bag_node");ros::NodeHandle nh;// 创建ROSbag对象，打开bag文件rosbag::Bag bag;bag.open("example.bag", rosbag::bagmode::Write);// 创建一个String消息std_msgs::String msg;msg.data = "Hello ROSbag!";// 将消息写入bag文件bag.write("my_topic", ros::Time::now(), msg);// 关闭bag文件bag.close();return 0;
}

在上面的代码中：

• 我们首先初始化了ROS节点 ：

• 然后创建了一个ROSbag对象 ：

• 打开了一个名为"example.bag"的bag文件 ：

• 接着创建了一个名为"my_topic"的ROS话题 ：

• 并将一个String类型的消息写入了bag文件中 ：

• 最后关闭了bag文件 ：

请注意，这只是一个示例，你需要根据你自己的需求修改ROS话题和消息类型。

2. 录制多个话题

#include <ros/ros.h>
#include <rosbag/bag.h>
#include <std_msgs/String.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/LaserScan.h>int main(int argc, char** argv)
{// 初始化ROS节点ros::init(argc, argv, "record_bag_node");ros::NodeHandle nh;// 创建ROSbag对象，打开bag文件rosbag::Bag bag;bag.open("example.bag", rosbag::bagmode::Write);// 创建一个String消息std_msgs::String str_msg;str_msg.data = "Hello ROSbag!";// 创建一个Image消息sensor_msgs::Image img_msg;img_msg.header.stamp = ros::Time::now();img_msg.header.frame_id = "camera";img_msg.width = 640;img_msg.height = 480;img_msg.encoding = "rgb8";img_msg.step = img_msg.width * 3;img_msg.data.resize(img_msg.step * img_msg.height);// 创建一个LaserScan消息sensor_msgs::LaserScan scan_msg;scan_msg.header.stamp = ros::Time::now();scan_msg.header.frame_id = "laser";scan_msg.angle_min = -M_PI/2;scan_msg.angle_max = M_PI/2;scan_msg.angle_increment = M_PI/180;scan_msg.time_increment = 0.001;scan_msg.scan_time = 0.1;scan_msg.range_min = 0.1;scan_msg.range_max = 10.0;scan_msg.ranges.resize(180);scan_msg.intensities.resize(180);// 将消息写入bag文件bag.write("my_string_topic", ros::Time::now(), str_msg);bag.write("my_image_topic", ros::Time::now(), img_msg);bag.write("my_laser_topic", ros::Time::now(), scan_msg);// 关闭bag文件bag.close();return 0;}

在上面的代码中，我们创建了三个不同的消息类型（String、Image和LaserScan），并将它们分别写入了三个不同的ROS话题中。最终，将所有话题的消息保存到一个名为"example.bag"的ROSbag文件中。

3. 订阅ROS消息，实时录制

#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/LaserScan.h>
#include <std_msgs/String.h>// 定义一个函数，用于录制多个话题的数据到ROSbag文件
// 参数：
// - topics: 一个包含多个话题名的vector
// - bag_filename: ROSbag文件名
// - duration: 录制数据的时间长度（单位：秒），如果duration<=0，则持续录制直到手动停止
void recordRosbag(const std::vector<std::string>& topics, const std::string& bag_filename, double duration = 0) {// 创建ROS节点ros::NodeHandle nh;// 创建ROSbag对象rosbag::Bag bag;bag.open(bag_filename, rosbag::bagmode::Write);// 创建ROS话题订阅器std::vector<ros::Subscriber> subs;for (const auto& topic : topics) {if (topic == "/image") {subs.push_back(nh.subscribe<sensor_msgs::Image>(topic, 1, [&](const sensor_msgs::Image::ConstPtr& msg) {bag.write(topic, ros::Time::now(), *msg);}));}else if (topic == "/scan") {subs.push_back(nh.subscribe<sensor_msgs::LaserScan>(topic, 1, [&](const sensor_msgs::LaserScan::ConstPtr& msg) {bag.write(topic, ros::Time::now(), *msg);}));}else if (topic == "/string") {subs.push_back(nh.subscribe<std_msgs::String>(topic, 1, [&](const std_msgs::String::ConstPtr& msg) {bag.write(topic, ros::Time::now(), *msg);}));}else {ROS_WARN_STREAM("Unknown topic: " << topic);}}// 持续录制数据if (duration <= 0) {ROS_INFO_STREAM("Start recording indefinitely...");ros::spin();}// 持续录制一段时间后停止else {ROS_INFO_STREAM("Start recording for " << duration << " seconds...");ros::Time start_time = ros::Time::now();while ((ros::Time::now() - start_time).toSec() < duration) {ros::spinOnce();}}// 停止订阅器并关闭ROSbag文件for (auto& sub : subs) {sub.shutdown();}bag.close();ROS_INFO_STREAM("Recording finished!");
}

// 示例使用
int main(int argc, char** argv) 
{ros::init(argc, argv, "rosbag_recorder_player");// 录制多个话题的数据到ROSbag文件std::vectorstd::string record_topics = {"/image", "/scan", "/string"};std::string bag_filename = "/tmp/test.bag";double duration = 10; // 持续10秒录制recordRosbag(record_topics, bag_filename, duration);// 回放ROSbag文件中的所有话题数据playbackRosbag(bag_filename);// 回放ROSbag文件中的指定话题数据std::vectorstd::string playback_topics = {"/scan", "/string"};playbackRosbag(bag_filename, playback_topics);return 0;
}