ROS2高效学习第九章 -- ros2 bag之编程实现包录制

本文主要是介绍ROS2高效学习第九章 -- ros2 bag之编程实现包录制，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

ros2 bag之编程实现包录制

1 前言和资料
2 正文
- 2.1 bag_record 功能介绍
- 2.2 bag_operator 之 bag_record
3 总结

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/cxxoptscxxopts::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 ，就用格式化的时间戳命名输出 bagstd::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 ，就自动获取当前系统所有 topic，然后订阅他们，全部录制// 如果指定 -t ，就只录制指定的 topicstd::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_recordDESTINATION 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

在这里插入图片描述