how to implement multiple subscribers simply
I am studying ROS2 dashing. I 'd like to create many simple subscribers which subscribe same topic and test CPU performance. Now I am trying as follws. This is a example which two subscribers run. I'd like to test a case in which ,for example, 100 subscribers are running. Is there any better way to realize multiple such ? Thanks.
//subnodes.hpp
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/int16.hpp>
class MinimalSubscriber1:public rclcpp::Node{
private:
rclcpp::Subscription<std_msgs::msg::Int16>::SharedPtr subscription_;
void topic_callback_(const std_msgs::msg::Int16::SharedPtr msg);
public:
MinimalSubscriber1();
};
class MinimalSubscriber2:public rclcpp::Node{
private:
rclcpp::Subscription<std_msgs::msg::Int16>::SharedPtr subscription_;
void topic_callback_(const std_msgs::msg::Int16::SharedPtr msg);
public:
MinimalSubscriber2();
};
//subnodes.cpp
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/int16.hpp>
#include "subnodes.hpp"
void MinimalSubscriber1::topic_callback_(const std_msgs::msg::Int16::SharedPtr msg){
RCLCPP_INFO(this->get_logger(), "No.1 heard:%d",msg->data);
}
MinimalSubscriber1::MinimalSubscriber1()
:Node("minimal_subscriber_test1"){
subscription_ = this->create_subscription<std_msgs::msg::Int16>(
"testtopic",
std::bind(&MinimalSubscriber1::topic_callback_,this,std::placeholders::_1)
);
}
void MinimalSubscriber2::topic_callback_(const std_msgs::msg::Int16::SharedPtr msg){
RCLCPP_INFO(this->get_logger(), "No.2 heard:%d",msg->data);
}
MinimalSubscriber2::MinimalSubscriber2()
:Node("minimal_subscriber_test2"){
subscription_ = this->create_subscription<std_msgs::msg::Int16>(
"testtopic",
std::bind(&MinimalSubscriber2::topic_callback_,this,std::placeholders::_1)
);
}
//main.cpp
#include <rclcpp/rclcpp.hpp>
#include "subnodes.hpp"
int main(int argc,char*argv[]){
rclcpp::init(argc,argv);
rclcpp::executors::SingleThreadedExecutor exec;
auto node1 = std::make_shared<MinimalSubscriber1>();
auto node2 = std::make_shared<MinimalSubscriber2>();
exec.add_node(node1);
exec.add_node(node2);
exec::spin();
rclcpp::shutdown();
return 0;
}
Long answer: Have a look here: https://discourse.ros.org/t/singlethr... And here: https://github.com/nobleo/ros2_perfor...
Short answer: You can create subscribers in a loop but you have to save a pointer to the subscribers outside of the loop
I'm not sure if the example is using the dashing API, but a similar implementation should be possible in dashing. The important thing is that you have the sub_refs declared outside of the loop.
Full example of source code can be found at the nobleo github on the dashing branch in the source folder. Look for ros.cc , that one partly does what you want.
Just a heads up, working with CPU utilization is not as straight-forward as some people might think. Especially when dealing with big.LITTLE configurations and other factors that might influence CPU utilization. The results you can find in the discourse post and on the nobleo github are all hardware and software specific and the tests may return different results on your setup.