Any way to check if/ensure that ROS node subscribed to topic properly?
Hey,
So I'm attempting to connect to multiple nodes through rosserial over bluetooth, but the connections are very inconsistent. Each node should subscribe to three topics, but a lot of the time, one or two of these subscriptions fail, and I have to re-try the connection.
It should look something like this, with three subscriptions set up:
[INFO] [WallTime: 1355082105.350746] ROS Serial Python Node
[INFO] [WallTime: 1355082105.364278] Connected on /dev/rfcomm2 at 57600 baud
[INFO] [WallTime: 1355082109.857011] Note: subscribe buffer size is 280 bytes
[INFO] [WallTime: 1355082109.857714] Setup subscriber on PS3_Status_1 [std_msgs/Int32MultiArray]
[INFO] [WallTime: 1355082109.867275] Setup subscriber on redBLANK [std_msgs/Int32MultiArray]
[INFO] [WallTime: 1355082109.883375] Setup subscriber on toArduinoLeadRED [std_msgs/Int32MultiArray]
But will typically look like this, with one or two missing:
[INFO] [WallTime: 1355082105.350746] ROS Serial Python Node
[INFO] [WallTime: 1355082105.364278] Connected on /dev/rfcomm2 at 57600 baud
[INFO] [WallTime: 1355082109.857011] Note: subscribe buffer size is 280 bytes
[INFO] [WallTime: 1355082109.857714] Setup subscriber on PS3_Status_1 [std_msgs/Int32MultiArray]
[INFO] [WallTime: 1355082109.867275] Setup subscriber on redBLANK [std_msgs/Int32MultiArray]
[INFO] [WallTime: 1355082109.883375] Packet Failed : Failed to read msg data
Is there any way that I can make the node keep attempting to subscribe before running the code? Or is there a better way to make sure all of these subscriptions are properly connected? It's kind of annoying to have to turn everything off and reset everything if the subscription fails.
The nodes are arduinos, and the set up code is standard:
void messageCont( const std_msgs::Int32MultiArray& x);
void AIStates( const std_msgs::Int32MultiArray& x);
void leadUpdate( const std_msgs::Int32MultiArray& x);
.
.
.
char controlSUB[13] = "PS3_Status_1";
char aiSUB[9] = "redWHITE";
char leadSUB[17] = "toArduinoLeadRED";
.
.
.
ros::NodeHandle nh;
ros::Subscriber<std_msgs::Int32MultiArray> subControl(controlSUB, &messageCont );
ros::Subscriber<std_msgs::Int32MultiArray> subAI(aiSUB, &AIStates );
ros::Subscriber<std_msgs::Int32MultiArray> subLeader(leadSUB, &leadUpdate );
void setup(){
.
.
.
nh.initNode();
nh.subscribe(subControl);
nh.subscribe(subAI);
nh.subscribe(subLeader);
}
.
.
.
Thanks!
EDIT: After looking at the link posted by PerkinsJames, I think those are for the 'regular' ros subscriptions, and not the ones that arduino uses.
I think the functions that I need are the ones from rosserial_arduino (someone let me know if I am wrong): http://ftp.isr.ist.utl.pt/pub/roswiki/doc/diamondback/api/rosserial_arduino/html/libraries_2ros__lib_2ros__lib_8cpp_source.html
It would seem that the subscribe function gives a boolean value, but it only affirms that the subscription has been added to the array, and not if it has been connected (Line 105)
bool ros::NodeHandle::subscribe(Subscriber & s)
{
int i;
for(i = 0; i < MAX_SUBSCRIBERS; i++)
{
if(subscribers[i] == 0) // empty slot
{
subscribers[i] = &s;
s.id_ = i+100;
s.nh_ = this;
return true;
}
}
return false;
}
So the questions now are:
- Is there any way to affirm the rosserial_arduino subscriptions are actually connecting? From looking at this file, I can't really see any. There aren't any functions that seem to do that.
- Is there a way to use the 'regular', 'non-arduino' subscribers and nodehandles to be able ...