从前慢现在也慢

这个屌丝很懒，什么也没留下！

热门标签

【ROS2】高级:从包文件读取 (C++)

作者：从前慢现在也慢 | 2024-07-21 23:05:33

踩

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

教程级别：高级

时间：10 分钟

背景
先决条件
任务
- 1 创建一个包裹
- 2 编写 C++ 读取器
- 3 构建并运行
摘要

背景

rosbag2 不仅提供 ros2 bag 命令行工具。它还提供了一个 C++ API，用于从您自己的源代码中读取和写入包。这使您可以在不播放包的情况下读取包中的内容，这有时会很有用。

先决条件

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

如果您需要安装 ROS 2，请查看安装说明。

你应该已经完成了基本的 ROS 2 包教程https://docs.ros.org/en/jazzy/Tutorials/Beginner-CLI-Tools/Recording-And-Playing-Back-Data/Recording-And-Playing-Back-Data.html ，我们将使用你在那里创建的 subset 包。

任务

1 创建一个包裹

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

在新的或现有的工作区中，导航到 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="cxy@126.com">cxy</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"  // ROS 2 的 C++ 客户端库
#include "rclcpp/serialization.hpp"  // ROS 2 的序列化库
#include "rosbag2_transport/reader_writer_factory.hpp"  // 用于读写 rosbag2 的工厂
#include "turtlesim/msg/pose.hpp"  // turtlesim 包中的 Pose 消息类型
 
 
using namespace std::chrono_literals;  // 使用 chrono 的字面量
 
 
class PlaybackNode : public rclcpp::Node  // 定义 PlaybackNode 类，继承自 rclcpp::Node
{
  public:
    PlaybackNode(const std::string & bag_filename)  // 构造函数，接受一个 bag 文件名作为参数
    : Node("playback_node")  // 调用基类构造函数，设置节点名称为 "playback_node"
    {
      publisher_ = this->create_publisher<turtlesim::msg::Pose>("/turtle1/pose", 10);  // 创建一个发布者，发布到 "/turtle1/pose" 话题，队列大小为 10
 
 
      timer_ = this->create_wall_timer(100ms,  // 创建一个定时器，每 100 毫秒触发一次
          [this](){return this->timer_callback();}  // 定时器的回调函数
      );
 
 
      rosbag2_storage::StorageOptions storage_options;  // 创建存储选项实例
      storage_options.uri = bag_filename;  // 设置存储选项的 URI 为 bag 文件名
      reader_ = rosbag2_transport::ReaderWriterFactory::make_reader(storage_options);  // 通过工厂创建一个 rosbag2 读取器
      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") {  // 如果消息的主题不是 "/turtle1/pose"
          continue;  // 跳过本次循环
        }
 
 
        rclcpp::SerializedMessage serialized_msg(*msg->serialized_data);  // 创建一个序列化消息
        turtlesim::msg::Pose::SharedPtr ros_msg = std::make_shared<turtlesim::msg::Pose>();  // 创建一个 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) {  // 如果参数数量不等于 2
    std::cerr << "Usage: " << argv[0] << " <bag>" << std::endl;  // 输出用法提示
    return 1;  // 返回错误码 1
  }
 
 
  rclcpp::init(argc, argv);  // 初始化 ROS 2
  rclcpp::spin(std::make_shared<PlaybackNode>(argv[1]));  // 创建 PlaybackNode 实例并进入事件循环
  rclcpp::shutdown();  // 关闭 ROS 2
 
 
  return 0;  // 返回 0
}

2.1 检查代码

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

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

class PlaybackNode : public rclcpp::Node

现在，我们可以创建一个以 10 赫兹运行的定时器回调。我们的目标是在每次运行回调时向 /turtle1/pose 主题重放一条消息。请注意，构造函数将路径作为参数传递给包文件。


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

我们还在构造函数中打开bag。 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

接下来，获取安装文件

source install/setup.bash

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

ros2 run bag_reading_cpp simple_bag_reader ~/ros2_ws/subset

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

摘要

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

附录：

记录的包内容: ~/ros2_ws/subset/metadata.yaml


rosbag2_bagfile_information:
  version: 9
  storage_identifier: mcap
  duration:
    nanoseconds: 8911648921
  starting_time:
    nanoseconds_since_epoch: 1721184333150957762
  message_count: 640
  topics_with_message_count:
    - topic_metadata:
        name: /turtle1/cmd_vel
        type: geometry_msgs/msg/Twist
        serialization_format: cdr
        offered_qos_profiles:
          - history: unknown
            depth: 0
            reliability: reliable
            durability: volatile
            deadline:
              sec: 9223372036
              nsec: 854775807
            lifespan:
              sec: 9223372036
              nsec: 854775807
            liveliness: automatic
            liveliness_lease_duration:
              sec: 9223372036
              nsec: 854775807
            avoid_ros_namespace_conventions: false
        type_description_hash: RIHS01_9c45bf16fe0983d80e3cfe750d6835843d265a9a6c46bd2e609fcddde6fb8d2a
      message_count: 82
    - topic_metadata:
        name: /turtle1/pose
        type: turtlesim/msg/Pose
        serialization_format: cdr
        offered_qos_profiles:
          - history: unknown
            depth: 0
            reliability: reliable
            durability: volatile
            deadline:
              sec: 9223372036
              nsec: 854775807
            lifespan:
              sec: 9223372036
              nsec: 854775807
            liveliness: automatic
            liveliness_lease_duration:
              sec: 9223372036
              nsec: 854775807
            avoid_ros_namespace_conventions: false
        type_description_hash: RIHS01_739beba26bcba6920404ba722b7b8321348512f92ea5be235c47251940dd8aa9
      message_count: 558
  compression_format: ""
  compression_mode: ""
  relative_file_paths:
    - subset_0.mcap
  files:
    - path: subset_0.mcap
      starting_time:
        nanoseconds_since_epoch: 1721184333150957762
      duration:
        nanoseconds: 8911648921
      message_count: 640
  custom_data: ~
  ros_distro: jazzy

记录多个主题：

ros2 bag record -o subset /turtle1/cmd_vel /turtle1/pose

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/从前慢现在也慢/article/detail/862511

【ROS2】高级:从包文件读取 (C++)

背景

先决条件

任务

1 创建一个包裹

1.1 更新 package.xml

2 编写 C++ 读取器

2.1 检查代码

2.2 添加可执行文件

3. 构建并运行

摘要

1.1 更新 `package.xml`