複数のsubscriberの呼び出しについて

[akitosh...@gmail.com]

学生です。

私は複数のsubscriberを周期的かつ同時に呼び出すプログラムを書きたいと思っています。そのため最初はprogram1のようにプログラムを書いていましたが、main関数だけをprogram2のように書き換えました。fig1のプログラムではros::spin()によって、３つのsubscriberがバラバラに呼び出されていました。そこでwhile文を使って周期的に呼びだそうと思いました。しかし、program2のプログラムでもバラバラに呼び出されてしまいました。しかも、program1のプログラムでは実行されていたdepthCbの中で記述されているpublisherも実行されなくなってしまいました。どのようにプログラムを書けば複数のsubscriberを定期的かつ同時に呼び出すことができるのでしょうか。


program1

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

void messageCallback(const sensor_msgs::JointState::ConstPtr& msg3)
{
   ...
}

void imageCb(const sensor_msgs::ImageConstPtr& msg2)
{
   ...
}

void depthCb( const sensor_msgs::ImageConstPtr& msg1)
{
   ...
   ros::Publisher pub = n.advertise<PointCloud> ("points2", 1);
   PointCloud::Ptr cloud (new PointCloud);
   cloud->header.frame_id = "/base";
   pcl_conversions::toPCL(ros::Time::now(), cloud->header.stamp);
   ...
   cloud->points.push_back (cloud->points[0]);
   pcl_conversions::toPCL(ros::Time::now(), cloud->header.stamp);
   pub.publish (cloud);
}

int main( int argc, char* argv[])
{
   ros::init( argc, argv, "depth_viewer" );
   ros::NodeHandle n;

   ros::Subscriber joint = n.subscribe("/hp3j/joint_states", 1000, messageCallback);
   ros::Subscriber color = n.subscribe("/pico_flexx_link_ir/image_raw", 1000, imageCb);
   ros::Subscriber depth = n.subscribe("/pico_flexx_link/depth_registered/image_raw", 3, &depthCb);

   ros::spin();
   return 0;
}

program2

int main(argc, char* argv[])
{
   ros::init( argc, argv, "depth_viewer" );
   ros::NodeHandle n;

   ros::Subscriber joint = n.subscribe("/hp3j/joint_states", 1000, messageCallback);
   ros::Subscriber color = n.subscribe("/pico_flexx_link_ir/image_raw", 1000, imageCb);
   ros::Subscriber depth = n.subscribe("/pico_flexx_link/depth_registered/image_raw", 3, &depthCb);

   ros::Rate loop_rate(0.1);
   while (ros::ok()) {
      ros::spinOnce();
      loop_rate.sleep();
   }

   return 0;
}

[Junya Hayashi]

用途に応じて、幾つか方法があります。

■ 画像データと CameraInfo を同期的に取得したい場合
image_transport が利用できます。

http://wiki.ros.org/image_transport#image_transport_Subscribers

■取得したいメッセージの timestamp が同期されている場合

同一カメラのRGB画像とDepth画像を取得したい場合で、カメラのドライバが画像ごとの timestamp を揃えてくれている場合などは、 message_filters の TimeSynchronizer が利用できます。

http://wiki.ros.org/message_filters#Time_Synchronizer

timestamp が同期されていない場合には、同じく message_filters の ApproximateTime Policy を利用する必要があります。

■ベタな方法

各 callback で

void callback(const sensor_msgs::ImageConstPtr& msg)

{

   last_msg = msg;

}

のように最後に受信したメッセージを記録しておき、 Timer を使ってメイン処理を一定周期ごとに呼びだす方法もあります。

http://wiki.ros.org/roscpp/Overview/Timers

処理の仕方によっては、同じメッセージを複数回処理することになったり、読まれないメッセージが発生しうるので、注意して下さい。

2017年8月4日 19:35 <akitosh...@gmail.com>:

--
このメールは Google グループのグループ「ROS JAPAN Users Group」に登録しているユーザーに送られています。
このグループから退会し、グループからのメールの配信を停止するには ros-japan-users+unsubscribe@googlegroups.com にメールを送信してください。
その他のオプションについては https://groups.google.com/d/optout にアクセスしてください。

[akitosh...@gmail.com]

Hayashiさん、ご返信ありがとうございます。

私が作ろうとしているプログラムはGazebo上で立ち上げたopenni_cameraを実装したマニピュレータから深度値、RGB値、関節角度を取得して環境地図を作成するというものです。

そのため、ご提案頂いた「ベタな方法」が当てはまるかと思い試してみたところ、一定周期で呼び出すことに成功しました！
加えて、これは後学のための質問なのですがmessage_filters の TimeSynchronizer は timestamp を揃えてくれているといった使用条件があるのですか？ message_filters の wiki を見た限りではそのような記述がなかったように思ったのですが。私のイメージだと同じcallback関数のなかで定義された関数を同期して呼び出すといったものだと考えています。

2017年8月4日 19:35 <akitosh...@gmail.com>:

このグループから退会し、グループからのメールの配信を停止するには ros-japan-use...@googlegroups.com にメールを送信してください。
その他のオプションについては https://groups.google.com/d/optout にアクセスしてください。

[Takashi Ogura]

小倉です。

> しかも、program1のプログラムでは実行されていたdepthCbの中で記述されているpublisherも実行されなくなってしまいました。

これの原因ですが、以下のコールバックでは、関数の最初でadvertiseしていて、その後即座にpublishしています。

ROSのadvertise()関数はrosmasterに登録するところまでやりますが、

その瞬間にsubscriberとの接続が確立されるわけではありません。

実際に接続されるかどうかはタイミング次第

（rosmasterがsubscriberに通知し、subscriberが接続しにくるというタイムラグがある）

プログラムになってしまっています。

ではどうするか、というと、

publisherは毎回作るのではなく、１つだけ作って使いまわすのが普通です。
（クラスメンバなどにするのが普通かと思います）

void depthCb( const sensor_msgs::ImageConstPtr& msg1)

{
   ...
   ros::Publisher pub = n.advertise<PointCloud> ("points2", 1);
   PointCloud::Ptr cloud (new PointCloud);
   cloud->header.frame_id = "/base";
   pcl_conversions::toPCL(ros::Time::now(), cloud->header.stamp);
   ...
   cloud->points.push_back (cloud->points[0]);
   pcl_conversions::toPCL(ros::Time::now(), cloud->header.stamp);
   pub.publish (cloud);
}

2017年8月4日 19:35 <akitosh...@gmail.com>:

--
このメールは Google グループのグループ「ROS JAPAN Users Group」に登録しているユーザーに送られています。

このグループから退会し、グループからのメールの配信を停止するには ros-japan-users+unsubscribe@googlegroups.com にメールを送信してください。
その他のオプションについては https://groups.google.com/d/optout にアクセスしてください。

--

/*************************
Takashi Ogura (小倉 崇)

t.o...@gmail.com
http://youtube.com/ogutti
twitter: @OTL
*************************/

[akitosh...@gmail.com]

小倉さん、ご返信ありがとうございます。

publisherとsubscriberを同じnodeに書くプログラミングにずっと苦戦していたのですが、それが原因のように思われます。また、プログラムを終了すると毎回コアダンプが生じるのも毎回publisherを作ってしまっていることに関連するのでしょうか？

2017年8月4日 19:35 <akitosh...@gmail.com>:

このグループから退会し、グループからのメールの配信を停止するには ros-japan-use...@googlegroups.com にメールを送信してください。
その他のオプションについては https://groups.google.com/d/optout にアクセスしてください。

[Junya Hayashi]

林です。

> message_filters の TimeSynchronizer は timestamp を揃えてくれているといった使用条件があるのですか？

はい、Subscribe 対象のトピックのメッセージの timestamp は、同じでないと動作しません。

基本的には、同一の入力ソースを元にした情報（例: カメラ画像と、カメラ画像を元に抽出した顔検出結果など。ソースは同じなので、timestamp は正確に一致する）を同時に利用する場合に使います。

timestamp のずれが発生する場合には、上記のベタな方法か、ApproximateTime Policy を利用する必要があります。

> 私のイメージだと同じcallback関数のなかで定義された関数を同期して呼び出すといったものだと考えています。

書かれている意味がよく分からなかったのですが、wiki の例にあるように、callback の引数には、同期された複数の ROS Message が渡されます。

void callback(const ImageConstPtr& image, const CameraInfoConstPtr& cam_info)

{

  // Solve all of perception here...

}

2017年8月8日 17:28 <akitosh...@gmail.com>:

このグループから退会し、グループからのメールの配信を停止するには ros-japan-users+unsubscribe@googlegroups.com にメールを送信してください。
その他のオプションについては https://groups.google.com/d/optout にアクセスしてください。

[akitosh...@gmail.com]

林さん、ご返信ありがとうございます。また、下手な日本語にも関わらずご回答いただき重ねてお礼申し上げます。

TimeSynchronizer について誤った認識をしておりました。それなら、今回はご提案頂いた「ベタな方法」を使わせていただくことにします。また、後学のため ApproximateTime Policy についても試してみようと思います。

なにわともあれ、おかげさまで解決することができました。心より感謝申し上げます。