ROS C++ : 读取RosBag包
文章目录
1. 读取单个话题
1.1. 核心代码
// 获取名为"my_topic"的话题的迭代器
rosbag::View view(bag, rosbag::TopicQuery("my_topic"));
1.2. 完整示例
#include <ros/ros.h>
#include <rosbag/bag.h>
#include <std_msgs/String.h>
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<std_msgs::String>();
if (msg != NULL)
{
// 打印消息内容
ROS_INFO("Message: %s", msg->data.c_str());
}
}
// 关闭bag文件
bag.close();
return 0;
}
2. 读取多个话题
2.1. 核心代码
// 获取三个话题的迭代器
rosbag::View view(bag, rosbag::TopicQuery("my_string_topic") && rosbag::TopicQuery("my_image_topic") && rosbag::TopicQuery("my_laser_topic"));
2.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, "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>())
{
std_msgs::String::ConstPtr str_msg = msg.instantiate<std_msgs::String>();
ROS_INFO_STREAM("String message: " << str_msg->data);
}
else if (msg.isType<sensor_msgs::Image>())
{
sensor_msgs::Image::ConstPtr img_msg = msg.instantiate<sensor_msgs::Image>();
ROS_INFO_STREAM("Image message: " << img_msg->header.stamp);
// 在此处添加处理Image消息的代码
}
else if (msg.isType<sensor_msgs::LaserScan>())
{
sensor_msgs::LaserScan::ConstPtr scan_msg = msg.instantiate<sensor_msgs::LaserScan>();
ROS_INFO_STREAM("LaserScan message: " << scan_msg->header.stamp);
// 在此处添加处理LaserScan消息的代码
}
}
// 关闭bag文件
bag.close();
return 0;
}
3. 读取全部话题
3.1. 核心代码
rosbag::View view(bag);
或者
rosbag::View view(bag, rosbag::TopicQuery(bag.getTopics()));
3.2. 读取方式1
rosbag::Bag bag;
bag.open(bagpath, rosbag::BagMode::Read);
if(!bag.isOpen())
{
cout<<"can't open bag file!"<<endl;
}
rosbag::View view(bag);
rosbag::View::iterator it = view.begin();
int iIndexTopic = 0;
for(;it!= view.end(); it++)
{
if(it->getTopic() == "") continue;
m[it->getTopic()] = 1;
cout << "bag : " << it->getTopic() << it->getDataType() <<endl;
}
3.3. 读取方式2
int topic_num = view.getConnections().size();
std::vector<std::string> tmp_topic;
for (int i = 0; i < topic_num; i++)
{
tmp_topic.push_back(view.getConnections().at(i)->topic);
}
3.4. 完整示例
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <ros/ros.h>
#include <string>
#include <vector>
// 定义一个函数，用于回放多个话题的数据
// 参数：
// - bag_filename: ROSbag文件名
// - topics: 一个包含多个话题名的vector，如果为空，则回放所有话题的数据
void playbackRosbag(const std::string& bag_filename, const std::vector<std::string>& topics = {})
{
// 创建ROS节点
// ros::NodeHandle nh("~");
ros::NodeHandle nh;//sukai
// 创建ROS话题发布器
std::vector<ros::Publisher> pubs;
for (const auto& topic : topics.empty() ? bag.getTopics() : topics)
{
if (topic == "/image")
{
pubs.push_back(nh.advertise<sensor_msgs::Image>(topic, 1));
}
else if (topic == "/scan")
{
pubs.push_back(nh.advertise<sensor_msgs::LaserScan>(topic, 1));
}
else if (topic == "/string")
{
pubs.push_back(nh.advertise<std_msgs::String>(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<sensor_msgs::Image>();
if (image != nullptr)
{
pubs[0].publish(image);
}
}
else if (topic == "/scan")
{
sensor_msgs::LaserScan::ConstPtr scan = msg.instantiate<sensor_msgs::LaserScan>();
if (scan != nullptr)
{
pubs[1].publish(scan);
}
}
else if (topic == "/string")
{
std_msgs::String::ConstPtr str = msg.instantiate<std_msgs::String>();
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文件中的所有话题数据
playbackRosbag(bag_filename);
// 回放ROSbag文件中的指定话题数据
std::vectorstd::string playback_topics = {"/scan", "/string"};
//playbackRosbag(bag_filename, playback_topics);
playbackRosbag(bag_filename);
return 0;
}
原文地址：https://blog.csdn.net/BullKing8185/article/details/142712767
免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！