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

1.代码实现ROSbag录制数据和回放数据(C++)

录制数据：

#include 
#include 
#include 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文件。

回放数据：

#include 
#include 
#include int main(int argc, char** argv)
{// 初始化ROS节点ros::init(argc, argv, "play_bag_node");ros::NodeHandle nh;// 创建ROSbag对象，打开bag文件rosbag::Bag bag;bag.open("example.bag", rosbag::bagmode::Read);// 获取名为"my_topic"的话题的迭代器rosbag::View view(bag, rosbag::TopicQuery("my_topic"));// 循环遍历话题中的所有消息for (rosbag::View::iterator it = view.begin(); it != view.end(); ++it){// 转换消息类型std_msgs::String::ConstPtr msg = it->instantiate();if (msg != NULL){// 打印消息内容ROS_INFO("Message: %s", msg->data.c_str());}}// 关闭bag文件bag.close();return 0;
}

在上面的代码中，我们首先初始化了ROS节点，然后创建了一个ROSbag对象，打开了一个名为"example.bag"的bag文件，接着获取了名为"my_topic"的话题的迭代器，并循环遍历话题中的所有消息。在每个消息中，我们将其转换为String类型，并打印其内容。最后关闭了bag文件。

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

2.代码实现ROSbag录制多个话题的数据和回放数据(C++)

录制数据：

#include 
#include 
#include 
#include 
#include 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文件中。

回放数据：

#include 
#include 
#include 
#include 
#include int main(int argc, char** argv)
{// 初始化ROS节点ros::init(argc, argv, "play_bag_node");ros::NodeHandle nh;// 创建ROSbag对象，打开bag文件rosbag::Bag bag;bag.open("example.bag", rosbag::bagmode::Read);// 获取三个话题的迭代器rosbag::View view(bag, rosbag::TopicQuery("my_string_topic") && rosbag::TopicQuery("my_image_topic") && rosbag::TopicQuery("my_laser_topic"));// 循环遍历话题中的所有消息for (rosbag::View::iterator it = view.begin(); it != view.end(); ++it
) {
// 从bag文件中读取消息
const rosbag::MessageInstance& msg = *it;// 根据消息类型进行强制转换
if (msg.isType()) {std_msgs::String::ConstPtr str_msg = msg.instantiate();ROS_INFO_STREAM("String message: " << str_msg->data);
}
else if (msg.isType()) {sensor_msgs::Image::ConstPtr img_msg = msg.instantiate();ROS_INFO_STREAM("Image message: " << img_msg->header.stamp);// 在此处添加处理Image消息的代码
}
else if (msg.isType()) {sensor_msgs::LaserScan::ConstPtr scan_msg = msg.instantiate();ROS_INFO_STREAM("LaserScan message: " << scan_msg->header.stamp);// 在此处添加处理LaserScan消息的代码
}}// 关闭bag文件
bag.close();return 0;
}

在上面的代码中，我们首先创建了一个ROSbag对象，并打开了名为"example.bag"的ROSbag文件。然后，我们通过rosbag::TopicQuery函数获取了三个话题的迭代器，并循环遍历了所有话题的消息。在循环中，我们使用rosbag::MessageInstance对象来读取每条消息，并根据消息类型进行强制转换，以便能够对每个消息进行处理。最后，我们关闭了ROSbag文件。

请注意，以上代码中仅仅输出了每条消息的时间戳和消息内容，您可以根据需要修改代码来处理具体的消息内容。同时，如果需要处理更多的ROS话题，只需在代码中添加相应的话题即可。
 

3.代码实现ROSbag录制多个话题的数据和回放数据用c++封装成函数

#include 
#include 
#include 
#include 
#include 
#include // 定义一个函数，用于录制多个话题的数据到ROSbag文件
// 参数：
// - topics: 一个包含多个话题名的vector
// - bag_filename: ROSbag文件名
// - duration: 录制数据的时间长度（单位：秒），如果duration<=0，则持续录制直到手动停止
void recordRosbag(const std::vector& 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 subs;for (const auto& topic : topics) {if (topic == "/image") {subs.push_back(nh.subscribe(topic, 1, [&](const sensor_msgs::Image::ConstPtr& msg) {bag.write(topic, ros::Time::now(), *msg);}));}else if (topic == "/scan") {subs.push_back(nh.subscribe(topic, 1, [&](const sensor_msgs::LaserScan::ConstPtr& msg) {bag.write(topic, ros::Time::now(), *msg);}));}else if (topic == "/string") {subs.push_back(nh.subscribe(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!");
}

定义一个函数，用于回放多个话题的数据

#include 
#include 
#include 
#include 
#include 
// 定义一个函数，用于回放多个话题的数据
// 参数：
// - bag_filename: ROSbag文件名
// - topics: 一个包含多个话题名的vector，如果为空，则回放所有话题的数据
void playbackRosbag(const std::string& bag_filename, const std::vector& topics = {}) {// 创建ROS节点// ros::NodeHandle nh("~");ros::NodeHandle nh;//sukai// 创建ROS话题发布器std::vector pubs;for (const auto& topic : topics.empty() ? bag.getTopics() : topics) {if (topic == "/image") {pubs.push_back(nh.advertise(topic, 1));}else if (topic == "/scan") {pubs.push_back(nh.advertise(topic, 1));}else if (topic == "/string") {pubs.push_back(nh.advertise(topic, 1));}else {ROS_WARN_STREAM("Unknown topic: " << topic);}}// 回放数据// rosbag::Bag bag(bag_filename, rosbag::bagmode::Read);rosbag::Bag bag;//sukaibag.open(bag_filename, rosbag::bagmode::Read); //sukairosbag::View view(bag, rosbag::TopicQuery(topics.empty() ? bag.getTopics() : topics));for (const auto& msg : view) {ros::Time timestamp = msg.getTime();std::string topic = msg.getTopic();if (topic == "/image") {sensor_msgs::Image::ConstPtr image = msg.instantiate();if (image != nullptr) {pubs[0].publish(image);}}else if (topic == "/scan") {sensor_msgs::LaserScan::ConstPtr scan = msg.instantiate();if (scan != nullptr) {pubs[1].publish(scan);}}else if (topic == "/string") {std_msgs::String::ConstPtr str = msg.instantiate();if (str != nullptr) {pubs[2].publish(str);}}else {ROS_WARN_STREAM("Unknown topic: " << topic);}}// 关闭ROSbag文件bag.close();ROS_INFO_STREAM("Playback 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;
}

参考：

ros代码中获取ros节点名称，rostopic名称_再遇当年的博客-CSDN博客_ros查看节点名 自动获取topic名称与对应的消息类型

其中：void getTopics() 获取所有topic名称

1. cout << "C++打印出当前运行的所有topic name: " <

2. cout << "C++打印出当前运行的所有topic datatype: " <

例子：控制台打印的数据

    C++打印出当前运行的所有topic name: /map
    C++打印出当前运行的所有topic datatype: nav_msgs/OccupancyGrid

datatype: nav_msgs/OccupancyGrid这个数据需要把/替换成::后填入代码中