从节点记录包（C++）

最新推荐文章于 2024-07-16 21:41:12 发布

莫回首�

最新推荐文章于 2024-07-16 21:41:12 发布

阅读量1k

点赞数 14

分类专栏： ros 文章标签： c++ 开发语言机器人自动驾驶

本文链接：https://blog.csdn.net/weixin_43841155/article/details/135596023

版权

ros 专栏收录该内容

8 篇文章 0 订阅

订阅专栏

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

背景

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

先决条件

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

如果您从 Linux 上的 Debian 软件包安装，则默认情况下可能会安装它。如果不是，您可以使用此命令安装它。

sudo apt install ros-galactic-rosbag

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

任务

1 创建软件包

打开一个新终端并获取 ROS 2 安装的来源，以便命令可以工作。ros2

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

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

您的终端将返回一条消息，验证您的软件包及其所有必要文件和文件夹的创建。该参数将自动向和添加必要的依赖行。在这种情况下，包将同时使用包和包。在本教程的后面部分，还需要对包的依赖关系。bag_recorder_nodes--dependenciespackage.xmlCMakeLists.txtrosbag2_cpprclcppexample_interfaces

1.1 更新`package.xml`

由于您在包创建期间使用了该选项，因此您不必手动将依赖项添加到或。但是，与往常一样，请确保将描述、维护者电子邮件和名称以及许可证信息添加到。--dependenciespackage.xmlCMakeLists.txtpackage.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/srcsimple_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<rclcpp::SerializedMessage>::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 检查代码

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

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

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

现在我们有了一个 writer 对象，我们可以使用它打开袋子了。我们只指定要创建的包的 URI，将其他选项保留为默认值。使用默认存储选项，这意味着将创建一个 -format 包。也使用默认转换选项，这些选项将不执行转换，而是以接收消息的序列化格式存储消息。sqlite3

writer_->open("my_bag");

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

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

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

消息数据在写入包之前需要序列化，因此，我们要求 ROS 按原样向我们提供序列化的消息，而不是在接收数据时取消序列化然后重新序列化。rosbag2
编写器 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);

该类包含两个成员变量。

订阅对象。请注意，template 参数是回调的类型，而不是主题的类型。在这种情况下，回调接收共享指针，因此这是模板参数必须包含的内容。rclcpp::SerializedMessage
指向用于写入包的编写器对象的托管指针。请注意，此处使用的编写器类型是，泛型编写器接口。其他作家可能有不同的行为。rosbag2_cpp::Writer

rclcpp::Subscription<rclcpp::SerializedMessage>::SharedPtr subscription_;
std::unique_ptr<rosbag2_cpp::Writer> writer_;

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

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_STANDARD1417

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

在包含的 dependencies 块下方，添加以下代码行。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

Linux操作系统macOS操作系统窗户

colcon build --packages-select bag_recorder_nodes

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

Linux操作系统macOS操作系统窗户

source install/setup.bash

现在运行节点：

ros2 run bag_recorder_nodes simple_bag_recorder

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

ros2 run demo_nodes_cpp talker

这将开始发布有关该主题的数据。当 bag-writing 节点接收到此数据时，它会将其写入 bag。chattermy_bag

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

ros2 run demo_nodes_cpp listener

在另一个终端中，用于播放您的节点录制的袋子。ros2 bag

ros2 bag play my_bag

您将看到节点正在接收的包中的消息。listener

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

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

4.1 编写 C++ 节点

在目录中，创建一个名为并将以下代码粘贴到其中的新文件。ros2_ws/src/bag_recorder_nodes/srcdata_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 消息数据类型传递给 writer 对象，在本例中为 . 在将数据写入袋子之前，编写器会为我们序列化数据。example_interfaces/msg/Int32

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

4.3 添加可执行文件

打开文件，并在之前添加的行之后添加以下行（具体而言，在宏调用之后）。CMakeLists.txtinstall(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

Linux操作系统macOS操作系统窗户

colcon build --packages-select bag_recorder_nodes

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

Linux操作系统macOS操作系统窗户

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/srcdata_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

Linux操作系统macOS操作系统窗户

colcon build --packages-select bag_recorder_nodes

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

Linux操作系统macOS操作系统窗户

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

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

莫回首�

关注

14
点赞
踩
12

收藏

觉得还不错? 一键收藏
打赏
0
评论
从节点记录包（C++）

在计时器回调中，我们生成（或以其他方式获取，例如从连接到某些硬件的串行端口读取）我们希望存储在包中的数据。取而代之的是，我们将 ROS 消息数据类型传递给 writer 对象，在本例中为 . 在将数据写入袋子之前，编写器会为我们序列化数据。在订阅回调中，首先要做的是确定用于存储消息的时间戳。这可以是适合您的数据的任何值，但两个常见值是生成数据的时间（如果已知）和接收数据的时间。消息数据在写入包之前需要序列化，因此，我们要求 ROS 按原样向我们提供序列化的消息，而不是在接收数据时取消序列化然后重新序列化。
复制链接

扫一扫