ROS2学习之路之回调组

rclpy中有SingleThreadedExecutor和MultiThreadedExecutor两种类型的executor，rclcpp还包含StaticSingleThreadedExecutor。SingleThreadedExecutor使用一个线程排队处理多个消息。MultiThreadedExecutor可创建多个线程（可配置）以并行处理多个消息。

一个节点配一个执行器，可以允许多个回调组

1、创建节点

rclcpp::Node::SharedPtr srv_node_ = std::make_shared<rclcpp::Node>("srv");

2、创建执行器
类中创建

  rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_ ;  // 多线程执行器
  executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
  executor_->add_node(nodePtr_);

或者
main函数创建

  auto exec_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>(rclcpp::executor::create_default_executor_arguments(), 5);
  exec_->add_node(nodePtr);
  exec_->spin();
  exec_->remove_node(nodePtr);

3、创建分组

// MutuallyExclusive；互斥，即这个组别中每时刻只允许1个线程，一个callback在执行时，其他只能等待
// Reentrant；可重入，这个组别中每时刻允许多个线程，一个Callback在执行时，其他callback可开启新的线程

rclcpp::CallbackGroup::SharedPtr callBackGroup_;//不能写在构造中，否则请求不到服务
callBackGroup_      = nodePtr_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);//重入（Reentrant：每时刻允许多个线程） 互斥（MutuallyExclusive：每时刻只允许1个线程）
rclcpp::CallbackGroup::SharedPtr srvCallBackGroup_;
srvCallBackGroup_   = nodePtr_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);//重入（Reentrant：每时刻允许多个线程） 互斥（MutuallyExclusive：每时刻只允许1个线程）
rclcpp::SubscriptionOptions options;
options.callback_group     = callBackGroup_;
//用于订阅  options
//用于服务  srvCallBackGroup_

eg.1

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/int8.hpp>
#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include "std_msgs/msg/string.hpp"
#include "msg_srv/srv/test.hpp"
#include "msg_srv/msg/test.hpp"
using namespace std::chrono_literals;
using std::placeholders::_1;
using std::placeholders::_1;

class MinimalNode
{
  private:
    rclcpp::Node::SharedPtr nodePtr_;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_,subscription1_,subscription2_;
    rclcpp::Service<msg_srv::srv::Test>::SharedPtr  Srv1_,Srv2_;

    rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_ ;  // 多线程执行器

    rclcpp::CallbackGroup::SharedPtr callBackGroup_;//不能写在构造中，否则请求不到服务
    rclcpp::CallbackGroup::SharedPtr srvCallBackGroup_;//不能写在构造中，否则请求不到服务

  public:
    MinimalNode(rclcpp::Node::SharedPtr& nodePtr):nodePtr_(nodePtr)
    {
      // 一个节点配一个执行器，可以允许多个回调组
      //法一，在main中创建执行器
      // // main中写入
      // // auto exec_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>(rclcpp::executor::create_default_executor_arguments(), 5);
      // // exec_->add_node(nodePtr);
      // // exec_->spin();
      // rclcpp::CallbackGroup::SharedPtr callBackGroup_      = nodePtr_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);//重入（Reentrant：每时刻允许多个线程） 互斥（MutuallyExclusive：每时刻只允许1个线程）
      // rclcpp::CallbackGroup::SharedPtr srvCallBackGroup_   = nodePtr_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);//重入（Reentrant：每时刻允许多个线程） 互斥（MutuallyExclusive：每时刻只允许1个线程）
      // rclcpp::SubscriptionOptions options;
      // options.callback_group     = callBackGroup_;

      // subscription_  = nodePtr_->create_subscription<std_msgs::msg::String>("topic", 10, std::bind(&MinimalNode::topic_callback, this, _1),options);
      // subscription1_ = nodePtr_->create_subscription<std_msgs::msg::String>("topic1", 10, std::bind(&MinimalNode::topic_callback1, this, _1),options);
      // subscription2_ = nodePtr_->create_subscription<std_msgs::msg::String>("topic2", 10, std::bind(&MinimalNode::topic_callback2, this, _1),options);

      // Srv1_ = nodePtr_->create_service<msg_srv::srv::Test>("Srv1", std::bind(&MinimalNode::srv_callback1, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default, srvCallBackGroup_);
      // Srv2_ = nodePtr_->create_service<msg_srv::srv::Test>("Srv2", std::bind(&MinimalNode::srv_callback2, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default, srvCallBackGroup_);

      //法二，在类中创建执行器，但是在类中需要创建线程堵塞回调线程
      // // rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_ ;  // 多线程执行器
      // // executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
      // // executor_->add_node(nodePtr_);
      // rclcpp::CallbackGroup::SharedPtr callBackGroup_      = nodePtr_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);//重入（Reentrant：每时刻允许多个线程） 互斥（MutuallyExclusive：每时刻只允许1个线程）
      // rclcpp::CallbackGroup::SharedPtr srvCallBackGroup_   = nodePtr_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);//重入（Reentrant：每时刻允许多个线程） 互斥（MutuallyExclusive：每时刻只允许1个线程）
      // rclcpp::SubscriptionOptions options;
      // options.callback_group     = callBackGroup_;

      // subscription_  = nodePtr_->create_subscription<std_msgs::msg::String>("topic", 10, std::bind(&MinimalNode::topic_callback, this, _1),options);
      // subscription1_ = nodePtr_->create_subscription<std_msgs::msg::String>("topic1", 10, std::bind(&MinimalNode::topic_callback1, this, _1),options);
      // subscription2_ = nodePtr_->create_subscription<std_msgs::msg::String>("topic2", 10, std::bind(&MinimalNode::topic_callback2, this, _1),options);

      // Srv1_ = nodePtr_->create_service<msg_srv::srv::Test>("Srv1", std::bind(&MinimalNode::srv_callback1, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default, srvCallBackGroup_);
      // Srv2_ = nodePtr_->create_service<msg_srv::srv::Test>("Srv2", std::bind(&MinimalNode::srv_callback2, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default, srvCallBackGroup_);
      // void run()
      // {
      //   RCLCPP_INFO(nodePtr_->get_logger(), "111111111");
      //   executor_->spin();
      // }


      // //法三，在类中创建另外的节点，并且创建另外的执行器，在类中需要创建线程堵塞回调线程
      // srv_node_ = std::make_shared<rclcpp::Node>("srv");
      // rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_ ;  // 多线程执行器
      // rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_srv_ ;  // 多线程执行器
      // executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
      // executor_srv_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
      // executor_->add_node(nodePtr_);
      // executor_srv_->add_node(srv_node_);//添加节点
      // rclcpp::CallbackGroup::SharedPtr callBackGroup_      = nodePtr_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);//重入（Reentrant：每时刻允许多个线程） 互斥（MutuallyExclusive：每时刻只允许1个线程）
      // rclcpp::CallbackGroup::SharedPtr srvCallBackGroup_   = srv_node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);//重入（Reentrant：每时刻允许多个线程） 互斥（MutuallyExclusive：每时刻只允许1个线程）
      // rclcpp::SubscriptionOptions options;
      // options.callback_group     = callBackGroup_;

      // subscription_  = nodePtr_->create_subscription<std_msgs::msg::String>("topic", 10, std::bind(&MinimalNode::topic_callback, this, _1),options);
      // subscription1_ = nodePtr_->create_subscription<std_msgs::msg::String>("topic1", 10, std::bind(&MinimalNode::topic_callback1, this, _1),options);
      // subscription2_ = nodePtr_->create_subscription<std_msgs::msg::String>("topic2", 10, std::bind(&MinimalNode::topic_callback2, this, _1),options);

      // Srv1_ = srv_node_->create_service<msg_srv::srv::Test>("Srv1", std::bind(&MinimalNode::srv_callback1, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default, srvCallBackGroup_);
      // Srv2_ = srv_node_->create_service<msg_srv::srv::Test>("Srv2", std::bind(&MinimalNode::srv_callback2, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default, srvCallBackGroup_);
      // void run()
      // {
      //   RCLCPP_INFO(nodePtr_->get_logger(), "111111111");
      //   std::thread{[&]{executor_srv_->spin();}}.detach();//堵塞线程
      //   executor_->spin();
      // }


      //eg.1 双节点
      // callBackGroup_   = nodePtr_->create_callback_group(rclcpp::callback_group::CallbackGroupType::MutuallyExclusive);//重入（Reentrant：每时刻允许多个线程） 互斥（MutuallyExclusive：每时刻只允许1个线程）
      // executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
      // executor_->add_node(nodePtr_);
      // rclcpp::SubscriptionOptions options;
      // options.callback_group     = callBackGroup_;

      // srv_node_ = std::make_shared<rclcpp::Node>("srv");
      // srvCallBackGroup_   = nodePtr_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);//重入（Reentrant：每时刻允许多个线程） 互斥（MutuallyExclusive：每时刻只允许1个线程）
      // srvCallBackExecutor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>(rclcpp::ExecutorOptions(), 2);
      // srvCallBackExecutor_->add_node(srv_node_);





      //一个节点，并且在main函数中生成执行器，两个回调分组
      callBackGroup_      = nodePtr_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);//重入（Reentrant：每时刻允许多个线程） 互斥（MutuallyExclusive：每时刻只允许1个线程）
      srvCallBackGroup_   = nodePtr_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);//重入（Reentrant：每时刻允许多个线程） 互斥（MutuallyExclusive：每时刻只允许1个线程）
      rclcpp::SubscriptionOptions options;
      options.callback_group     = callBackGroup_;




      subscription_  = nodePtr_->create_subscription<std_msgs::msg::String>("topic", 10, std::bind(&MinimalNode::topic_callback, this, _1),options);
      subscription1_ = nodePtr_->create_subscription<std_msgs::msg::String>("topic1", 10, std::bind(&MinimalNode::topic_callback1, this, _1),options);
      subscription2_ = nodePtr_->create_subscription<std_msgs::msg::String>("topic2", 10, std::bind(&MinimalNode::topic_callback2, this, _1),options);

      Srv1_ = nodePtr_->create_service<msg_srv::srv::Test>("Srv1", std::bind(&MinimalNode::srv_callback1, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default, srvCallBackGroup_);
      Srv2_ = nodePtr_->create_service<msg_srv::srv::Test>("Srv2", std::bind(&MinimalNode::srv_callback2, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default, srvCallBackGroup_);


    }
    void topic_callback(const std_msgs::msg::String::SharedPtr msg) const {RCLCPP_INFO(nodePtr_->get_logger(), "I heard: '%s'", msg->data.c_str());}
    void topic_callback1(const std_msgs::msg::String::SharedPtr msg) const {RCLCPP_INFO(nodePtr_->get_logger(), "I heard: '%s'", msg->data.c_str());}
    void topic_callback2(const std_msgs::msg::String::SharedPtr msg) const {RCLCPP_INFO(nodePtr_->get_logger(), "I heard: '%s'", msg->data.c_str());}
    
    bool srv_callback1(const std::shared_ptr<msg_srv::srv::Test::Request> req, std::shared_ptr<msg_srv::srv::Test::Response> res) const
    {
      std::vector<int> num(req->goal_ids);
      std::string id = req->id;
      res->success = false;
      RCLCPP_INFO(nodePtr_->get_logger(), "I heard: '%s'  call success!!!", req->id.c_str());
      return false;
    }
    bool srv_callback2(const std::shared_ptr<msg_srv::srv::Test::Request> req, std::shared_ptr<msg_srv::srv::Test::Response> res) const
    {
      std::vector<int> num(req->goal_ids);
      std::string id = req->id;
      res->success = false;
      RCLCPP_INFO(nodePtr_->get_logger(), "I heard: '%s'  call success!!!", req->id.c_str());
      return false;
    }
    void run()
    {
      RCLCPP_INFO(nodePtr_->get_logger(), "111111111");
      // std::thread{[&]{srvCallBackExecutor_->spin();}}.detach();
      // executor_->spin();
    }

};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);

  auto nodePtr = rclcpp::Node::make_shared("mpac_points_filter");

  auto heartBeatPubPtr = nodePtr->create_publisher<std_msgs::msg::Int8>("heartBeat", 10);

  std_msgs::msg::Int8 heartBeatData;
  heartBeatData.data = 6;  //6-filter node
  auto heartBeatTimer = nodePtr->create_wall_timer(std::chrono::seconds(1), [heartBeatPubPtr, heartBeatData]() { heartBeatPubPtr->publish(heartBeatData); });

  RCLCPP_INFO(nodePtr->get_logger(), "start");
  MinimalNode vcq(nodePtr);
  vcq.run();
  auto exec_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>(rclcpp::executor::create_default_executor_arguments(), 5);
  exec_->add_node(nodePtr);
  exec_->spin();
  exec_->remove_node(nodePtr);
  RCLCPP_INFO(nodePtr->get_logger(), "end");
  rclcpp::shutdown();

  return 0;
}