代码实现ROSbag录制数据和回放数据(C++)
代码实现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;//sukai
bag.open(bag_filename, rosbag::bagmode::Read); //sukai
rosbag::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名称
-
cout << "C++打印出当前运行的所有topic name: " <
-
cout << "C++打印出当前运行的所有topic datatype: " <
例子：控制台打印的数据
C++打印出当前运行的所有topic name: /map
C++打印出当前运行的所有topic datatype: nav_msgs/OccupancyGrid
datatype: nav_msgs/OccupancyGrid这个数据需要把/替换成::后填入代码中