从 bag 文件中读取数据（C++）

目标： 在不使用 CLI 的情况下从 bag 中读取数据。

教程级别： 高级

时间: 10分钟

内容

背景
先决条件
任务
总结

背景 

rosbag2 不仅提供了 ros2 bag 命令行工具，还提供了用于从您自己的源代码读取和写入 bag 的 C++ API。这允许您读取 bag 中的内容，而无需播放 bag，这在某些情况下可能非常有用。

先决条件 

你应该在常规的 ROS 2 配置中安装了 rosbag2 包。

如果你是通过在 Linux 上安装 Debian 包进行安装的，它可能是默认安装的。如果没有安装，可以使用以下命令安装：

sudo apt install ros-humble-rosbag2

本教程讨论了如何使用 ROS 2 bags。您应该已经完成了基本的 ROS 2 bag 教程，我们将使用您在那里创建的 subset bag。

任务 

1 创建一个包 

在一个新的终端中源化你的 ROS 2 安装，这样 ros2 命令才能正常工作。

在新的或现有的工作空间中，导航到 src 目录并创建一个新包：

ros2 pkg create --build-type ament_cmake --license Apache-2.0 bag_reading_cpp --dependencies rclcpp rosbag2_cpp turtlesim

您的终端将返回一条消息，验证包 bag_reading_cpp 及其所有必要的文件和文件夹的创建。--dependencies 参数将自动将必要的依赖行添加到 package.xml 和 CMakeLists.txt 中。在本例中，包将使用 rosbag2_cpp 包以及 rclcpp 包。还需要依赖于 turtlesim 包，以便使用自定义 turtlesim 消息。

1.1 更新 `package.xml`

由于您在软件包创建过程中使用了 --dependencies 选项，因此您无需手动向 package.xml 或 CMakeLists.txt 添加依赖项。但请确保向 package.xml 添加描述、维护者电子邮件和姓名以及许可信息。

<description>C++ bag reading tutorial</description>
<maintainer email="you@email.com">Your Name</maintainer>
<license>Apache-2.0</license>

2 编写 C++ 读取器 

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

#include <chrono>
#include <functional>
#include <iostream>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp/serialization.hpp"
#include "rosbag2_cpp/reader.hpp"
#include "turtlesim/msg/pose.hpp"

using namespace std::chrono_literals;

class PlaybackNode : public rclcpp::Node
{
  public:
    PlaybackNode(const std::string & bag_filename)
    : Node("playback_node")
    {
      publisher_ = this->create_publisher<turtlesim::msg::Pose>("/turtle1/pose", 10);
      timer_ = this->create_wall_timer(
          100ms, std::bind(&PlaybackNode::timer_callback, this));

      reader_.open(bag_filename);
    }

  private:
    void timer_callback()
    {
      while (reader_.has_next()) {
        rosbag2_storage::SerializedBagMessageSharedPtr msg = reader_.read_next();

        if (msg->topic_name != "/turtle1/pose") {
          continue;
        }

        rclcpp::SerializedMessage serialized_msg(*msg->serialized_data);
        turtlesim::msg::Pose::SharedPtr ros_msg = std::make_shared<turtlesim::msg::Pose>();

        serialization_.deserialize_message(&serialized_msg, ros_msg.get());

        publisher_->publish(*ros_msg);
        std::cout << '(' << ros_msg->x << ", " << ros_msg->y << ")\n";

        break;
      }
    }

    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<turtlesim::msg::Pose>::SharedPtr publisher_;

    rclcpp::Serialization<turtlesim::msg::Pose> serialization_;
    rosbag2_cpp::Reader reader_;
};

int main(int argc, char ** argv)
{
  if (argc != 2) {
    std::cerr << "Usage: " << argv[0] << " <bag>" << std::endl;
    return 1;
  }

  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<PlaybackNode>(argv[1]));
  rclcpp::shutdown();

  return 0;
}

2.1 检查代码

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

接下来的行创建了一个节点，该节点将从 bag 文件中读取数据并播放回来。

class PlaybackNode : public rclcpp::Node

现在，我们可以创建一个计时器回调，它将以 10 Hz 运行。我们的目标是在每次运行回调时向 /turtle1/pose 主题重放一条消息。请注意，构造函数接受 bag 文件的路径作为参数。

public:
  PlaybackNode(const std::string & bag_filename)
  : Node("playback_node")
  {
    publisher_ = this->create_publisher<turtlesim::msg::Pose>("/turtle1/pose", 10);
    timer_ = this->create_wall_timer(
        100ms, std::bind(&PlaybackNode::timer_callback, this));

我们还在构造函数中打开了 bag 文件。

reader_.open(bag_filename);

现在，在我们的计时器回调内部，我们循环遍历 bag 中的消息，直到读取到来自我们所需主题的消息为止。请注意，序列化消息除了主题名称之外还具有时间戳元数据。

void timer_callback()
{
  while (reader_.has_next()) {
    rosbag2_storage::SerializedBagMessageSharedPtr msg = reader_.read_next();

    if (msg->topic_name != "/turtle1/pose") {
      continue;
    }

然后，我们从刚刚读取的序列化数据构造了一个 rclcpp::SerializedMessage 对象。此外，我们需要创建一个 ROS 2 反序列化消息，它将保存我们反序列化的结果。然后，我们可以将这两个对象传递给 rclcpp::Serialization::deserialize_message 方法。

rclcpp::SerializedMessage serialized_msg(*msg->serialized_data);
turtlesim::msg::Pose::SharedPtr ros_msg = std::make_shared<turtlesim::msg::Pose>();

serialization_.deserialize_message(&serialized_msg, ros_msg.get());

最后，我们发布反序列化后的消息并将 xy 坐标打印到终端上。我们还跳出循环，以便在下一个计时器回调期间发布下一条消息。

  publisher_->publish(*ros_msg);
  std::cout << '(' << ros_msg->x << ", " << ros_msg->y << ")\n";

  break;
}

我们还必须声明在整个节点中使用的私有变量。

  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<turtlesim::msg::Pose>::SharedPtr publisher_;

  rclcpp::Serialization<turtlesim::msg::Pose> serialization_;
  rosbag2_cpp::Reader reader_;
};

最后，我们创建了主函数，该函数将检查用户是否传递了 bag 文件路径的参数，并启动我们的节点。

int main(int argc, char ** argv)
{
  if (argc != 2) {
    std::cerr << "Usage: " << argv[0] << " <bag>" << std::endl;
    return 1;
  }

  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<PlaybackNode>(argv[1]));
  rclcpp::shutdown();

  return 0;
}

2.2 添加可执行文件

现在打开 CMakeLists.txt 文件。

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

add_executable(simple_bag_reader src/simple_bag_reader.cpp)
ament_target_dependencies(simple_bag_reader rclcpp rosbag2_cpp turtlesim)

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

3. 构建和运行 

返回到工作空间的根目录并构建您的新包。

colcon build --packages-select bag_reading_cpp

colcon build --packages-select bag_reading_cpp

colcon build --merge-install --packages-select bag_reading_cpp

接下来，使用 source 命令执行设置文件。

source install/setup.bash

source install/setup.bash

call install/setup.bat

现在，运行脚本。确保将 /path/to/setup 替换为您的 setup bag 的路径。

ros2 run bag_reading_cpp simple_bag_reader /path/to/setup

您应该会在控制台上看到乌龟的 (x, y) 坐标被打印出来。

总结 

您创建了一个 C++ 可执行文件，用于从 bag 中读取数据。然后，您编译并运行了该可执行文件，它会将 bag 中的一些信息打印到控制台上。

从 bag 文件中读取数据（C++）

背景

先决条件

任务

1 创建一个包

1.1 更新 package.xml

2 编写 C++ 读取器