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, "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;
}

在上面的代码中：

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

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

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

• 接着获取了名为"my_topic"的话题的迭代器，并循环遍历话题中的所有消息。

• 在每个消息中，我们将其转换为String类型，并打印其内容。

• 最后关闭了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, "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;
    
}
 

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

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

3. 回放/提取数据包，并实时发布

#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);
 
return 0;
}