ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

Any way to check if/ensure that ROS node subscribed to topic properly?

asked 2012-12-09 07:59:14 -0500

sgwhack gravatar image

updated 2012-12-14 05:26:25 -0500

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:

  1. 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.
  2. Is there a way to use the 'regular', 'non-arduino' subscribers and nodehandles to be able ...
(more)
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
1

answered 2012-12-11 11:03:18 -0500

PerkinsJames gravatar image

updated 2012-12-11 11:03:59 -0500

Can you just use the information here?

http://www.ros.org/doc/api/roscpp/html/classros_1_1NodeHandle.html

If you go to your proper subscribe function you are using, it actually throws two exceptions:

InvalidNameException & ConflictingSubscriptionException

and can check your "handle."

edit flag offensive delete link more

Comments

which part is the 'handle'? I'm not sure what to put in there

sgwhack gravatar image sgwhack  ( 2012-12-11 14:39:30 -0500 )edit

Looking other subscribe functions on that page there is a part that shows this: ros::Subscriber sub = handle.subscribe("my_topic", 1, callback) and then gives an example of it in an if(handle). It comes from the statement ros::NodeHandle handle that you have to create before you make subscribers.

PerkinsJames gravatar image PerkinsJames  ( 2012-12-12 02:55:42 -0500 )edit

how does that distinguish the success of each individual subscription? if I use "if(handle{...}", and use nh as my handle, am I to assume that as long as one subscription fails, the if statement will detect this?

sgwhack gravatar image sgwhack  ( 2012-12-12 06:41:33 -0500 )edit

good point. Check this previous post too for checking number of publishers that I had in the past: http://answers.ros.org/question/46116/determine-if-another-rosnode-is-subscribing-to-published-topic/ . Maybe there is something equivalent for a subscriber?

PerkinsJames gravatar image PerkinsJames  ( 2012-12-13 09:22:51 -0500 )edit

I did want to use getNumPublishers(), from the subscription on the arduino side.

However, i'm not so sure if this option is available for the arduino version of the subscriber. If i do the check on the publisher side, i dont see how i can tell the arduino to try to re-subscribe if it fails

sgwhack gravatar image sgwhack  ( 2012-12-13 09:30:45 -0500 )edit
1

answered 2012-12-11 03:26:01 -0500

Kevin gravatar image

have you tried roswtf to see if there are any errors when everything is running?

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2012-12-09 07:59:14 -0500

Seen: 11,211 times

Last updated: Dec 14 '12