【ROS2】高级:从节点记录一个包（C++）

目标：将数据从您自己的 C++ 节点记录到一个包中。

 教程级别：高级

 时间：20 分钟

 目录

•  背景

•  先决条件

•  任务

  • 1. 创建一个包

  • 2. 编写 C++ 节点

  • 3. 建立并运行

  • 4. 从节点记录合成数据

  • 5. 从可执行文件记录合成数据

•  摘要

 背景

rosbag2 不仅提供 ros2 bag 命令行工具，还提供了一个 C++ API，用于从您自己的源代码中读取和写入 bag 文件。这使您可以订阅一个主题，并在对数据进行任何其他处理的同时，将接收到的数据保存到 bag 文件中。

 先决条件

您应该在常规的 ROS 2 设置中安装 rosbag2 软件包。

如果您已经从 Linux 上的 Debian 软件包安装，它可能会默认安装。如果没有，您可以使用此命令安装它。

sudo apt install ros-jazzy-rosbag2

本教程讨论了如何使用 ROS 2 包，包括从终端使用。您应该已经完成了基本的 ROS 2 包教程。

任务

1. 创建一个包

打开一个新的终端并且初始化您的 ROS 2 安装，这样 ros2 命令就会生效。

导航到在上一个教程中创建的 ros2_ws 目录。导航到 ros2_ws/src 目录并创建一个新包：

ros2 pkg create --build-type ament_cmake --license Apache-2.0 bag_recorder_nodes --dependencies example_interfaces rclcpp rosbag2_cpp std_msgs

您的终端将返回一条消息，验证包 bag_recorder_nodes 及其所有必要文件和文件夹的创建。 --dependencies 参数将自动添加必要的依赖行到 package.xml 和 CMakeLists.txt 。在这种情况下，该包将使用 rosbag2_cpp 包以及 rclcpp 包。后续部分的教程还需要依赖 example_interfaces 包。

 1.1 更新 package.xml 

因为您在创建包时使用了 --dependencies 选项，您无需手动将依赖项添加到 package.xml 或 CMakeLists.txt 。但是，一如既往，请确保将描述、维护者电子邮件和姓名以及许可信息添加到 package.xml 。

<description>C++ bag writing tutorial</description>
  <maintainer email="cxy@126.com">cxy</maintainer>
  <license>Apache-2.0</license>

编写 C++ 节点

在 ros2_ws/src/bag_recorder_nodes/src 目录中，创建一个名为 simple_bag_recorder.cpp 的新文件，并将以下代码粘贴到其中。

#include <rclcpp/rclcpp.hpp> // 包含 ROS 2 的 C++ 客户端库
#include <std_msgs/msg/string.hpp> // 包含 std_msgs/String 消息类型


#include <rosbag2_cpp/writer.hpp> // 包含 rosbag2_cpp 的 Writer 类


class SimpleBagRecorder : public rclcpp::Node // 定义一个继承自 rclcpp::Node 的类，名为 SimpleBagRecorder
{
public:
  SimpleBagRecorder() // 类的构造函数
  : Node("simple_bag_recorder") // 调用父类构造函数，初始化节点名称为 "simple_bag_recorder"
  {
    writer_ = std::make_unique<rosbag2_cpp::Writer>(); // 创建一个 rosbag2_cpp::Writer 对象的唯一指针


    writer_->open("my_bag"); // 打开一个名为 "my_bag" 的 rosbag 文件


    auto subscription_callback_lambda = [this](std::shared_ptr<rclcpp::SerializedMessage> msg){
      // 定义一个 lambda 回调函数，当接收到消息时调用
      rclcpp::Time time_stamp = this->now(); // 获取当前时间戳


      writer_->write(msg, "chatter", "std_msgs/msg/String", time_stamp); // 将消息写入 rosbag 文件
    };


    subscription_ = create_subscription<std_msgs::msg::String>(
      // 创建一个订阅者，订阅 "chatter" 话题，队列大小为 10，并指定回调函数
      "chatter", 10, subscription_callback_lambda);
  }


private:


  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_; // 定义一个订阅者指针
  std::unique_ptr<rosbag2_cpp::Writer> writer_; // 定义一个 rosbag2_cpp::Writer 的智能指针
};


int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv); // 初始化 ROS 2
  rclcpp::spin(std::make_shared<SimpleBagRecorder>()); // 创建 SimpleBagRecorder 节点并进入事件循环
  rclcpp::shutdown(); // 关闭 ROS 2
  return 0; // 返回 0 表示程序正常结束
}

检查代码 2.1

顶部的 #include 语句是包依赖项。请注意， rosbag2_cpp 包中的头文件包含了处理包文件所需的函数和结构。

在类构造函数中，我们首先创建将用于写入包的写入器对象。

writer_ = std::make_unique<rosbag2_cpp::Writer>();

现在我们有了一个 writer 对象，可以使用它打开包。我们只需指定要创建的包的 URI，其他选项保持默认。使用默认存储选项，这意味着将创建一个 mcap 格式的包。默认转换选项也被使用，这将不进行任何转换，而是将消息存储在接收到的序列化格式中。

writer_->open("my_bag");

现在 writer 已经设置好记录我们传递给它的数据，我们创建一个订阅并为其指定一个回调。我们将在回调中将数据写入包中。

auto subscription_callback_lambda = [this](std::shared_ptr<rclcpp::SerializedMessage> msg){
  rclcpp::Time time_stamp = this->now();


  writer_->write(msg, "chatter", "std_msgs/msg/String", time_stamp);
};


subscription_ = create_subscription<std_msgs::msg::String>(
  "chatter", 10, subscription_callback_lambda);

回调本身不同于典型的回调。我们不是接收主题数据类型的实例，而是接收一个 rclcpp::SerializedMessage 。我们这样做有两个原因。

1. 消息数据需要在写入包之前由 rosbag2 序列化，因此与其在接收数据时反序列化然后重新序列化，我们要求 ROS 直接给我们序列化的消息。

2.  writer  API 可以接受序列化消息。

auto subscription_callback_lambda = [this](std::shared_ptr<rclcpp::SerializedMessage> msg){

在订阅回调中，首先要做的是确定用于存储消息的时间戳。这可以是适合您的数据的任何内容，但两个常见的值是数据生成的时间（如果已知）和接收的时间。这里使用第二个选项，即接收时间。

rclcpp::Time time_stamp = this->now();

然后我们可以将消息写入包中。因为我们尚未在包中注册任何主题，所以必须在消息中指定完整的主题信息。这就是为什么我们传入主题名称和主题类型。

writer_->write(msg, "chatter", "std_msgs/msg/String", time_stamp);

该类包含两个成员变量。

1.  订阅对象。

2. 用于写入包的写入器对象的托管指针。请注意，这里使用的写入器类型是 rosbag2_cpp::Writer ，即通用写入器接口。其他写入器可能具有不同的行为。

rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
std::unique_ptr<rosbag2_cpp::Writer> writer_;

文件以 main 函数结束，该函数用于创建节点实例并开始处理 ROS。

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<SimpleBagRecorder>());
  rclcpp::shutdown();
  return 0;
}

 2.2 添加可执行文件

现在打开 CMakeLists.txt 文件。

在文件顶部附近，将 CMAKE_CXX_STANDARD 从 14 更改为 17 。

# Default to C++17
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 17)
endif()

在包含 find_package(rosbag2_cpp REQUIRED) 的依赖块下面，添加以下代码行。

add_executable(simple_bag_recorder src/simple_bag_recorder.cpp)
ament_target_dependencies(simple_bag_recorder rclcpp rosbag2_cpp std_msgs)


install(TARGETS
  simple_bag_recorder
  DESTINATION lib/${PROJECT_NAME}
)

完整CMakeLists.txt

cmake_minimum_required(VERSION 3.8) 
# 指定 CMake 的最低版本为 3.8


project(bag_recorder_nodes) 
# 定义项目名称为 bag_recorder_nodes


# Default to C++17
# 默认使用 C++17 标准
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 17)
endif()


if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
  # 如果使用的是 GNU 编译器或 Clang，则添加编译选项 -Wall -Wextra -Wpedantic
endif()


# find dependencies
# 查找依赖项
find_package(ament_cmake REQUIRED)
find_package(example_interfaces REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rosbag2_cpp REQUIRED)
find_package(std_msgs REQUIRED)


add_executable(simple_bag_recorder src/simple_bag_recorder.cpp)
# 添加可执行文件 simple_bag_recorder，其源文件为 src/simple_bag_recorder.cpp


ament_target_dependencies(simple_bag_recorder rclcpp rosbag2_cpp std_msgs)
# 指定 simple_bag_recorder 的依赖项为 rclcpp、rosbag2_cpp 和 std_msgs


install(TARGETS
  simple_bag_recorder
  DESTINATION lib/${PROJECT_NAME}
)
# 安装目标 simple_bag_recorder 到 lib/${PROJECT_NAME} 目录


if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # 如果启用了测试构建，查找 ament_lint_auto 包
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # 以下行跳过版权检查器，添加版权和许可证后可注释掉该行
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  # 以下行跳过 cpplint（仅在 git 仓库中有效），添加版权和许可证后可注释掉该行
  ament_lint_auto_find_test_dependencies()
  # 自动查找测试依赖项
endif()


ament_package()
# 标记此项目为一个 ament 包

3. 构建并运行

导航回到工作区的根目录， ros2_ws ，并构建您的新包。

colcon build --packages-select bag_recorder_nodes

打开一个新的终端，导航到 ros2_ws ，并加载设置文件。

source install/setup.bash

现在运行节点：

ros2 run bag_recorder_nodes simple_bag_recorder

打开第二个终端并运行 talker 示例节点。

ros2 run demo_nodes_cpp talker

这将开始在 chatter 主题上发布数据。当 bag-writing 节点接收到这些数据时，它将写入 my_bag bag。

终止两个节点。然后，在一个终端启动 listener 示例节点。

ros2 run demo_nodes_cpp listener

在另一个终端中，使用 ros2 bag 播放由您的节点记录的包。

ros2 bag play my_bag

您将看到来自 bag 的消息被 listener 节点接收。

如果您希望再次运行 bag-writing 节点，您首先需要删除 my_bag 目录。

4 从节点记录合成数据

任何数据都可以记录到一个包中，而不仅仅是通过主题接收到的数据。将数据写入包中的一个常见用例是生成和存储合成数据。在本节中，您将学习如何编写一个生成一些数据并将其存储在包中的节点。我们将演示两种方法来实现这一点。第一种方法使用带有计时器的节点；如果您的数据生成是节点外部的，例如直接从硬件（例如相机）读取数据，这是您将使用的方法。第二种方法不使用节点；当您不需要使用 ROS 基础设施的任何功能时，可以使用这种方法。

4.1 编写一个 C++ 节点

在 ros2_ws/src/bag_recorder_nodes/src 目录中，创建一个名为 data_generator_node.cpp 的新文件，并将以下代码粘贴到其中。

#include <chrono> 
// 包含时间相关的库


#include <example_interfaces/msg/int32.hpp> 
// 包含 example_interfaces 中的 Int32 消息类型


#include <rclcpp/rclcpp.hpp> 
// 包含 ROS 2 的 C++ 客户端库


#include <rosbag2_cpp/writer.hpp> 
// 包含 rosbag2_cpp 的 Writer 类


using namespace std::chrono_literals; 
// 使用 chrono 的字面量


class DataGenerator : public rclcpp::Node 
// 定义一个继承自 rclcpp::Node 的类，名为 DataGenerator
{
public:
  DataGenerator() 
  // 类的构造函数
  : Node("data_generator") 
  // 调用父类构造函数，初始化节点名称为 "data_generator"
  {
    data_.data = 0; 
    // 初始化数据为 0
    writer_ = std::make_unique<rosbag2_cpp::Writer>(); 
    // 创建一个 rosbag2_cpp::Writer 对象


    writer_->open("timed_synthetic_bag"); 
    // 打开一个名为 "timed_synthetic_bag" 的 rosbag 文件


    writer_->create_topic(
    {
      0u, // 主题 ID
      "synthetic", // 主题名称
      "example_interfaces/msg/Int32", // 消息类型
      rmw_get_serialization_format(), // 序列化格式
      {}, // 兼容性选项
      "", // 主题描述
    });
    // 创建一个名为 "synthetic" 的话题，类型为 example_interfaces/msg/Int32


    auto timer_callback_lambda = [this](){return this->timer_callback();}; 
    // 定义一个 lambda 回调函数，当定时器触发时调用
    timer_ = create_wall_timer(1s, timer_callback_lambda); 
    // 创建一个定时器，每秒触发一次
  }


private:
  void timer_callback() 
  // 定时器回调函数
{
    writer_->write(data_, "synthetic", now()); 
    // 将数据写入 rosbag 文件


    ++data_.data; 
    // 数据递增
  }


  rclcpp::TimerBase::SharedPtr timer_; 
  // 定义一个定时器指针
  std::unique_ptr<rosbag2_cpp::Writer> writer_; 
  // 定义一个 rosbag2_cpp::Writer 的智能指针
  example_interfaces::msg::Int32 data_; 
  // 定义一个 example_interfaces::msg::Int32 类型的数据
};


int main(int argc, char * argv[]) 
// 主函数
{
  rclcpp::init(argc, argv); 
  // 初始化 ROS 2
  rclcpp::spin(std::make_shared<DataGenerator>()); 
  // 创建 DataGenerator 节点并进入事件循环
  rclcpp::shutdown(); 
  // 关闭 ROS 2
  return 0; 
  // 返回 0 表示程序正常结束
}

4.2 检查代码

这段代码的大部分与第一个示例相同。这里描述了重要的区别。

首先，包的名称被更改。

writer_->open("timed_synthetic_bag");

在此示例中，我们提前将主题注册到包中。这在大多数情况下是可选的，但在传递没有主题信息的序列化消息时必须这样做。

writer_->create_topic(
{
  0u,//无符号整数（unsigned integer），用于标识主题的唯一 ID
  "synthetic",//消息将发布到名为 "synthetic" 的主题上
  "example_interfaces/msg/Int32",//消息的类型
  rmw_get_serialization_format(),//获取当前使用的序列化格式。
  {},//空的兼容性选项列表，表示没有特殊的兼容性要求
  "",//主题的描述，当前为空字符串。
});

与其订阅一个主题，这个节点有一个计时器。计时器以一秒的周期触发，并在触发时调用给定的成员函数。

auto timer_callback_lambda = [this](){return this->timer_callback();};
timer_ = create_wall_timer(1s, timer_callback_lambda);

在定时器回调中，我们生成（或以其他方式获取，例如从连接到某些硬件的串行端口读取）我们希望存储在包中的数据。这个示例与前一个示例的重要区别在于数据尚未序列化。相反，我们将 ROS 消息数据类型传递给 writer 对象，在这种情况下是 example_interfaces/msg/Int32 的实例。writer 将在将数据写入包之前为我们序列化数据。

writer_->write(data_, "synthetic", now());

 4.3 添加可执行文件

打开 CMakeLists.txt 文件，并在先前添加的行之后（特别是在 install(TARGETS ...) 宏调用之后）添加以下几行。

add_executable(data_generator_node src/data_generator_node.cpp)
ament_target_dependencies(data_generator_node rclcpp rosbag2_cpp example_interfaces)


install(TARGETS
  data_generator_node
  DESTINATION lib/${PROJECT_NAME}
)

4.4 构建和运行 

导航回到工作区的根目录， ros2_ws ，并构建你的包。

colcon build --packages-select bag_recorder_nodes

打开一个新的终端，导航到 ros2_ws ，并加载设置文件。

source install/setup.bash

如果 timed_synthetic_bag 目录已经存在，则必须先删除它，然后再运行节点。

现在运行节点：

ros2 run bag_recorder_nodes data_generator_node

等待大约 30 秒，然后使用 ctrl-c 终止节点。接下来，播放创建的包。

ros2 bag play timed_synthetic_bag

打开第二个终端并回显 /synthetic 主题。

ros2 topic echo /synthetic

您将看到生成并存储在包中的数据以每秒一条消息的速度打印到控制台。

5. 从可执行文件记录合成数据 

现在您可以创建一个存储来自非主题源数据的包，您将学习如何从非节点可执行文件生成和记录合成数据。这种方法的优点是代码更简单，并且可以快速创建大量数据。

5.1 编写一个 C++ 可执行文件

在 ros2_ws/src/bag_recorder_nodes/src 目录中，创建一个名为 data_generator_executable.cpp 的新文件，并将以下代码粘贴到其中。

#include <chrono> 
// 包含时间相关的库


#include <rclcpp/rclcpp.hpp>  // For rclcpp::Clock, rclcpp::Duration and rclcpp::Time
// 包含 ROS 2 的 C++ 客户端库，提供 rclcpp::Clock、rclcpp::Duration 和 rclcpp::Time


#include <example_interfaces/msg/int32.hpp> 
// 包含 example_interfaces 中的 Int32 消息类型


#include <rosbag2_cpp/writer.hpp> 
// 包含 rosbag2_cpp 的 Writer 类
#include <rosbag2_cpp/writers/sequential_writer.hpp> 
// 包含 rosbag2_cpp 的 SequentialWriter 类
#include <rosbag2_storage/serialized_bag_message.hpp> 
// 包含 rosbag2_storage 中的 SerializedBagMessage 类


using namespace std::chrono_literals; 
// 使用 chrono 的字面量


int main(int, char**) 
// 主函数
{
  example_interfaces::msg::Int32 data; 
  // 定义一个 example_interfaces::msg::Int32 类型的数据
  data.data = 0; 
  // 初始化数据为 0
  std::unique_ptr<rosbag2_cpp::Writer> writer_ = std::make_unique<rosbag2_cpp::Writer>(); 
  // 创建一个 rosbag2_cpp::Writer 对象


  writer_->open("big_synthetic_bag"); 
  // 打开一个名为 "big_synthetic_bag" 的 rosbag 文件


  writer_->create_topic(
  {
    0u,
    "synthetic",
    "example_interfaces/msg/Int32",
    rmw_get_serialization_format(),
    {},
    "",
  }); 
  // 创建一个名为 "synthetic" 的话题，类型为 example_interfaces/msg/Int32


  rclcpp::Clock clock; 
  // 创建一个 rclcpp::Clock 对象
  rclcpp::Time time_stamp = clock.now(); 
  // 获取当前时间戳
  for (int32_t ii = 0; ii < 100; ++ii) { 
    // 循环 100 次
    writer_->write(data, "synthetic", time_stamp); 
    // 将数据写入 rosbag 文件


    ++data.data; 
    // 数据递增
    time_stamp += rclcpp::Duration(1s); 
    // 时间戳增加 1 秒
  }


  return 0; 
  // 返回 0 表示程序正常结束
}

5.2 检查代码

对比这个样本和之前的样本会发现它们并没有太大区别。唯一显著的区别是使用了 for 循环来驱动数据生成，而不是计时器。

请注意，我们现在生成数据的时间戳，而不是依赖当前系统时间来获取每个样本的时间。时间戳可以是您需要的任何值。数据将按照这些时间戳给出的速率回放，因此这是控制样本默认回放速度的有用方法。还请注意，虽然每个样本之间的间隔是整整一秒钟，但这个可执行文件不需要在每个样本之间等待一秒钟。这使我们能够在比回放所需时间短得多的时间内生成覆盖较长时间跨度的大量数据。

rclcpp::Clock clock;
rclcpp::Time time_stamp = clock.now();
for (int32_t ii = 0; ii < 100; ++ii) {
  writer_->write(data, "synthetic", time_stamp);
  ++data.data;
  time_stamp += rclcpp::Duration(1s);
}

 5.3 添加可执行文件

打开 CMakeLists.txt 文件，并在先前添加的行之后添加以下几行。

add_executable(data_generator_executable src/data_generator_executable.cpp)
ament_target_dependencies(data_generator_executable rclcpp rosbag2_cpp example_interfaces)


install(TARGETS
  data_generator_executable
  DESTINATION lib/${PROJECT_NAME}
)

5.4 构建和运行 

导航回到工作区的根目录， ros2_ws ，并构建你的包。

colcon build --packages-select bag_recorder_nodes

打开终端，导航到 ros2_ws ，并加载设置文件。

source install/setup.bash

如果 big_synthetic_bag 目录已经存在，则必须先删除它，然后再运行可执行文件。

现在运行可执行文件：

ros2 run bag_recorder_nodes data_generator_executable

请注意，可执行文件运行并完成得非常快。

现在播放创建的包。

ros2 bag play big_synthetic_bag

打开第二个终端并回显 /synthetic 主题。

ros2 topic echo /synthetic

您将看到生成并存储在包中的数据以每秒一条消息的速度打印到控制台。尽管包生成得很快，但仍然按照时间戳指示的速度回放。

 摘要

您创建了一个节点，将其接收到的主题数据记录到一个包中。您使用该节点测试了记录包，并通过回放包验证了数据已被记录。然后，您继续创建一个节点和一个可执行文件，以生成合成数据并将其存储在包中。