ROS2学习之路之回调组_rclcpp::executors::multithreadedexecutor

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

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

1、创建节点

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

2、创建执行器
类中创建

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

或者
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);
1
2
3
4

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_
1
2
3
4
5
6
7
8
9
10
11
12

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


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184