ROS2多线程C++的实现和单进程通讯零拷贝的节点设计模式

最新推荐文章于 2024-05-28 21:13:10 发布

zh路西法

最新推荐文章于 2024-05-28 21:13:10 发布

阅读量1k

点赞数 32

文章标签： c++ 设计模式经验分享

本文链接：https://blog.csdn.net/m0_73800387/article/details/138862061

版权

前景回顾

上一期文章我们使用了ROS2内的执行器(Executor)配合回调组(CallbackGroups)实现了ROS2python的多线程调用，本期将使用C++介绍ROS2一种全新的进程内通讯的代码设计模式，配合使用多线程实现

“一个文件两个节点”–全新的进程内通讯零拷贝(zero_copy)的代码设计模式

先上代码，我们在逐一分析

#include <chrono>
#include <cinttypes>
#include <cstdio>
#include <memory>
#include <string>
#include <utility>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"

using namespace std::chrono_literals;


struct Producer : public rclcpp::Node
{
Producer()
: Node("pub_node", rclcpp::NodeOptions().use_intra_process_comms(true))
{

    pub_ = this->create_publisher<std_msgs::msg::Int32>("/topic", 10);
    std::weak_ptr<std::remove_pointer<decltype(pub_.get())>::type> captured_pub = pub_;
    

    auto callback = [captured_pub]() -> void {
        auto pub_ptr = captured_pub.lock();
        if (!pub_ptr) {
        return;
        }
        static int32_t count = 0;
        // 创建零拷贝数据
        std_msgs::msg::Int32::UniquePtr msg(new std_msgs::msg::Int32());
        msg->data = count++;

        printf(
        "Published message with value: %d, and address: 0x%" PRIXPTR "\n", msg->data,
        reinterpret_cast<std::uintptr_t>(msg.get()));
        pub_ptr->publish(std::move(msg));
    };
    timer_ = this->create_wall_timer(1ms, callback);
}

rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr pub_;
rclcpp::TimerBase::SharedPtr timer_;
};


struct Consumer : public rclcpp::Node
{
Consumer()
: Node("sub_node", rclcpp::NodeOptions().use_intra_process_comms(true))
{
    
    sub_ = this->create_subscription<std_msgs::msg::Int32>(
    "/topic",
    10,
    [](std_msgs::msg::Int32::UniquePtr msg) {
        printf(
        " Received message with value: %d, and address: 0x%" PRIXPTR "\n", msg->data,
        reinterpret_cast<std::uintptr_t>(msg.get()));
    });
}

rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr sub_;
};

int main(int argc, char * argv[])
{
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
rclcpp::init(argc, argv);
rclcpp::executors::SingleThreadedExecutor executor;

auto producer = std::make_shared<Producer>();
auto consumer = std::make_shared<Consumer>();

executor.add_node(producer);
executor.add_node(consumer);
executor.spin();

rclcpp::shutdown();

return 0;
}

单进程ROS2多节点通讯,需要在节点初始化指定参数use_intra_process_comms为true
```
rclcpp::NodeOptions().use_intra_process_comms(true)
```
零拷贝(zero_copy):在同一进程内通过std::unique_ptr进行发布/订阅的消息可以不需要额外的复制开销，这可以大大提高代码的运行效率。因为std::unique_ptr运行你在系统转移数据时安全转移，因此不需要额外的拷贝，这通常在const&或者std::shared_ptr是行不通的
```
// 声明传递的数据类型为对应类型下的UniquePtr
std_msgs::msg::Int32::UniquePtr msg(new std_msgs::msg::Int32());
```
std::weak_ptr这是一个用于持有对std::shared_ptr所管理对象的非拥有性引用的智能指针。这种类型的指针用于防止产生循环引用，从而允许所引用的对象能够在没有其他共享指针的情况下被正确销毁。
- 下面我们来看这行代码
```
 	std::weak_ptr<std::remove_pointer<decltype(pub_.get())>::type> captured_pub = pub_;
```
  - pub_.get()获得了这个发布者对象的原始指针
  - decltype(pub_.get()) 则获取了这个原始指针的类型
  - std::remove_pointer<decltype(pub_.get())>::type 则获取了去除指针后的类型，即发布者对象本身的类型。
  - std::weak_ptr<std::remove_pointer<decltype(pub_.get())>::type> 是用于创建 std::weak_ptr 的类型，它将持有对发布者对象的非拥有性引用。这个 std::weak_ptr 被初始化为 pub_，这样就可以在 lambda 函数中安全地使用，而不会影响 pub_ 的生命周期。
- 我们进一步分析std::weak_ptr的使用
```
auto callback = [captured_pub]() -> void {
// 尝试从 weak_ptr 创建一个 shared_ptr
auto pub_ptr = captured_pub.lock(); 

if (!pub_ptr) {
// 如果 pub_ptr 为空，说明原始的shared_ptr不再存在，就说明发布者已经被释放，我们要退出
return;
}
// 如果 pub_ptr 不为空，shared_ptr 仍然存在，
// 我们可以使用发布者来发布消息。
std_msgs::msg::Int32::UniquePtr msg(new std_msgs::msg::Int32());
msg->data = ...; // 设置消息数据
pub_ptr->publish(std::move(msg));
};
```
  - 右值引用的调用
```
std::move(msg)
```
    - std::move(msg)是一个将 msg 变为右值引用的调用。这意味着msg中的资源将被移动，而不是复制。这是高效的，因为它避免了不必要的拷贝，并且msg在这次调用后不再有效。
    - 关于左右值引用和完美转发std::forward以及std::move后续会出一篇文章详细说明，这里把他当作static_cast来理解是可以的,std::move的左右将左值转换为右值，也是一种类型的转换
- 在 ROS2节点使用中，这种用法通常出现在需要在一个回调函数中使用发布者，但又不想在这个回调函数中延长发布者对象的生命周期的情况下。通过使用std::weak_ptr，我们可以确保如果节点在回调函数执行之前被销毁，发布者对象也能被正确地销毁。
上述代码为什么使用std::weak_ptr，是由于类的成员函数外部创建了一个回调，例如在类的构造函数中。(上述代码的lambda)，在正常类内创建回调函数的前提下，使用std::bind将节点和回调函数绑定是正确的做法
```
    // 正常绑定类内成员回调函数使用std::bind()
    timer_ = this->create_wall_timer(500ms, std::bind(&MinimalPublisher::timer_callback, this));
```

ROS2C++多线程代码实现

相信根据上文的说明和解读，你可以很轻松的看明白底下的代码

#include <chrono>
#include <cinttypes>
#include <cstdio>
#include <memory>
#include <string>
#include <utility>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"

using namespace std::chrono_literals;

struct Producer : public rclcpp::Node
{
  Producer()
  : Node("pub_node", rclcpp::NodeOptions().use_intra_process_comms(true))
  {
    // 创建一个可重入的回调组
    auto callback_group = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);

    // 创建发布者，并将其与回调组关联
    pub_ = this->create_publisher<std_msgs::msg::Int32>("/topic", 10);

    // 使用 std::bind 绑定定时器回调
    auto callback = std::bind(&Producer::timer_callback, this, std::placeholders::_1);
    timer_ = this->create_wall_timer(1ms, callback, callback_group);
  }

  void timer_callback(const rclcpp::TimerEvent&)
  {
    static int32_t count = 0;
    // 创建零拷贝数据
    std_msgs::msg::Int32::UniquePtr msg(new std_msgs::msg::Int32());
    msg->data = count++;

    printf(
      "Published message with value: %d, and address: 0x%" PRIXPTR "\n", msg->data,
      reinterpret_cast<std::uintptr_t>(msg.get()));
    pub_->publish(std::move(msg));
  }

  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr pub_;
  rclcpp::TimerBase::SharedPtr timer_;
};

struct Consumer : public rclcpp::Node
{
  Consumer()
  : Node("sub_node", rclcpp::NodeOptions().use_intra_process_comms(true))
  {
    // 创建一个可重入的回调组
    auto callback_group = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);

    // 创建订阅者，并将其与回调组关联
    sub_ = this->create_subscription<std_msgs::msg::Int32>(
      "/topic",
      10,
      std::bind(&Consumer::subscription_callback, this, std::placeholders::_1),
      callback_group);
  }

  void subscription_callback(const std_msgs::msg::Int32::UniquePtr msg)
  {
    printf(
      "Received message with value: %d, and address: 0x%" PRIXPTR "\n", msg->data,
      reinterpret_cast<std::uintptr_t>(msg.get()));
  }

  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr sub_;
};

int main(int argc, char * argv[])
{
  setvbuf(stdout, NULL, _IONBF, BUFSIZ);
  rclcpp::init(argc, argv);
  rclcpp::executors::MultiThreadedExecutor executor;

  auto producer = std::make_shared<Producer>();
  auto consumer = std::make_shared<Consumer>();

  executor.add_node(producer);
  executor.add_node(consumer);
  executor.spin();

  rclcpp::shutdown();

  return 0;
}

单进程多线程ROS2多节点管理

通过以上的代码，我们掌握了多线程以及单进程内节点通讯的方法，那么我们是否可以写出一个动态管理多线程节点的程序呢

我们再次回到rclcpp::executors::MultiThreadedExecutor executor;中，通过查阅API，我们有

// 定时调用回调，但阻塞程序
virtual void rclcpp::executor::Executor::spin();
//添加节点到执行器
virtual void rclcpp::executor::Executor::add_node(std::shared_ptr< rclcpp::Node > 	node_ptr,bool 	notify = true );
//添加删除到执行器
virtual void rclcpp::executor::Executor::remove_node(std::shared_ptr< rclcpp::Node > 	node_ptr,bool 	notify = true);	
//
rclcpp::callback_group::CallbackGroup::SharedPtr rclcpp::executor::Executor::get_group_by_timer	(	rclcpp::TimerBase::SharedPtr 	timer	)

代码如下，rclcpp::executors::MultiThreadedExecutor 类的 add_node 和 remove_node 方法。这些方法允许我们在运行时添加或删除节点。

#include <chrono>
#include <cinttypes>
#include <cstdio>
#include <memory>
#include <string>
#include <utility>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"

using namespace std::chrono_literals;

struct Producer : public rclcpp::Node
{
  Producer()
  : Node("pub_node", rclcpp::NodeOptions().use_intra_process_comms(true))
  {
    // 创建发布者
    pub_ = this->create_publisher<std_msgs::msg::Int32>("/topic", 10);
  }
  
  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr pub_;
};

struct Consumer : public rclcpp::Node
{
  Consumer()
  : Node("sub_node", rclcpp::NodeOptions().use_intra_process_comms(true))
  {
    // 创建订阅者
    sub_ = this->create_subscription<std_msgs::msg::Int32>(
      "/topic",
      10,
      [](const std_msgs::msg::Int32::SharedPtr msg) {
        printf("Received message with value: %d\n", msg->data);
      });
  }

  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr sub_;
};

class NodeManager
{
public:
  NodeManager()
  : executor_(std::make_shared<rclcpp::executors::MultiThreadedExecutor>())
  {

    auto callback = std::bind(&Producer::timer_callback, this, std::placeholders::_1);
    timer_ = this->create_wall_timer(1ms, callback, callback_group);
  }

  ~NodeManager()
  {
    
    
  }

  void spin()
  {
    // 开始执行
    executor_->spin();
  }

private:
   
  

  void timer_callback(const rclcpp::TimerEvent&)
  {
    if(条件1)
    {
        producer_ = std::make_shared<Producer>();
        executor_->add_node(producer_);
    }else if (条件2)
    {
        consumer_ = std::make_shared<Consumer>();
        executor_->add_node(consumer_);

    }else if (条件3)
    {
       executor_->remove_node(producer_);
    }else if (条件4)
    {

       executor_->remove_node(consumer_);
    }
  }

  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_;
  std::shared_ptr<Producer> producer_;
  std::shared_ptr<Consumer> consumer_;
};

int main(int argc, char * argv[])
{
  setvbuf(stdout, NULL, _IONBF, BUFSIZ);
  rclcpp::init(argc, argv);

  NodeManager node_manager;

  // 开始执行
  node_manager.spin();

  rclcpp::shutdown();

  return 0;
}

预告

关于左右值引用和完美转发std::forward以及std::move后续会出一篇文章详细说明

参考文章

https://docs.ros2.org/dashing/api/rclcpp/classrclcpp_1_1executor_1_1Executor.html#a34509baf643c9a7f88bf0b7f4a983955
https://docs.ros.org/en/foxy/How-To-Guides/Using-callback-groups.html#id1
https://docs.ros.org/en/foxy/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Publisher-And-Subscriber.html