从节点记录包 (C++)

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

教程级别：高级

时间：20 分钟

背景

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

先决条件

您应该将 rosbag2 包作为常规 ROS 2 设置的一部分进行安装。

如果您已从 Linux 上的 deb 包安装，则它可能已默认安装。 如果不是，您可以使用此命令进行安装。

sudo apt install ros-rolling-rosbag2

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

任务

1 创建一个包

打开一个新终端并 source your ROS 2 installation，以便 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="you@email.com">Your Name</maintainer>
<license>Apache License 2.0</license>

2 编写 C++ 节点

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

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>

#include <rosbag2_cpp/writer.hpp>

using std::placeholders::_1;

class SimpleBagRecorder : public rclcpp::Node
{
public:
  SimpleBagRecorder()
  : Node("simple_bag_recorder")
  {
    writer_ = std::make_unique<rosbag2_cpp::Writer>();

    writer_->open("my_bag");

    subscription_ = create_subscription<std_msgs::msg::String>(
      "chatter", 10, std::bind(&SimpleBagRecorder::topic_callback, this, _1));
  }

private:
  void topic_callback(std::shared_ptr<rclcpp::SerializedMessage> msg) const
  {
    rclcpp::Time time_stamp = this->now();

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

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

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

2.1 检查代码

顶部的 #include 语句是包依赖项。

请注意，包含来自 rosbag2_cpp 包的标头，其中包含处理包文件所需的函数和结构。

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

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

现在我们有了一个写入器对象，我们可以使用它来打开包。 我们只指定要创建的包的 URI，其他选项保留默认设置。 使用默认存储选项，这意味着将创建一个 sqlite3 格式的包。 也使用默认转换选项，它不会执行任何转换，而是以接收消息的序列化格式存储消息。

writer_->open("my_bag");

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

subscription_ = create_subscription<std_msgs::msg::String>(
  "chatter", 10, std::bind(&SimpleBagRecorder::topic_callback, this, _1));

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

1. 消息数据在写入包之前需要由“rosbag2”序列化，因此，我们要求 ROS 直接向我们提供序列化的消息，而不是在接收数据时对其进行反序列化，然后重新进行序列化。

2. 写入器 API 可以接受序列化的消息。

void topic_callback(std::shared_ptr<rclcpp::SerializedMessage> msg) const
{

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

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

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

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

该类包含两个成员变量。

1. 订阅对象。 请注意，模板参数是回调的类型，而不是主题的类型。 在这种情况下，回调接收 rclcpp::SerializedMessage 共享指针，因此模板参数必须是这样的。 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}
)

3 构建并运行

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

LinuxmacOSWindows

colcon build --packages-select bag_recorder_nodes

打开一个新终端，导航到“ros2_ws”，并获取安装文件。

LinuxmacOSWindows

source install/setup.bash

现在运行节点：

ros2 run bag_recorder_nodes simple_bag_recorder

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

ros2 run demo_nodes_cpp talker

这将开始在“chatter”主题上发布数据。 当写入包节点收到此数据时，它会将其写入“my_bag”包。

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

ros2 run demo_nodes_cpp listener

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

ros2 bag play my_bag

您将看到“listener”节点接收到来自包的消息。

如果您希望再次运行包写入节点，则首先需要删除“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>
#include <rclcpp/rclcpp.hpp>

#include <rosbag2_cpp/writer.hpp>

using namespace std::chrono_literals;

class DataGenerator : public rclcpp::Node
{
public:
  DataGenerator()
  : Node("data_generator")
  {
    data_.data = 0;
    writer_ = std::make_unique<rosbag2_cpp::Writer>();

    writer_->open("timed_synthetic_bag");

    writer_->create_topic(
      {"synthetic",
       "example_interfaces/msg/Int32",
       rmw_get_serialization_format(),
       ""});

    timer_ = create_wall_timer(1s, std::bind(&DataGenerator::timer_callback, this));
  }

private:
  void timer_callback()
  {
    writer_->write(data_, "synthetic", now());

    ++data_.data;
  }

  rclcpp::TimerBase::SharedPtr timer_;
  std::unique_ptr<rosbag2_cpp::Writer> writer_;
  example_interfaces::msg::Int32 data_;
};

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

4.2 检查代码

此代码的大部分内容与第一个示例相同。 此处描述了重要的区别。

首先，包的名称已更改。

writer_->open(“timed_synthetic_bag”);

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

writer_->create_topic(
  {"synthetic",
   "example_interfaces/msg/Int32",
   rmw_get_serialization_format(),
   ""});

此节点没有订阅主题，而是有一个计时器。 计时器以一秒为周期触发，并在触发时调用给定的成员函数。

timer_ = create_wall_timer(1s, std::bind(&DataGenerator::timer_callback, this));

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

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”，然后构建包。

LinuxmacOSWindows

colcon build --packages-select bag_recorder_nodes

打开一个新终端，导航到“ros2_ws”，并获取安装文件。

LinuxmacOSWindows

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
#include <example_interfaces/msg/int32.hpp>

#include <rosbag2_cpp/writer.hpp>
#include <rosbag2_cpp/writers/sequential_writer.hpp>
#include <rosbag2_storage/serialized_bag_message.hpp>

using namespace std::chrono_literals;

int main(int, char**)
{
  example_interfaces::msg::Int32 data;
  data.data = 0;
  std::unique_ptr<rosbag2_cpp::Writer> writer_ = std::make_unique<rosbag2_cpp::Writer>();

  writer_->open("big_synthetic_bag");

  writer_->create_topic(
    {"synthetic",
     "example_interfaces/msg/Int32",
     rmw_get_serialization_format(),
     ""});

  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);
  }

  return 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”，然后构建包。

LinuxmacOSWindows

colcon build --packages-select bag_recorder_nodes

打开终端，导航到“ros2_ws”，并获取安装文件。

LinuxmacOSWindows

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

您将看到生成并存储在包中的数据以每秒一条消息的速率打印到控制台。 即使包生成得很快，它仍然以时间戳指示的速率播放。

摘要

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