ROS高效入门第八章 -- 掌握rosbag工具，编写c++包录制、读取、截断和回放样例

1 资料

ROS提供了一套工具，用来记录和回放程序运行时的topic数据，即rosbag工具。这套工具对测试机器人软件非常高效，即使是自动驾驶行业，也是深度使用这个工具。
本章先介绍rosbag的命令行用法，以及rostopic处理bag包的相关命令，分别是bag录制，读取，截断和回放。最后使用 c++ 编码实现相关的命令行功能，掌握 ros 处理bag包的API。本章参考资料如下：
（1）《机器人操作系统（ROS）浅析》[美] Jason M. O’Kane 著 肖军浩 译，第9章
（2）ros Tutorials 初级教程的10，17和18节： ros Tutorials
（3）chatgpt 4.0 （写代码大量参考了chatgpt给的样例，尽管不怎么solid）

2 正文

2.1 rosbag命令行工具

2.1.1 rosbag录制

（1）本节以turtlesim软件包为基础，演示rosbag包录制。首先，开四个窗口，分别执行：

roscore
rosrun turtlesim turtlesim_node
// 这个程序负责向turtlesim_node发送运动命令，让小乌龟画四方形
rosrun turtlesim draw_square

// rosbag record是录制bag的命令，并指定topic和bag名
rosbag record -O square_cmd_1.bag /turtle1/cmd_vel /turtle1/pose
// 录制所有topic，bag名会默认以时间戳命名
rosbag record -a

2.1.2 rosbag查看基本信息，读取内容和截断

（1）查看rosbag包的基本信息，可以看到包长，消息个数，类型，topic名，msg名

rosbag info square_cmd_1.bag

（2）读取rosbag包指定 topic 的所有 msg 内容

rostopic echo -b square_cmd_1.bag /turtle1/cmd_vel
rostopic echo -b square_cmd_1.bag /turtle1/pose

（3）按照时间片，截断rosbag包，生成一个新的短包

rosbag filter square_cmd_1.bag square_cmd_1_filter.bag "t.to_sec() > 1684977250.80 and t.to_sec() < 1684977450.45"

2.1.3 回放rosbag

（1）首先关闭前面的dram_square程序，然后在录制bag的窗口，执行：

rosbag play square_cmd_1_filter.bag
// 如果想倍速播放，可以使用 -r；
rosbag play -r 2 square_cmd_1_filter.bag
// 如果想偏移起点播放，可以使用 -s;
rosbag play -s 100 square_cmd_1_filter.bag

（2）仔细观察可以发现，回放的小乌龟轨迹与draw_square执行的结果是不一样的，ros Tutorials的第17节 录制和回放数据 里有一段解释：

2.2 C++实现rosbag命令行工具

2.2.1 C++录包样例

（1）创建 rosbag_test 软件包和文件，这个软件包将实现bag录制，读取，截断和回放四个功能，这里先讲解bag录制

cd ~/catkin_ws/src/cpp
// topic_tools 是用来写rosbag play程序用的
catkin_create_pkg rosbag_test rosbag turtlesim topic_tools roscpp rospy
cd rosbag_test 
mkdir launch
touch launch/start_cpp_record.launch src/bag_record.cpp

（2）编写bag_record.cpp

#include 
#include 
#include 
#include 
// topic_tools可以让使用者在不知道topic具体消息类型的情况下，订阅并存储消息
#include 
// 一个开源的，小型C++命令行解析工具：https://github.com/jarro2783/cxxopts
#include "cxxopts.hpp"

// bag的write接口，需要传入三个参数，其中消息类型是通用的;
// 这里需要注意的是，spin()是串行调用这个通用的回调函数，因此不存在并发问题，不用特殊保护！
void genericCallback(const topic_tools::ShapeShifter::ConstPtr& msg, rosbag::Bag* bag, const std::string& topic_name) {
    bag->write(topic_name, ros::Time::now(), *msg);
}

int main(int argc, char ** argv) {
    ros::init(argc, argv, "bag_record");
    ros::NodeHandle nh;
	// 通过参数可以看出这个bag_record的功能：支持指定输出的bag名；支持录制指定的topic；支持录制所有活跃的topic
    cxxopts::Options options(argv[0], "parse cmd line");
    options.add_options()
        ("O,output", "Name of the bag", cxxopts::value<std::string>())
        ("t,topic", "topic list", cxxopts::value<std::vector<std::string>>())
        ("a,all", "all topic")
        ("h,help", "show help");
    auto result = options.parse(argc, argv);
    if (result.count("help")) {
        ROS_INFO("%s", options.help().c_str());
        return 0;
    }
	// -t 和 -a 必须要有一个
    if (!result.count("topic") && !result.count("all")) {
        ROS_ERROR("please specify topic using -t or -a");
        return -1;
    }
	// 如果不用-O指定bag名，则默认以格式化的时间戳命名
    std::time_t current_time = std::time(nullptr);
    std::tm* time_info = std::localtime(&current_time);
    char buffer[50];
    std::strftime(buffer, sizeof(buffer), "%Y-%m-%d-%H-%M-%S", time_info);
    std::string bag_name = "./" + std::string(buffer) + ".bag";
    if (result.count("output")) {
        bag_name = result["output"].as<std::string>();
    }
	// 如果 -a 和 -t 同时出现，则以 -a 为准
    std::vector<std::string> topic_list;
    if (result.count("all")) {
        ros::master::V_TopicInfo topic_infos;
        // 这个接口可以从master那里获取当前活跃的topic
        if (ros::master::getTopics(topic_infos)) {
            for (const auto & topic_info : topic_infos) {
                topic_list.push_back(topic_info.name);
            }
        }
    } else if (result.count("topic")) {
        topic_list = result["topic"].as<std::vector<std::string>>();
    }

    rosbag::Bag bag;
    try {
        bag.open(bag_name.c_str(), rosbag::bagmode::Write);
    } catch (rosbag::BagException &e) {
        ROS_ERROR("open rosbag failed: %s", e.what());
        return -1;
    }
	// 使用 topic_tools 创建subscriber，并且一定不要忘了持久化，否则spin() 无法调用他们的回调函数！
    std::vector<ros::Subscriber> sub_list;
    for (auto & item : topic_list) {
        ros::Subscriber sub = nh.subscribe<topic_tools::ShapeShifter>(item, 1000, std::bind(genericCallback, std::placeholders::_1, &bag, item));        
        sub_list.push_back(sub);
        ROS_INFO("subscribed to %s", item.c_str());
    }
    ros::spin();
    // 由于spin()不捕捉ctrl+c，所以这里的bag.close()没有被执行，但测试发现，并没有什么问题
    bag.close();
    return 0;
}

（3）编写start_cpp_record.launch

<launch>
<node
    pkg="turtlesim"
    type="turtlesim_node"
    name="turtlesim"
    required="true"
/>
<node
    pkg="turtlesim"
    type="draw_square"
    name="draw_square"
    required="true"
/>
    // 等5秒再启动record，否则另外两个节点没起来，record看不到topic
<node
    pkg="rosbag_test"
    type="rosbag_test_record"
    name="bag_record"
    args="-O /home/ycao/Study/ros_noetic/bag_dir/square_1.bag -a"
    output="screen"
    launch-prefix="bash -c 'sleep 5; $0 $@'"
/>
launch>

（4）编译和运行（由于四个样例都在 rosbag_test 包里，因此最后一个样例再把CmakeLists放上来）

cd ~/catkin_ws/
catkin_make --source src/cpp/rosbag_test/
source devel/setup.bash
roslaunch rosbag_test start_cpp_record.launch

2.2.2 C++查看包样例

（1）创建并编写bag_echo.cpp

cd ~/catkin_ws/src/cpp/rosbag_test
touch src/bag_echo.cpp

bag_echo.cpp

#include 
#include 
#include 
// 为了能实现通用的bag内容查看工具，不依赖具体的msg类型，因此引入了这个开源库，其内部实现了反序列化功能
// https://github.com/facontidavide/ros_msg_parser
#include "ros_msg_parser/ros_parser.hpp"

// https://github.com/jarro2783/cxxopts
#include "cxxopts.hpp"

int main(int argc, char ** argv) {
	// 由于查看rosbag是离线工具，因此不需要创建ros::NodeHandle句柄
    ros::init(argc, argv, "bar_echo");
	// 该工具支持查看指定topic内容，可以是一个，也可以是多个
    cxxopts::Options options(argv[0], "parse cmd line");
    options.add_options()
        ("b,bag", "Name of the bag", cxxopts::value<std::string>())
        ("t,topic", "topic list", cxxopts::value<std::vector<std::string>>())
        ("h,help", "show help");
    auto result = options.parse(argc, argv);
    if (result.count("help")) {
        ROS_INFO("%s", options.help().c_str());
        return 0;
    }
    if (!result.count("bag")) {
        ROS_ERROR("please specify bag using -b");
        return -1;
    } 
    if (!result.count("topic")) {
        ROS_ERROR("please specify a topic using -t");
        return -1;
    } 
    std::string bag_name;
    if (result.count("bag")) {
        bag_name = result["bag"].as<std::string>();
    }    
    std::vector<std::string> topic_list;
    if (result.count("topic")) {
        topic_list = result["topic"].as<std::vector<std::string>>();
    }  

    rosbag::Bag bag;
    try {
        bag.open(bag_name.c_str(), rosbag::bagmode::Read);
    } catch (rosbag::BagException &e) {
        ROS_ERROR("open rosbag failed: %s", e.what());
        return -1;
    }
	// 从bag中提取特定topic的内容，如果不传入topic_query，则默认提取所有topic内容
    rosbag::TopicQuery topic_query(topic_list);
    rosbag::View view(bag, topic_query);
    
	// 下面是完全参考ros_msg_parser的样例而来
	ROS_INFO("starting to parse bag...");
    RosMsgParser::ParsersCollection parsers;
    for (const rosbag::ConnectionInfo* connection : view.getConnections()) {
        const std::string& topic_name = connection->topic;
        parsers.registerParser(topic_name, *connection);
    }
    for(const rosbag::MessageInstance& item : view) {
        std::string topic_name = item.getTopic();
        std::string data_type = item.getDataType();
        std::string md5_val = item.getMD5Sum();
        double time_sec = item.getTime().toSec();
        std::string msg_def = item.getMessageDefinition();

        ROS_INFO("--------- %s ----------\n", topic_name.c_str());
        const auto deserialized_msg = parsers.deserialize(topic_name, item);
        for (const auto& it : deserialized_msg->renamed_vals) {
            const std::string& key = it.first;
            const double value = it.second;
            ROS_INFO(" %s = %f\n", key.c_str(), value);
        }
        for (const auto& it : deserialized_msg->flat_msg.name) {
            const std::string& key = it.first.toStdString();
            const std::string& value = it.second;
            ROS_INFO(" %s = %s\n", key.c_str(), value.c_str());
        }
    }
    return 0;
}

（3）编译和运行

cd ~/catkin_ws/
catkin_make --source src/cpp/rosbag_test/
source devel/setup.bash
./devel/lib/rosbag_test/rosbag_test_echo -b ../bag_dir/square_1.bag -t /rosout

2.2.3 C++截取包样例

（1）创建并编写bag_filter.cpp

cd ~/catkin_ws/src/cpp/rosbag_test
touch src/bag_filter.cpp

bag_filter.cpp

#include 
#include 
#include 
// https://github.com/jarro2783/cxxopts
#include "cxxopts.hpp"

bool findTopic(const std::string topic, const std::vector<std::string>& topic_list) {
    if (topic_list.empty()) {
        return true;
    }
    for (auto & item : topic_list) {
        if (topic == item) {
            return true;
        }
    }
    return false;
}

int main(int argc, char ** argv) {
    ros::init(argc, argv, "bag_filter");
	// 支持根据前后时间偏移量截取，也支持指定topic，如果不指定，默认是所有topic
    cxxopts::Options options(argv[0], "parse cmd line");
    options.add_options()
        ("i,input_bag", "Name of the input bag", cxxopts::value<std::string>())
        ("o,output_bag", "Name of the output bag", cxxopts::value<std::string>())
        ("s,start_offset", "offset time base start time", cxxopts::value<int>())
        ("e,end_offset", "offset time base end time", cxxopts::value<int>())
        ("t,topic", "topic list", cxxopts::value<std::vector<std::string>>())
        ("h,help", "show help");
    auto result = options.parse(argc, argv);
    if (result.count("help")) {
      ROS_INFO("%s", options.help().c_str());
      return 0;
    }
    if (!result.count("input_bag")) {
      ROS_ERROR("please specify input bag using -i");
      return -1;
    }
    if (!result.count("output_bag")) {
      ROS_ERROR("please specify output bag using -o");
      return -1;
    }    

    std::string input_bag;
    std::string output_bag;
    int start_offset = 0;
    int end_offset = 0;
    std::vector<std::string> topic_list;
    if (result.count("input_bag")) {
        input_bag = result["input_bag"].as<std::string>();
    }  
    if (result.count("output_bag")) {
        output_bag = result["output_bag"].as<std::string>();
    }  
    if (result.count("start_offset")) {
        start_offset = result["start_offset"].as<int>();
    }  
    if (result.count("end_offset")) {
        end_offset = result["end_offset"].as<int>();
    }  
    if (result.count("topic")) {
        topic_list = result["topic"].as<std::vector<std::string>>();
    }  

    rosbag::Bag ibag;
    try {
        ibag.open(input_bag.c_str(), rosbag::bagmode::Read);
    } catch (rosbag::BagException& e) {
        ROS_ERROR("open input bag failed: %s", e.what());
        return -1;
    }
    rosbag::Bag obag;
    try {
        obag.open(output_bag.c_str(), rosbag::bagmode::Write);
    } catch (rosbag::BagException& e) {
        ROS_ERROR("open output bag failed: %s", e.what());
        return -1;
    }
    rosbag::View view(ibag);    
    
    // 获取起止时间，利用findTopic()函数，决定是否提取处当前的topic
    ros::Time begin_time = view.getBeginTime() + ros::Duration(start_offset, 0);
    ros::Time end_time = view.getEndTime() - ros::Duration(end_offset, 0);
    for (const auto& msg : view) {
        if (msg.getTime() >=  begin_time && msg.getTime() <= end_time) {
            if (findTopic(msg.getTopic(), topic_list)) {
                obag.write(msg.getTopic(), msg.getTime(), msg);
            }
        }
    }

    ibag.close();
    obag.close();
    ROS_INFO("successfully filtered bag : %s", output_bag.c_str());
    return 0;
}

（3）编译和运行

cd ~/catkin_ws/
catkin_make --source src/cpp/rosbag_test/
source devel/setup.bash
// 查看使用帮助
./devel/lib/rosbag_test/rosbag_test_filter -h
./devel/lib/rosbag_test/rosbag_test_filter -i ../bag_dir/square_1.bag -o ../bag_dir/square_1_filter.bag -s 5 -e 5 -t /turtle1/cmd_vel -t /turtle1/pose

2.2.4 C++包播放样例

（1）创建并编写bag_play.cpp

cd ~/catkin_ws/src/cpp/rosbag_test
touch src/bag_play.cpp

bag_play.cpp

#include 
#include 
#include 
#include 
#include 
// https://github.com/jarro2783/cxxopts
#include "cxxopts.hpp"

bool findTopic(const std::string topic, const std::vector<std::string>& topic_list) {
    if (topic_list.empty()) {
        return true;
    }
    for (auto & item : topic_list) {
        if (topic == item) {
            return true;
        }
    }
    return false;
}

int main(int argc, char ** argv) {
  ros::init(argc, argv, "bag_play");
  ros::NodeHandle nh;
  // 支持偏移起点播放，支持倍速播放，支持播放特定的topic，如果不指定，默认是所有topic
  cxxopts::Options options(argv[0], "parse cmd line");
  options.add_options()
      ("b,bag", "name of the bag", cxxopts::value<std::string>())
      ("s,start_offset", "sec offset of start time", cxxopts::value<int>())
      ("r,rate", "player rate", cxxopts::value<double>())
      ("t,topic", "topic list", cxxopts::value<std::vector<std::string>>())
      ("h,help", "show help");
  auto result = options.parse(argc, argv);  
  if (result.count("help")) {
    ROS_INFO("%s", options.help().c_str());
    return 0;
  }
  if (!result.count("bag")) {
    ROS_ERROR("please specify bag using -b");
    return -1;
  }   

  std::string bag_name;
  int start_offset_sec = 0;
  double play_delay_sec = 2.0;
  double play_rate = 1.0;
  std::vector<std::string> topic_list;
  if (result.count("bag")) {
    bag_name = result["bag"].as<std::string>();
  }  
  if (result.count("start_offset")) {
    start_offset_sec = result["start_offset"].as<int>();
  }
  if (result.count("rate")) {
    play_rate = result["rate"].as<double>();
  }  
  if (result.count("topic")) {
      topic_list = result["topic"].as<std::vector<std::string>>();
  } 

  rosbag::Bag bag;
  try {
    bag.open(bag_name.c_str(), rosbag::bagmode::Read);
  } catch (rosbag::BagException& e) {
    ROS_ERROR("open bag failed %s", e.what());
    return -1;
  }
  rosbag::View view(bag);
  // 利用这个view.getConnections()可以获取bag中一个msg的所有相关信息，除了消息内容
  std::map<std::string, ros::Publisher> pubs;
  const std::vector<const rosbag::ConnectionInfo*> connections = view.getConnections();
  for (auto& item : connections) {
    if (findTopic(item->topic, topic_list)) {
      std::string topic_name = item->topic;
      ROS_INFO("%s %s %s", topic_name.c_str(), item->datatype.c_str(), item->md5sum.c_str()/*, item->msg_def.c_str()*/);
      ros::AdvertiseOptions opts;
      opts.topic = topic_name;
      opts.queue_size = 1000;
      opts.md5sum = item->md5sum;
      opts.datatype = item->datatype;
      opts.message_definition = item->msg_def;
      // 利用nh.advertise()这个构造函数，可以在不依赖具体消息类型的情况下，创建publisher
      pubs[topic_name] = nh.advertise(opts);
    }
  }
  // 播放之前，等一下，避免头部数据丢失
  ROS_INFO("start play bag after %f sec...", play_delay_sec);
  ros::Duration(0, (int)(play_delay_sec * 1e9)).sleep();

  ros::Time begin_time = view.getBeginTime() + ros::Duration(start_offset_sec, 0);
  ros::Duration stone_delta = ros::Time::now() - begin_time;
  ros::Duration sum_du = ros::Duration();
  for (const auto& msg : view) {
    if (msg.getTime() >= begin_time) {
      // 这里实现了倍速播放的逻辑，调试了好久！后面的python实现的bag_play.py，对此进行了优化，逻辑更清晰
      ros::Duration msg_delta = ros::Duration(ros::Duration(msg.getTime() - begin_time).toSec() / play_rate);
      while (begin_time + msg_delta + stone_delta > ros::Time::now()) {
        ros::Duration du = ros::Duration(ros::Duration(msg.getTime() - begin_time).toSec()/play_rate) - sum_du;
        sum_du += du;
        du.sleep();
      }
	  // 这里是实现了进度显示功能，核心是：\r
      printf("[RUNNING] Bag Time: %.6f  Duration: %.6f\r", msg.getTime().toSec(), ros::Duration(msg.getTime() - begin_time).toSec());
      fflush(stdout);
	  // 使用topic_tools::ShapeShifter，可以在不知道msg类型的情况下，将bag里的msg播放出去，非常关键！
      if (findTopic(msg.getTopic(), topic_list)) {
        topic_tools::ShapeShifter::ConstPtr smsg = msg.instantiate<topic_tools::ShapeShifter>();
        pubs[msg.getTopic()].publish(smsg);
      }
    }
    if (!ros::ok()) {
      break;
    }
  }
  // 这句换行受不了，不然进度条留不住
  printf("\n");
  bag.close();
  return 0;
}

（3）创建并编写start_cpp_play.launch

cd ~/catkin_ws/src/cpp/rosbag_test
touch launch/start_cpp_play.launch

start_cpp_play.launch

<launch>
<node
    pkg="turtlesim"
    type="turtlesim_node"
    name="turtlesim"
    required="true"
/>
<node
    pkg="rosbag_test"
    type="rosbag_test_play"
    name="bag_play"
    args="-b /home/ycao/Study/ros_noetic/bag_dir/square_1.bag -t /turtle1/cmd_vel"
    output="screen"
/>
launch>

（4）编写最终版CMakeLists.txt，这四个样例都是用的这个CMakeLists

cmake_minimum_required(VERSION 3.0.2)
project(rosbag_test)
find_package(catkin REQUIRED COMPONENTS
  topic_tools
  rosbag
  roscpp
  rospy
  turtlesim
)
catkin_package()
include_directories(
  include/rosbag_test
  ${catkin_INCLUDE_DIRS}
  thirdparty/ros_msg_parser/include
)
add_executable(${PROJECT_NAME}_record src/bag_record.cpp)
add_executable(${PROJECT_NAME}_echo src/bag_echo.cpp)
add_executable(${PROJECT_NAME}_filter src/bag_filter.cpp)
add_executable(${PROJECT_NAME}_play src/bag_play.cpp)
target_link_libraries(${PROJECT_NAME}_record
  ${catkin_LIBRARIES}
)
target_link_libraries(${PROJECT_NAME}_echo
  ros_msg_parser
  ${catkin_LIBRARIES}
)
target_link_libraries(${PROJECT_NAME}_filter
  ${catkin_LIBRARIES}
)
target_link_libraries(${PROJECT_NAME}_play
  ${catkin_LIBRARIES}
)

（5）编译并运行

cd ~/catkin_ws/
catkin_make --source src/cpp/rosbag_test/
source devel/setup.bash
roslaunch rosbag_test start_cpp_play.launch

3 总结

本章的四个cpp样例是非常通用的，完全可以作为命令行工具的平替。下一章，我们将使用python重新实现这四个样例。本文中的例子放在了本人的github上： ros_src