search

start-jsk / rtmros_choreonoid

using chreonoid for simulator with hrpsys and other ros system
9 stars 43 forks source link

ROSBridgeに繋いだノードのros::spinOnce()が200Hzで頭打ちになる #207

Closed ishiguroJSK closed 8 years ago

ishiguroJSK commented 8 years ago

センサ読み取りノードなどで1000Hzでデータをpublishしたい場合に，普通に書くと

ros::Rate loop_rate(1000);
while(ros::ok()) {
    //(略)
    pub.publish(pubdata);
    ros::spinOnce();
    loop_rate.sleep();
}

みたいな感じだと思うのですが，choreonoid系のlaunchから起ち上げたroscoreに接続すると， loop_rate(1000)にしていても(rostopic hzなどで確認した出力トピックが)200Hzで頭打ちになってしまいます．

といったカンジです．ちなみにシミュレータ自体はほぼ実時間で回っています．

試しにloop_rate.sleep();をコメントアウトして全力ループで回すとどのケースも1200Hz(センサスペックの上限値)で回るので， 今回の件のloop_rate.sleep();の挙動がよく分からないです． (実機の場合に全力で回せば1200Hz出せるのに，1000Hz指定すると950Hzなのも若干納得できないのですが)

(センサがないと回りませんが参考までに以下のノードです) https://github.com/ishiguroJSK/leptrino_reader/blob/master/src/leptrino_reader.cpp#L156

YoheiKakiuchi commented 8 years ago

シミュレータで使うときは、rosの世界の時間は、/clockトピックを読んでいます。 hrpsys-simulatorやchoreonoidを使う場合は、/clockはHrpsysSeqStateRosBridgeが出しています。 choreonoidでは以下のとろころで200Hz(実際時間)にしていて、 https://github.com/start-jsk/rtmros_choreonoid/blob/master/hrpsys_choreonoid/launch/ros_bridge_choreonoid.launch#L23

/clockのpublishも200Hz（実際時間）になっています。 /clockが更新されるまでsleepは寝てしまうので、clockの更新が1000Hz以上にならないと、 1000Hzでまわらないと思います。

ros_bridgeは早く回しすぎると、計算コストが高すぎるように思いますが、 /clockは早く出したいという要求はありそうに思いますね。

ishiguroJSK commented 8 years ago

なるほど． 実時間で/clockが進んでいても更新が200Hzだったからダメだったんですね． そうなるとros::Rateを使うのは高周波数帯ではあまり安全な記述方法じゃないということになりそうですね．

センサはやはりいつでも1000Hzで回したいので，別スレッドで回す等で対応します． 回答有難うございました．

YoheiKakiuchi commented 8 years ago

http://wiki.ros.org/roscpp/Overview/Time ros::WallRate を使えば実際時間でスリープしてくれると思います。

そのときのセンサデータのスタンプを、現実時間をつけるか、シミュレーション時間をつけるかは ちょっと考える必要があるかと思います。 ros::Time::now でシミュレーション時間、 ros::WallTime::now で実際時間が得られます。

ishiguroJSK commented 8 years ago

ちゃんと用意されてたんですね． 使ってみます．

k-okada commented 8 years ago

シミュレーションのなかでは全ての時刻はシミュレーション時間でないとダメでない？ 実時間を使っていいのは，/clockが出ていないことを３秒間待って確認する，みたいな時ぐらいではないかな．

センサはやはりいつでも1000Hzで回したいので，別スレッドで回す等で対応します．

をしてPRを送ってみましょう．

◉ Kei Okada

On Wed, Aug 17, 2016 at 8:51 PM, Yasuhiro Ishiguro <notifications@github.com

wrote:

ちゃんと用意されてたんですね． 使ってみます．

— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/start-jsk/rtmros_choreonoid/issues/207#issuecomment-240389198, or mute the thread https://github.com/notifications/unsubscribe-auth/AAeG3OY10YU81_yOiKKKJXXjyVWhyPL-ks5qgvXegaJpZM4JmQvo .

ishiguroJSK commented 8 years ago

シミュレーション側の速度や/clockの更新頻度に関わらず，センサ内部は実世界で1000Hzで回したい(内部でフィルタ処理などもするため)という欲求でしたので， センサ内部のループ制御にはros::WallTimeを用いてきっちり1000Hzで回し，パブリッシュの時にはros::Timeでタイムスタンプを付加するのがスレッドを使わず出来るシンプルな書き方かなぁと思っています．

(そもそも論として実世界で計測した波形をシミュレーション空間に指令値として送っている時点でタイムスタンプにあまり意味は無くなってしまっているのですが・・・)

k-okada commented 8 years ago

その場合は，解釈としてシミュレータで動かすのではなくて実機で動かす，ということになるので，具体的にどう出来るかあれなんだけど， hrpsysだと，iobをロボット毎のものではなくて，hrpsys-baseのiobを使って，rosの世界でも/clock が出ていない状況で使うのがいいと思います． choreonoidでどうするかはわかっていないけど，そもそもchoreonoidが必要なのかな？ 少しはソースを直さないといけないかもしれないけど，小手先で今動けばいいやで進めると，どこかで壁にぶつかってそれ以上先にいけない 場面が出てくる気がします．

◉ Kei Okada

2016-08-18 19:22 GMT+09:00 Yasuhiro Ishiguro notifications@github.com:

シミュレーション側の速度や/clockの更新頻度に関わらず，センサ内部は実世界で1000Hzで回したい(内部でフィルタ処 理などもするため)という欲求でしたので， センサ内部のループ制御にはros::WallTimeを用いてきっちり1000Hzで回し，パブリッシュの時にはros::T imeでタイムスタンプを付加するのがスレッドを使わず出来るシンプルな書き方かなぁと思っています．

(そもそも論として実世界で計測した波形をシミュレーション空間に指令値として送っている時点でタイムスタンプにあまり意味は無 くなってしまっているのですが・・・)

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/start-jsk/rtmros_choreonoid/issues/207#issuecomment-240684004, or mute the thread https://github.com/notifications/unsubscribe-auth/AAeG3FxnnS6RZ_LGPyl2jMjl8M1hUFP-ks5qhDJogaJpZM4JmQvo .

ishiguroJSK commented 8 years ago

すいません質問ばかりになってしまうのですが，

rosの世界でも/clock が出ていない状況で使うのがいい

というのは無理してデバイスの駆動ループをros::Timeやros::WallTimeに依存して書くべきでないということですか？

hrpsysだと，iobをロボット毎のものではなくて，hrpsys-baseのiobを使って

実機のiobとシミュレータの(？)iobでも今回の件に近いような話があるのでしょうか？

そもそもchoreonoidが必要なのかな？

ほぼ実時間で実行できていることが大前提ですが，やはり実機実験前の動力学込の安全性チェックとしていまだ手放せないですね・・・(特に僕は何しても派手に転倒するような事ばかりしているので・・・)

k-okada commented 8 years ago

すいません質問ばかりになってしまうのですが，

rosの世界でも/clock が出ていない状況で使うのがいい

というのは無理してデバイスの駆動ループをros::Timeやros::WallTimeに依存して書くべきでないということですか？

ros::Timeに依存するのはいいと思うけど，ros::WallTimeに依存するのは良くないと思います．ということです．

hrpsysだと，iobをロボット毎のものではなくて，hrpsys-baseのiobを使って

実機のiobとシミュレータの(？)iobでも今回の件に近いような話があるのでしょうか？

上の文章はなんか間違っていて，実機はiobの先が実際のロボットのAD/DAにつながっているんだと思いますが，そうではなくて，シミュレータにつながっているようにすればいい，ということでした． で，シミュレータの使い方としては，普通は全力でまわしてその時刻を/clockにだすような使い方をしますが，ここでは，普通のiobと同じように実時間ループとしてiob自体がCPU時間をみて周期ごとに シミュレーションを書ければ良いということです．

そもそもchoreonoidが必要なのかな？

ほぼ実時間で実行できていることが大前提ですが，やはり実機実験前の動力学込の安全性チェックとしていまだ手放せないですね・・・( 特に僕は何しても派手に転倒するような事ばかりしているので・・・)

choreonoidではなくてhrpsysだけでなんとかならないかな？という意味です．であれば，僕でも手伝ったりデバッグ出来ますが，choreonoidだと手伝えないけど，文句だけは言う，ということになります． 皆のリソースは限られているので，使うべきツールはできるだけ絞り込むのが賢いです．

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/start-jsk/rtmros_choreonoid/issues/207#issuecomment-240960192, or mute the thread https://github.com/notifications/unsubscribe-auth/AAeG3BnYQch38Srsev6f1ba-rQGkCEnuks5qhWokgaJpZM4JmQvo .

ishiguroJSK commented 8 years ago

ros::Timeに依存するのはいいと思うけど，ros::WallTimeに依存するのは良くないと思います．ということです．

僕がやや勘違いしていて書き方が不正確だったのですが，(ros::WallTimeとros::WallRateの機能を混同していました)

で，どんなシミュレータや実機につないでもデバイスは1000Hzで駆動し， パブリッシュするタイムスタンプはシミュレータや実機に同期している，ということになりませんでしょうか・・・？

ros::WallRate loop_rate(1000);
while(ros::ok()) {
    //(略)
    pubdata.header.stamp = ros::Time::now();
    pub.publish(pubdata);
    ros::spinOnce();
    loop_rate.sleep();
}

それとも，「どのみち実時間で実行する実機かシミュレータに繋ぐことしか無いのだからros::WallRate::sleep()など使わずともros::Rate::sleep()で正しく1000Hzで回るようになっているべき．」ということでしょうか？

ちなみに

普通のiobと同じように実時間ループとしてiob自体がCPU時間をみて周期ごとに シミュレーションを書ければ良いということです．

この点はマシンパワーが不足しない限りx1.0倍速固定でシミュレーションは進むのでクリアされていると思います．

snozawa commented 8 years ago

なんとなくですが、@ishiguroJSKさんのやりたいことがうまく伝わってないのかもと思いました。

オフラインで話をきいたかんじだと シミュレータのときは 　シミュレータ（ロボット・センサ...etc）<= (iob) => hrpsys <= (OpenRTM) => ROSBridge <= (ROS) => それぞれのROSノード 実機のときは 　実機（ロボット・センサ...etc）<= (iob) => hrpsys <= (OpenRTM) => ROSBridge <= (ROS) => それぞれのROSノード という構造で、@ishiguroJSKのいっている「センサを使いたい」はiobの先のシミュレートされたセンサでなくて、 「それぞれのROSノード」としてうごかす、人間が入力を与える力センサなどのデバイスということだと思ってます。

だとすると、@ishiguroJSKのいっている「センサ」はジョイスティクコントローラと同じROSプログラミングになり、 ジョイスティックコントローラは実機・シミュレータ気にせずどちらでも動いてるような気がするので、 http://wiki.ros.org/ps3joy あたりのソースコードが参考になるか検討してみるといいのではと思いました。

k-okada commented 8 years ago

多分やりたいことは理解できているけど，もうすこし広く世界をみて，この宇宙に流れている時間と，自分のPCのプロセスの中だけで流れている，２つの異なる 時間について感じることができているか，という話をしているつもりです．

実機（ロボット・センサ...etc）<= (iob) => hrpsys <= (OpenRTM) => ROSBridge <= (ROS) => それぞれのROSノード

で，実機の代わりにiobの出力をそのままiobの入力にするのがdummy io (nervousの）だとすると，基本はこのモードで使うべきユ ースケースですね．という話で， iobの出力をdynamics計算機を通してiobの入力につなげたものが必要ではないか，という話です．

シミュレータ（ロボット・センサ...etc）<= (iob) => hrpsys <= (OpenRTM) => ROSBridge <= (ROS) => それぞれのROSノード

の場合は，シミュレーション時間で動くことが想定されていてだから ROS的には/clockをだし，hrpsysでもヘッダにはシ ミュレーション時刻を入れるように していたつもりだけど，この現実空間の時間のながれとは違う空間で動かしていることになって，それが「x1.0倍速固定」してあっても，それはたまたまであって， あくまでもシミュレーションの時間で動いている，という世界になっている，ということで，もしセンサをつなげたければ，そのセンサは現実世界にあるので， 現実の時間の流れを使う必要があるでしょう．ということです．

多分の予想だけど，いまはセンサの読み取りから制御部分までごチャットプログラムがかかれている気がするけど，その部分だけ取り出して， いままでシミュレションとよんでいる世界に入れた時に，ある部分はros::Timeをつかってある部分はros::WallT imeをつかうと破綻するからです．

なので，基本はdummy iob である，つまり use_simtimeは false で /clock もでていない，というのが 実機は使っていないけどセンサを使いたい時の 基本設定になるはずで，その状況の中で dummy iob だけど，入出力の間に力学計算をいれる，という使い方のケースではないか，という意見です．

多分，プログラムとしてはそんなに変わらないんだけど，そこで起こっていること．そこの時間がntpdateで取られてくる時間に同期されているのか， あるいは，その自分で動かしたシミュレーションの中の世界で創りだした時計に同期されているのか，というのは構造として違いますよね，という意見です．

◉ Kei Okada

2016-08-19 18:39 GMT+09:00 Shunichi Nozawa notifications@github.com:

なんとなくですが、@ishiguroJSK https://github.com/ishiguroJSKさんのやりたいこと がうまく伝わってないのかもと思いました。

オフラインで話をきいたかんじだと シミュレータのときは シミュレータ（ロボット・センサ...etc）<= (iob) => hrpsys <= (OpenRTM) => ROSBridge <= (ROS) => それぞれのROSノード 実機のときは 実機（ロボット・センサ...etc）<= (iob) => hrpsys <= (OpenRTM) => ROSBridge <= (ROS) => それぞれのROSノード という構造で、@ishiguroJSK https://github.com/ishiguroJSKのいっている「センサを 使いたい」はiobの先のシミュレートされたセンサでなくて、 「それぞれのROSノード」としてうごかす、人間が入力を与える力センサなどのデバイスということだと思ってます。

だとすると、@ishiguroJSK https://github.com/ishiguroJSKのいっている「センサ」は ジョイスティクコントローラと同じROSプログラミングになり、 ジョイスティックコントローラは実機・シミュレータ気にせずどちらでも動いてるような気がするので、 http://wiki.ros.org/ps3joy あたりのソースコードが参考になるか検討してみるといいのではと思いました。

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/start-jsk/rtmros_choreonoid/issues/207#issuecomment-240974276, or mute the thread https://github.com/notifications/unsubscribe-auth/AAeG3FpiGKdV2Yqbl7Ty7J_7FB9itci3ks5qhXnSgaJpZM4JmQvo .

ishiguroJSK commented 8 years ago

なるほど，徐々に分かってきました． 仰る通りシミュレータ空間内に実世界の波形データを繋げている時点で怪しいというのは自覚があります．

なので，基本はdummy iob である，つまり use_simtimeは false で /clock もでていない，というのが 実機は使っていないけどセンサを使いたい時の 基本設定になるはずで，その状況の中で dummy iob だけど，入出力の間に力学計算をいれる，という使い方のケースではないか，という意見です．

多分，プログラムとしてはそんなに変わらないんだけど，そこで起こっていること．そこの時間がntpdateで取られてくる時間に同期されているのか， あるいは，その自分で動かしたシミュレーションの中の世界で創りだした時計に同期されているのか，というのは構造として違いますよね，という意見です．

実世界のセンサをシミュレータ時間に同期させるのではなく，実世界に対して動力学シミュレータが同期しているべき，というご意見ですね． 確かに同じことをしているようで構造としては別だというのも理解できます． シミュレータ側が実世界クロックによって駆動されるようになっていればこの件でのセンサ時間とシミュレータ時間の齟齬も解消されそうです．

そうなるといろいろとシステムの大改造が必要そうですし，一般的に十分な精度の順動力学計算が実時間で回るのかという問題もありますので一朝一夕では解決できないものではありますが， これからシステムを拡張してゆく際に常に念頭に置いておくべき方針ですね．

k-okada commented 8 years ago

この点はマシンパワーが不足しない限りx1.0倍速固定でシミュレーションは進むのでクリアされていると思います．

が達成されていればサクッと終わる話だと思います．大改造とか，一朝一夕とか，そういう話ではないと思っています．

◉ Kei Okada

2016-08-22 16:56 GMT+09:00 Yasuhiro Ishiguro notifications@github.com:

なるほど，徐々に分かってきました． 仰る通りシミュレータ空間内に実世界の波形データを繋げている時点で怪しいというのは自覚があります．

なので，基本はdummy iob である，つまり use_simtimeは false で /clock もでていない，というのが 実機は使っていないけどセンサを使いたい時の 基本設定になるはずで，その状況の中で dummy iob だけど，入出力の間に力学計算をいれる， という使い方のケースではないか，という意見です．

多分，プログラムとしてはそんなに変わらないんだけど，そこで起こっていること．そこの時間がntpdateで取られてくる時間に同期されているのか， あるいは，その自分で動かしたシミュレーションの中の世界で創りだした時計に同期されているのか，というのは構造として違いますよね，という意見です．

実世界のセンサをシミュレータ時間に同期させるのではなく，実世界に対して動力学シミュレータが同期しているべき，というご意見ですね． 確かに同じことをしているようで構造としては別だというのも理解できます． シミュレータ側が実世界クロックによって駆動されるようになっていればこの件でのセンサ時間とシミュレータ時間の齟齬も解消されそうです．

そうなるといろいろとシステムの大改造が必要そうですし，一般的に十分な精度の順動力学計算が実時間で回るのかという問題 もありますので一朝一夕では解決できないものではありますが， これからシステムを拡張してゆく際に常に念頭に置いておくべき方針ですね．

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/start-jsk/rtmros_choreonoid/issues/207#issuecomment-241338855, or mute the thread https://github.com/notifications/unsubscribe-auth/AAeG3B4DHRQeR39BIj_-p5KkLNxdThnzks5qiVYfgaJpZM4JmQvo .