rosbag Code API

示例来源于：

http://wiki.ros.org/rosbag/Code%20API

http://wiki.ros.org/rosbag/Cookbook

Example usage for write:

#include 
#include 
#include 

rosbag::Bag bag;
bag.open("test.bag", rosbag::bagmode::Write);

std_msgs::String str;
str.data = std::string("foo");

std_msgs::Int32 i;
i.data = 42;

bag.write("chatter", ros::Time::now(), str);
bag.write("numbers", ros::Time::now(), i);

bag.close();

Example usage for read:

#include 
#include 
#include 
#include 
 
#include 
#define foreach BOOST_FOREACH
 
int main()
{
    rosbag::Bag bag;
    bag.open("test.bag", rosbag::bagmode::Read);
 
    std::vector topics;
    topics.push_back(std::string("chatter"));
    topics.push_back(std::string("numbers"));
 
    rosbag::View view(bag, rosbag::TopicQuery(topics));
 
    foreach(rosbag::MessageInstance const m, view)
    {
        std_msgs::String::ConstPtr s = m.instantiate();
        if (s != NULL)
            std::cout << s->data << std::endl;
 
        std_msgs::Int32::ConstPtr i = m.instantiate();
        if (i != NULL)
            std::cout << i->data << std::endl;
    }
 
    bag.close();
}

Another C++ example, using C++11:

#include 
#include 
#include 
rosbag::Bag bag;
bag.open("test.bag");  // BagMode is Read by default
for(rosbag::MessageInstance const m: rosbag::View(bag))
{
  std_msgs::Int32::ConstPtr i = m.instantiate();
  if (i != nullptr)
    std::cout << i->data << std::endl;
}
bag.close();

Analyzing Stereo Camera Data

Stereo camera data is stored on four separate topics: image topics for each camera sensor_msgs/Image, and camera info topics for each camera sensor_msgs/CameraInfo. In order to process the data, you need to synchronize messages from all four topics using a message_filters::TimeSynchronizer.

In this example, we're loading the entire bag file to memory before analyzing the images (as opposed to lazy loading).

#include 
#include 
#include 

#include 

#include 
#include 

#include 
#include 

// A struct to hold the synchronized camera data 
// Struct to store stereo data
class StereoData
{
public:
  sensor_msgs::Image::ConstPtr image_l, image_r;
  sensor_msgs::CameraInfo::ConstPtr cam_info_l, cam_info_r;
  
  StereoData(const sensor_msgs::Image::ConstPtr &l_img, 
             const sensor_msgs::Image::ConstPtr &r_img, 
             const sensor_msgs::CameraInfo::ConstPtr &l_info, 
             const sensor_msgs::CameraInfo::ConstPtr &r_info) :
    image_l(l_img),
    image_r(r_img),
    cam_info_l(l_info),
    cam_info_r(r_info)
  {}
};

/**
 * Inherits from message_filters::SimpleFilter
 * to use protected signalMessage function 
 */
template 
class BagSubscriber : public message_filters::SimpleFilter
{
public:
  void newMessage(const boost::shared_ptr &msg)
  {
    signalMessage(msg);
  }
};

// Callback for synchronized messages
void callback(const sensor_msgs::Image::ConstPtr &l_img, 
              const sensor_msgs::Image::ConstPtr &r_img, 
              const sensor_msgs::CameraInfo::ConstPtr &l_info,
              const sensor_msgs::CameraInfo::ConstPtr &r_info)
{
  StereoData sd(l_img, r_img, l_info, r_info);

  // Stereo dataset is class variable to store data
  stereo_dataset_.push_back(sd);
}
 
// Load bag
void loadBag(const std::string &filename)
{
  rosbag::Bag bag;
  bag.open(filename, rosbag::bagmode::Read);
  
  std::string l_cam = image_ns_ + "/left";
  std::string r_cam = image_ns_ + "/right";
  std::string l_cam_image = l_cam + "/image_raw";
  std::string r_cam_image = r_cam + "/image_raw";
  std::string l_cam_info = l_cam + "/camera_info";
  std::string r_cam_info = r_cam + "/camera_info";
  
  // Image topics to load
  std::vector topics;
  topics.push_back(l_cam_image);
  topics.push_back(r_cam_image);
  topics.push_back(l_cam_info);
  topics.push_back(r_cam_info);
  
  rosbag::View view(bag, rosbag::TopicQuery(topics));
  
  // Set up fake subscribers to capture images
  BagSubscriber l_img_sub, r_img_sub;
  BagSubscriber l_info_sub, r_info_sub;
  
  // Use time synchronizer to make sure we get properly synchronized images
  message_filters::TimeSynchronizer sync(l_img_sub, r_img_sub, l_info_sub, r_info_sub, 25);
  sync.registerCallback(boost::bind(&callback, _1, _2, _3, _4));
  
  // Load all messages into our stereo dataset
  BOOST_FOREACH(rosbag::MessageInstance const m, view)
  {
    if (m.getTopic() == l_cam_image || ("/" + m.getTopic() == l_cam_image))
    {
      sensor_msgs::Image::ConstPtr l_img = m.instantiate();
      if (l_img != NULL)
        l_img_sub.newMessage(l_img);
    }
    
    if (m.getTopic() == r_cam_image || ("/" + m.getTopic() == r_cam_image))
    {
      sensor_msgs::Image::ConstPtr r_img = m.instantiate();
      if (r_img != NULL)
        r_img_sub.newMessage(r_img);
    }
    
    if (m.getTopic() == l_cam_info || ("/" + m.getTopic() == l_cam_info))
    {
      sensor_msgs::CameraInfo::ConstPtr l_info = m.instantiate();
      if (l_info != NULL)
        l_info_sub.newMessage(l_info);
    }
    
    if (m.getTopic() == r_cam_info || ("/" + m.getTopic() == r_cam_info))
    {
      sensor_msgs::CameraInfo::ConstPtr r_info = m.instantiate();
      if (r_info != NULL)
        r_info_sub.newMessage(r_info);
    }
  }
  bag.close();
}