search

start-jsk / rtmros_common

OpenRTM - ROS interoperability packages
http://wiki.ros.org/rtmros_common
12 stars 52 forks source link

[HrpsysSeqStateROSBridge.cpp] センサのTFをtfではなくtf_staticで出したい #1117

Open Naoki-Hiraoka opened 2 years ago

Naoki-Hiraoka commented 2 years ago

現在，センサはURDFに含まれていないためrobot_state_publisherがTFを出してくれないので，代わりに親リンク-センサ間のTFをHrpsysSeqStateROSBridgeがwrlを読んで出しています． https://github.com/start-jsk/rtmros_common/blob/d99980f45f2492d038e4ba47df9e196cd158f7a7/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp#L523

このTFがstaticではなく普通のTFとしてpublishされているので，センサの数が200個近くあるロボットでは，tfをsubscribeする各種nodeの処理が遅くなってしまうという問題がありました．

普通のｔｆではなくtf_staticで出すように変更したいのですが，何か問題はありますでしょうか．

k-okada commented 2 years ago

動かしてテストコード通れば問題ない気がします．

-- ◉ Kei Okada

2022年1月21日(金) 15:27 Naoki Hiraoka @.***>:

現在，センサはURDFに含まれていないためrobot_state_publisherがTFを出してくれないので，代わりに親リンク-センサ間のTFをHrpsysSeqStateROSBridgeがwrlを読んで出しています． https://github.com/start-jsk/rtmros_common/blob/d99980f45f2492d038e4ba47df9e196cd158f7a7/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp#L523

このTFがstaticではなく普通のTFとしてpublishされているので，センサの数が200個近くあるロボットでは，tfをsubscribeする各種nodeの処理が遅くなってしまうという問題がありました．

普通のｔｆではなくtf_staticで出すように変更したいのですが，何か問題はありますでしょうか．

— Reply to this email directly, view it on GitHub, or unsubscribe. You are receiving this because you are subscribed to this thread.Message ID: @.***>

pazeshun commented 2 years ago

tf_staticに変更した場合、rosbag recordしたデータをplayする時に問題があるかもしれません。 tf_staticはlatchedトピックといって、これをechoするsubscriberが生成されるたびに、そのsubscriberが一回だけメッセージを受け取ることができるものですが、この仕組みがrosbag play時には再現されない気がします。 少なくともindigo時代はそうでした。今は直っているかもしれません。 https://answers.ros.org/question/207551/how-to-save-static-transforms-in-bag-files/ tf_staticはplayした直後に一回だけpublishされることになるので、playした後に立ち上げたノードはtf_staticを受け取ることができません。 つまり、起動順番が大変重要になってしまうことになり、rosbag playとその他のノードを一つのlaunchファイルにまとめられないことになります。 で、一般的には、tf_staticを出すのはrobot_state_publisherで、URDFはあるので、rosbag playと一緒にrobot_state_publisherも動かせばいいや、となるのですが、今回はそうはいかないことになります。 デフォルトの挙動を変えるのではなく、どうしても必要な場合のみ指定して変更するなどの対応の方がいいかもわかりません。 急いで書いたので間違いがあるかもしれないので、色々試してみて、良さそうな方法を考えてみてください。

Naoki-Hiraoka commented 2 years ago

ありがとうございます． 確かに，rosbag play時に親リンク-センサ間 のTFを出す手段が無いので，問題があります．

とりあえず今の実験では，wrlからセンサを削除して，代わりにｗｒｌに同名のリンクをfixed jointで取り付けることで，URDFにセンサと同じ名前のリンクを含め，robot_state_publisherからstatic tfを出すという方法をとっています．この方法だとrosbag play時にrobot_state_publisherを立ち上げれば表示されます．しかし，今後wrlを使うレイヤでセンサを使いたい場合に問題になりそうです．

Naoki-Hiraoka commented 1 year ago

jsk_topic_toolsのstatic_tf_republisherか、jsk_rosbag_toolsのtf_static_to_tf.pyを使えば、rosbagの問題は解決できます。

そのため、tf_staticで出して問題なさそうです。今度必要になったときにPRを出します。