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

2024-03-19 00:36

本文主要是介绍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

在这里插入图片描述

3 总结

本文代码托管在本人github上:bag_operator

这篇关于ROS2高效学习第九章 -- ros2 bag之编程实现包录制的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/824217

相关文章

Linux下删除乱码文件和目录的实现方式

《Linux下删除乱码文件和目录的实现方式》:本文主要介绍Linux下删除乱码文件和目录的实现方式,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录linux下删除乱码文件和目录方法1方法2总结Linux下删除乱码文件和目录方法1使用ls -i命令找到文件或目录

SpringBoot+EasyExcel实现自定义复杂样式导入导出

《SpringBoot+EasyExcel实现自定义复杂样式导入导出》这篇文章主要为大家详细介绍了SpringBoot如何结果EasyExcel实现自定义复杂样式导入导出功能,文中的示例代码讲解详细,... 目录安装处理自定义导出复杂场景1、列不固定,动态列2、动态下拉3、自定义锁定行/列,添加密码4、合并

mybatis执行insert返回id实现详解

《mybatis执行insert返回id实现详解》MyBatis插入操作默认返回受影响行数,需通过useGeneratedKeys+keyProperty或selectKey获取主键ID,确保主键为自... 目录 两种方式获取自增 ID:1. ​​useGeneratedKeys+keyProperty(推

Spring Boot集成Druid实现数据源管理与监控的详细步骤

《SpringBoot集成Druid实现数据源管理与监控的详细步骤》本文介绍如何在SpringBoot项目中集成Druid数据库连接池,包括环境搭建、Maven依赖配置、SpringBoot配置文件... 目录1. 引言1.1 环境准备1.2 Druid介绍2. 配置Druid连接池3. 查看Druid监控

Linux在线解压jar包的实现方式

《Linux在线解压jar包的实现方式》:本文主要介绍Linux在线解压jar包的实现方式,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录linux在线解压jar包解压 jar包的步骤总结Linux在线解压jar包在 Centos 中解压 jar 包可以使用 u

c++ 类成员变量默认初始值的实现

《c++类成员变量默认初始值的实现》本文主要介绍了c++类成员变量默认初始值,文中通过示例代码介绍的非常详细,对大家的学习或者工作具有一定的参考学习价值,需要的朋友们下面随着小编来一起学习学习吧... 目录C++类成员变量初始化c++类的变量的初始化在C++中,如果使用类成员变量时未给定其初始值,那么它将被

Qt使用QSqlDatabase连接MySQL实现增删改查功能

《Qt使用QSqlDatabase连接MySQL实现增删改查功能》这篇文章主要为大家详细介绍了Qt如何使用QSqlDatabase连接MySQL实现增删改查功能,文中的示例代码讲解详细,感兴趣的小伙伴... 目录一、创建数据表二、连接mysql数据库三、封装成一个完整的轻量级 ORM 风格类3.1 表结构

基于Python实现一个图片拆分工具

《基于Python实现一个图片拆分工具》这篇文章主要为大家详细介绍了如何基于Python实现一个图片拆分工具,可以根据需要的行数和列数进行拆分,感兴趣的小伙伴可以跟随小编一起学习一下... 简单介绍先自己选择输入的图片,默认是输出到项目文件夹中,可以自己选择其他的文件夹,选择需要拆分的行数和列数,可以通过

Python中将嵌套列表扁平化的多种实现方法

《Python中将嵌套列表扁平化的多种实现方法》在Python编程中,我们常常会遇到需要将嵌套列表(即列表中包含列表)转换为一个一维的扁平列表的需求,本文将给大家介绍了多种实现这一目标的方法,需要的朋... 目录python中将嵌套列表扁平化的方法技术背景实现步骤1. 使用嵌套列表推导式2. 使用itert

Python使用pip工具实现包自动更新的多种方法

《Python使用pip工具实现包自动更新的多种方法》本文深入探讨了使用Python的pip工具实现包自动更新的各种方法和技术,我们将从基础概念开始,逐步介绍手动更新方法、自动化脚本编写、结合CI/C... 目录1. 背景介绍1.1 目的和范围1.2 预期读者1.3 文档结构概述1.4 术语表1.4.1 核