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