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

[Naoki-Hiraoka]

現在，センサはURDFに含まれていないためrobot_state_publisherがTFを出してくれないので，代わりに親リンク-センサ間のTFをHrpsysSeqStateROSBridgeがwrlを読んで出しています．

rtmros_common/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp

Line 523 in d99980f

updateSensorTransform(sensor_tf_stamp); // transform depends on joint angles, not sensor values

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

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

[k-okada]

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

…

-- ◉ 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]

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]

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

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

[Naoki-Hiraoka]

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

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