2011年4月26日火曜日

roscppのPublisherの挙動を調べる

一度だけメッセージをPublishしたい、というとき以下のようなコードを書くと思います。
これが下のコメントアウトしてあるsleep部分がないとうまくpublishされません。



#include <ros/ros.h>
#include <std_msgs/Int32.h>

int main(int argc, char* argv[]) {
  ros::init(argc, argv, "pubtest");
  ros::NodeHandle node;
  ros::Publisher pub = node.advertise<std_msgs::Int32>("hoge", 1);
  // sleep(1);
  std_msgs::Int32 msg;
  msg.data = 1;
  pub.publish(msg);
  // sleep(1);
  return 0;
}


何が起きているのか調べてみます。
advertiseした時点でmasterとのコネクションが確立され、publishが終わった時点でmasterとの通信が終わっているつもりになっていましたが、違うようです。

ということは実際にデータを送信するスレッドがいる、ということになりますね。

どの時点でそのスレッドは作られているんでしょうか?
たしかrospyはそんな仕様だったきがしますが、roscppもそうだったんですね。


後のsleepがないと動かないのは、そのスレッドが動く前にプログラムが終わってしまうからでしょう。
最初のsleepがないと動かない、ということはやはりPublisherが作られたときにスレッドが作られているんでしょう。Publisherごとにスレッドが作られているんでしょうか?

ソースを追いかけてみるとpoll_managerというのがinit時に作られていて、そいつがスレッドを持っているので、

そこにpublishキューを処理するコールバックを登録するみたいですね。
このスレッドはros::init時に作られているもよう。


じゃあ、なぜ最初のsleepが必要なんでしょうか?


以下のように getNumSubscribers()してみると少しわかります。
rostopic echo /hoge
している状態で、


sleepすると1で、sleepしないと0。



#include <ros/ros.h>
#include <std_msgs/Int32.h>

int main(int argc, char* argv[]) {
  ros::init(argc, argv, "pubtest");
  ros::NodeHandle node;
  ros::Publisher pub = node.advertise<std_msgs::Int32>("hoge", 0);
  sleep(1);
  if (pub) {
    ROS_INFO("num:%d", pub.getNumSubscribers());
  }
  std_msgs::Int32 msg;
  msg.data = 1;
  pub.publish(msg);
  sleep(1);
  return 0;
}

今度は以下のようにすると2になります。
同一プロセス内だとadvertiseした瞬間につながるみたいです。



int main(int argc, char* argv[]) {
  ros::init(argc, argv, "pubtest");
  ros::NodeHandle node;
  ros::Subscriber sub = node.subscribe<std_msgs::Int32>("hoge", 1, hoge);
  ros::Publisher pub = node.advertise<std_msgs::Int32>("hoge", 1);
  sleep(1);
  if (pub) {
    ROS_INFO("num:%d", pub.getNumSubscribers());
  }
  std_msgs::Int32 msg;
  msg.data = 1;
  pub.publish(msg);
  sleep(1);
  return 0;
}

ということで、(プロセス外の)subscriberとのリンクも別スレッドで行われているから、でした。

つまり、最初のsleepはsubscriberとのリンク待ち、2つ目はpublish待ちのsleepということでした。

ちなみに今回の場合は以下のようにlatchedにするとsleepは1回で済みますね。

int main(int argc, char* argv[]) {
  ros::init(argc, argv, "pubtest");
  ros::NodeHandle node;
  ros::Publisher pub = node.advertise<std_msgs::Int32>("hoge", 1, true);
  std_msgs::Int32 msg;
  msg.data = 1;
  pub.publish(msg);

  sleep(1); 
  if (pub) {
    ROS_INFO("hoge:%d", pub.getNumSubscribers());
  }

  return 0;
}

以上すごい適当に調べた結果メモでした〜。間違っていたら教えてください。

0 件のコメント:

コメントを投稿