addPostDynamicsFunctionをメッセージのpublishに使うのは避けたほうが良いかもしれない

[yosuke]

ここ数年の変更でWorldRosItemがaddPostDynamicsFunctionのコールバックを使ってメッセージをpublishするように実装が変更されているようなのですが、その影響でタイムバーが進まなくなっていたり、シミュレーションが安定して開始してくれないなどの問題が発生しているようです。

以前のTimeBar::instance()->sigTimeChanged()を使った実装であれば安定すると思うので、戻したほうが良いかもしれません。

また、デフォルトではcollisionが10ms単位でpublishされているようですが、大きなデータになる場合はこの頻度でpublishすると大きな負荷になると思うので、調整したほうが良さそうに思います。

[fkanehiro]

タイムバーは描画の進行を表していて、シミュレーションの進行とは必ずしも同期していないと思ったのですがいかがでしょうか。

[yosuke]

厳密な意味ではそうだと思うのですが、addPostDynamicsFunctionを使うとシミュレーションループの中にROS topicのTCP通信が入り込んでしまうので、いろいろよろしくなさそうな気がします。

TimeBar::instance()->sigTimeChanged()であれば、GUI側のスレッドに逃がせるので。

ROS topicはソフトリアルタイムでしか使えない通信方式（ハードリアルタイムはros_controlで実装する）なので、シミュレーションとの厳密な同期はそもそもあまり考えないで良いように思います。

ただ、この実装だとタイムバーを戻したときなどに変なことが起こりそうなので、一番良いのはそれとは更に別のスレッドを作ることですが、不安定なのは困るのでとりあえず安定側に戻したいです。

[yosuke]

ただ少し不思議に思うのは、Body以下にぶら下げる形で動くBodyRosItemの方だと、同期的にpublishしているにもかかわらず、このような問題が起こらず安定して動いています。

おそらくChoreonoidがcontrollerを安定して動かすためのスレッド運用を裏でしてくれている（addPostDynamicsFunctionだとそれがない）のだろうと理解しています。

WorldRosItemで重要なのは/clockトピックなので、それをBodyRosItemの方でpublishするようにしてしまうのも手の一つだと思うのですが、この実装だとBodyRosItemを複数のBodyにぶら下げたときに少し気持ちが悪いかなと思っています。

[yosuke]

よくよく実装を見たら、ros::AsyncSpinnerがItem側のstart()関数内で、おそらくChoreonoidが管理している子スレッド内で開始されているのですが、publishが行われるのはaddPostDynamicsFunctionを使うとシミュレーションスレッド内なので、その不整合が問題の原因になっているのかもしれません。

addPostDynamicsFunctionを使ってspinさせることで問題が解消するのであれば、それが一番きれいかも。

[yosuke]

ros::spinOnceできちんと同期的に出力するように変更しました。

その他、シミュレーションが0でない時間でstartした場合の問題も修正してあります。

タイムバーが進まないことがあるのはchoreonoid本体の問題かもしれません（ワールドにstaticなオブジェクトしか無いとWordRosItemが無くてもこの症状が出るようです）。

[yosuke]

タイムバーの問題に関しては、本家にレポート済み。

s-nakaoka/choreonoid#212

[yosuke]

当初の問題認識ではシミュレータスレッド内でpublish&spinするのは良くないと思っていたが、実際はspinはasyncSpinがItem側でかかっていたので非同期だった。

問題はむしろstart()でnext_publish_timeがゼロになっており、各ループでインクリメントされるのみの実装だったため、ゼロでない時間でstart()するとしばらくは最短周期でpublishがかかってしまうことだったのかもしれない。実際のパケットの送信はasyncSpinのタイミングで行われるので、送信が間に合わず詰まってしまっているような状態だったのかも（今はspinOnceで同期して送信するので詰まらずシミュレーションが遅くなる）。

[fkanehiro]

ROSの仕組みを確認させて下さい。
spin()またはそれに類似する関数が行う処理はsubscribeしているトピックの受信処理、外部からのサービス呼び出しへの対応ではないかと思っていた（実装は確認していない）のですが、トピックのpublishに関してもspin()が関係しているということでしょうか。

[yosuke]

私もあやふやでpublishでキューされてspinで送信されるものと思い込んでいたのですが、調べると @fkanehiro さんのほうが正解で、publishはspinを待たずに新しいスレッドを作って送信されるみたいです。

https://answers.ros.org/question/257361/what-is-the-actual-meaning-of-rosspin/

なので、私のpull requestの以下の行は不要ですね。

https://github.com/yosuke/choreonoid_ros_pkg/blob/master/choreonoid_plugins/src/WorldRosItem.cpp#L391

publishに関しては結局どうやっても非同期になってしまうという理解で良さそうです。

また、よくよくソースを見てみると、以下の行はasyncSpinと等価なので本当は不要そうです。

https://github.com/fkanehiro/choreonoid_ros_pkg/blob/master/choreonoid_plugins/src/WorldRosItem.cpp#L504

また、 #22 で一度pauseするとunpause_physicsサービスコールができないと書いてありますが、原因はstop()でspinやqueueを止めてしまっているからかもしれません。

https://github.com/fkanehiro/choreonoid_ros_pkg/blob/master/choreonoid_plugins/src/WorldRosItem.cpp#L608

[fkanehiro]

そうするとspinnerは元のasyncSpinnerの方がパフォーマンス的に有利と思われるので、PRのそのあたりは元に戻して頂いて、時刻が0以外から始まるケースに対応するPRとして頂けますでしょうか。

なお、リンク先のページからはpublishがスレッドを持っているかどうかは読み取れませんでした。publishがRTMで言う所のflushタイプで動作しているとするとシミュレーションのパフォーマンスに大きな影響を与えそうですね。

[yosuke]

asyncSpinnerに戻す件、了解しました。
結局、今はシミュレータスレッドとItemのスレッドで2つspinnerに相当するものが走ってしまっている状態ですし、asyncSpinner一つにしてしまったほうが良いと思います。

ROSのpublishの流れについて、私も興味があったので調べてみたのですが、publishされたキューは、実際にはTopicManagerの以下の部分で処理されるようです。

https://github.com/ros/ros_comm/blob/melodic-devel/clients/roscpp/src/libros/topic_manager.cpp#L88

TopicManagerがlistenしているPollManagerはPollSetを同期に使っており、

https://github.com/ros/ros_comm/blob/melodic-devel/clients/roscpp/src/libros/poll_manager.cpp#L88

PollSetはソケットの状態をモニタしているようです。

https://github.com/ros/ros_comm/blob/melodic-devel/clients/roscpp/src/libros/poll_set.cpp#L186

なので、flushタイプではなく、best effort型で動作するようです。

ただこの場合であってもデータのシリアライズやキューの管理はpublishリクエストごとに動作するので、あんまり高頻度でpublishすると遅くなると思います。