ros2 bag之 cpp 实现在线录制 topic 其一
1 前言和资料
在ROS2高效学习第二章 – ros2常用命令和相关概念学习，熟练玩起来小乌龟样例这篇博客里，我们简单介绍了 ros2 的录包和播包命令，以及与 ros1 包格式的区别。更早一点，我们在ROS高效入门第八章 – 掌握rosbag工具，编写c++包录制、读取、截断和回放样例中，使用 cpp 编程实现了 ros1 bag的录制。
在大型机器人项目中，通常需要在正常功能程序之外，开发一个 topic 录制模块，负责实时采集机器人运行时的数据，尤其是故障时的运行数据。数据回流后，通过离线回放进行场景复现，排查问题。这个 topic 录制模块，通常也是采用 cpp 实现，性能远比 python 实现更优。
本文我们将参考 ROS高效入门第八章 – 掌握rosbag工具，编写c++包录制、读取、截断和回放样例 中的 bag_record 样例，使用 ros2 的接口，实现一个 topic 录制节点，功能等同于 ros2 bag record 命令行。
本文参考资料如下：
（1）ROS2高效学习第二章 – ros2常用命令和相关概念学习，熟练玩起来小乌龟样例 第2.9节
（2）ROS高效入门第八章 – 掌握rosbag工具，编写c++包录制、读取、截断和回放样例 第2.2.1节
（3）Recording-A-Bag-From-Your-Own-Node-CPP
2 正文
2.1 bag_record 功能介绍
（1）bag_record 的功能同 ros2 bag record 命令，支持 -a 录制所有 topic ，支持 -t 录制指定 topic（可指定多个），支持 -o 指定输出 bag 名。其中 -o 是可选的，-a 和 -t 二选一。
2.2 bag_operator 之 bag_record
（1）创建 bag_operator 包以及文件
cd ~/colcon_ws/src
ros2 pkg create --build-type ament_cmake --license Apache-2.0 bag_operator --dependencies rclcpp rosbag2_cpp
cd bag_operator
touch src/bag_record.cpp
（2）编写 bag_record.cpp
#include <ctime>
#include <iomanip>
#include <rclcpp/rclcpp.hpp>
#include <rosbag2_cpp/writer.hpp>
// 命令行文件以源码的方式引入了软件包，源文件：https://github.com/jarro2783/cxxopts
#include "bag_operator/cxxopts.hpp"
class BagRecord : public rclcpp::Node {
public:
BagRecord(const std::string& output_name, const std::vector<std::string>& topic_list = {}) :
Node("bag_record"), writer_(std::make_unique<rosbag2_cpp::Writer>()) {
init(output_name, get_topic_msg_pair(topic_list));
}
private:
std::vector<std::pair<std::string, std::string>> get_topic_msg_pair(const std::vector<std::string>& topic_list) {
bool is_all = topic_list.empty();
std::vector<std::pair<std::string, std::string>> topic_msg_pair;
// 获取当前系统所有注册的 topic 名和对应的 msg 名
auto topics_and_types = this->get_topic_names_and_types();
for (const auto &topic_info : topics_and_types) {
topic_msg_pair.push_back(std::make_pair(topic_info.first, topic_info.second[0]));
}
std::vector<std::pair<std::string, std::string>> tmp_pair;
if (is_all) {
tmp_pair = topic_msg_pair;
} else {
// 如果指定了 topic，则从中抽出来指定的 topic 名和 msg 名
for (const auto &topic_name : topic_list) {
auto iter = std::find_if(topic_msg_pair.begin(), topic_msg_pair.end(),
[&topic_name](const auto& pair) { return pair.first == topic_name; });
if (iter != topic_msg_pair.end()) {
tmp_pair.emplace_back(*iter);
continue;
}
}
}
return tmp_pair;
}
void init(const std::string& output_name, std::vector<std::pair<std::string, std::string>> topic_msg_pair) {
writer_->open(output_name);
for (const auto &topic_info : topic_msg_pair) {
RCLCPP_INFO(this->get_logger(), "record topic list: [%s, %s]", topic_info.first.c_str(), topic_info.second.c_str());
// create_generic_subscription() 接口指定订阅通用类型的 topic，不需要引入特定的 msg 头文件
// 这是整个程序的关键 ！！！
auto sub = this->create_generic_subscription(
topic_info.first, topic_info.second, 10,
std::bind(&BagRecord::topic_callback, this, std::placeholders::_1, topic_info.first, topic_info.second),
rclcpp::SubscriptionOptions());
sub_list_.push_back(sub);
}
}
// 这里获取的 msg 是序列化之后的数据，bag 里存入的就是这种格式
void topic_callback(const std::shared_ptr<rclcpp::SerializedMessage>& msg,
const std::string& topic_name, const std::string& msg_type) {
rclcpp::Time time_stamp = this->now();
// 带着时间戳写入数据到bag文件
writer_->write(msg, topic_name, msg_type, time_stamp);
}
private:
std::vector<rclcpp::SubscriptionBase::SharedPtr> sub_list_;
std::unique_ptr<rosbag2_cpp::Writer> writer_;
};
int main(int argc, char * argv[]) {
rclcpp::init(argc, argv);
// 使用 cxxopts 作为 cpp 命令行工具
// https://github.com/jarro2783/cxxopts
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")) {
RCLCPP_INFO(rclcpp::get_logger("bag_record"), "%s", options.help().c_str());
return 0;
}
// -a 和 -t 必须指定一个
if (!result.count("topic") && !result.count("all")) {
RCLCPP_ERROR(rclcpp::get_logger("bag_record"), "please specify topic using -t or -a");
return -1;
}
// 如果指定了 -o ，就用指定的名称命名输出 bag，路径是在当前命令行执行目录
// 如果没有指定 -o ，就用格式化的时间戳命名输出 bag
std::time_t current_time = std::time(nullptr);
std::tm* time_info = std::localtime(¤t_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 ，就自动获取当前系统所有 topic，然后订阅他们，全部录制
// 如果指定 -t ，就只录制指定的 topic
std::shared_ptr<BagRecord> bag_record_ptr;
if (result.count("all")) {
bag_record_ptr = std::make_shared<BagRecord>(bag_name);
} else if (result.count("topic")) {
bag_record_ptr = std::make_shared<BagRecord>(bag_name, result["topic"].as<std::vector<std::string>>());
}
rclcpp::spin(bag_record_ptr);
rclcpp::shutdown();
return 0;
}
（3）编写 CMakeLists.txt
cmake_minimum_required(VERSION 3.8)
project(bag_operator)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rosbag2_cpp REQUIRED)
add_executable(bag_record src/bag_record.cpp)
target_include_directories(bag_record PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}/include)
ament_target_dependencies(bag_record rclcpp rosbag2_cpp)
install(TARGETS
bag_record
DESTINATION lib/${PROJECT_NAME})
ament_package()
（4）编译并测试录包
# 开一个窗口，启动小乌龟
ros2 run turtlesim turtlesim_node
# 再开一个窗口，启动方向键控制小屋过
ros2 run turtlesim turtle_teleop_key
# 再开一个窗口，编译并启动录制
~/colcon_ws
colcon build --packages-select bag_operator
source install/local_setup.bash
./install/bag_operator/lib/bag_operator/bag_record -a
# 鼠标回到 turtle_teleop_key 所在窗口，使用方向键控制小乌龟跑
# 回到录制窗口，查看录制的bag
ros2 bag info 2024-03-14-18-09-20-bag/2024-03-14-18-09-20-bag_0.db3
（5）回放录制的bag
# 回到小乌龟窗口，重启小乌龟
ros2 run turtlesim turtlesim_node
# 回到录制窗口，执行回放
# 可以看出回放效果与上面的略有不同，这是精度的原因
ros2 bag play 2024-03-14-18-09-20-bag/2024-03-14-18-09-20-bag_0.db3
3 总结
本文代码托管在本人github上：bag_operator
196
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
