ROS2 C++ Re-Created Subscriber Not Listening
I'd like to be able to disable a subscription temporarily, but as far as I know, the Subscription<t> type does not support that. Rather than use if(!disabled)
in all of my message handlers, I've taken the approach of destroying my subscriptions when they are not in use via shared_ptr::reset()
and then calling node::create_subscription()
again when I want to start listening.
I've been seeing a lot of cases of dropped communications, where I need to restart my C++ application to get it to start listening again, which kind of defeats the purpose of trying to do this.
I've put together a minimal test case, included below. The node subscribes to channel A and publishes every second on channel B. You can run two instances built with different topic names or run a python script to work in tandem with it, the result will be the same, as far as I can tell (I've tried both). If any command line arguments are given, then after publishing, the node has a 20% chance of resetting its subscriber and creating a new one.
Running this, against another node to publish and listen, this is what I find:
- If the subscription is never reset, either node can drop and restart and messaging continues
- If the subscription IS reset, it continues working, but only until the node publishing to it dies; if that node is started again, the subscription never reconnects
I would expect that destroying and re-creating the subscription would reconnect and continue listening as it does in the case where it's never destroyed.
So, is this an incorrect use of ROS, or is it a bug? If I am using subscriptions incorrectly, what is the correct way to achieve the effect I want? One of the things my application does is allow users to change the topics at runtime, so if they change to topic from A to B, and then back to A, isn't that going to yield the same behavior I am seeing here?
System Details:
- Ubuntu 16.04 Kernel 4.15.0-30-generic
- ROS2 Ardent binaries installed through apt-get
- I have not set the RMW_IMPLEMENTATION environment variable, so my understanding is that FastRTPS will be used
int main(int argc, char** argv)
{
bool doResets = argc > 1;
rclcpp::init(argc, argv);
shared_ptr<rclcpp::Node> node = make_shared<rclcpp::Node>("pub1");
auto publisher = node->create_publisher<std_msgs::msg::String>("world", 10);
auto msg = make_shared<std_msgs::msg::String>();
auto subFunc =
[](std_msgs::msg::String::ConstSharedPtr str)
{
cout << "I heard: " << str->data << endl;
};
auto subscriber = node->create_subscription<std_msgs::msg::String>("hello", subFunc);
auto timer = node->create_wall_timer(1000ms,
[&]()
{
cout << "Publish!" << endl;
msg->data = "Hello World Pub3";
publisher->publish(msg);
if(doResets && rand() % 100 > 80)
{
cout << "Rebuild subscriber" << endl;
subscriber.reset();
subscriber = node->create_subscription<std_msgs::msg::String>("hello", subFunc);
}
});
rclcpp::spin(node);
rclcpp::shutdown();
}