从包文件中读取（C++）

目标：不使用 CLI 从包中读取数据。

教程级别：高级

时间：10 分钟

背景 

rosbag2 不仅提供 ros2 bag 命令行工具。它还提供了一个 C++ API，用于从您自己的源代码读取和写入包。这允许您从包中读取内容，而无需播放包，这有时很有用。

先决条件 

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

如果您需要安装 ROS 2，请参阅安装说明。

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

任务 

1 创建包 

打开一个新终端并 source your ROS 2 installation，以便 ros2 命令可以正常工作。

在新的或现有的 workspace 中，导航到 src 目录并创建

一个新包：

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

您的终端将返回一条消息，验证您的包“bag_reading_cpp”及其所有必要的文件和文件夹的创建。 “–dependencies”参数将自动将必要的依赖项行添加到“package.xml”和“CMakeLists.txt”中。在这种情况下，包将使用“rosbag2_transport”包以及“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_transport/reader_writer_factory.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,
          [this](){return this->timer_callback();}
      );

      rosbag2_storage::StorageOptions storage_options;
      storage_options.uri = bag_filename;
      reader_ = rosbag2_transport::ReaderWriterFactory::make_reader(storage_options);
      reader_->open(storage_options);
    }

  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_;
    std::unique_ptr<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_transport 包的标头，这些标头用于处理包文件所需的函数和结构。

下一行创建将从包文件中读取并回放数据的节点。

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,
      [this](){return this->timer_callback();}
    );

我们还在构造函数中打开了包。 ``rosbag2_transport::ReaderWriterFactory``是一个可以根据存储选项构造压缩或未压缩的读取器或写入器的类。

rosbag2_storage::StorageOptions storage_options;
storage_options.uri = bag_filename;
reader_ = rosbag2_transport::ReaderWriterFactory::make_reader(storage_options);
reader_->open(storage_options);

现在，在我们的计时器回调中，我们循环遍历包中的消息，直到我们读取从所需主题记录的消息。请注意，序列化消息除了主题名称外，还具有时间戳元数据。

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_;
  std::unique_ptr<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.2 添加可执行文件

现在打开 CMakeLists.txt 文件。

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

add_executable(simple_bag_reader src/simple_bag_reader.cpp)
ament_target_dependencies(simple_bag_reader rclcpp rosbag2_transport 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 install/setup.bash

source install/setup.bash

call install/setup.bat

现在，运行脚本。确保将“/path/to/subset”替换为您的“subset”包的路径。

ros2 run bag_reading_cpp simple_bag_reader /path/to/subset

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

摘要 

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