enable to read custom limb name from .yaml file (fix for #249)

[Naoki-Hiraoka]

https://github.com/jsk-ros-pkg/jsk_model_tools/pull/249 に、

• 複数yamlを読み込む機能
• replace_xmlsなどのキーをlimbと誤って解釈しないようにする修正

を追加しました。

[k-okada]

@pazeshun https://gist.github.com/k-okada/5d1b7bfbfa3f84792ee97cfdb3431eee にdiff を添付します． master のソースツリーに https://github.com/jsk-ros-pkg/jsk_model_tools/pull/250 を加えたもの https://gist.github.com/k-okada/5d1b7bfbfa3f84792ee97cfdb3431eee#file-custom_limb-checkjoint-diff https://github.com/start-jsk/rtmros_common/pull/1130 も加えたもの https://gist.github.com/k-okada/5d1b7bfbfa3f84792ee97cfdb3431eee#file-src-scripts_custom_limb-diff です．タイムスタンプだけでなくて，eusのコードも変わっているけど，そういもの？？？

ロボットモデルの見た目は同じかなと思いました．

[pazeshun]

大変遅くなりました。 @k-okada 自分の手元でもdiffが出てしまうことを確認しました。 @Naoki-Hiraoka このPRとhttps://github.com/start-jsk/rtmros_common/pull/1130 を使ってhironxjsk.lを生成すると、使わない場合と比べて以下のようなdiffが生じます。 https://gist.githubusercontent.com/pazeshun/7952f16ede3aa9229e08a314c60bc030/raw/05858c10d557564a03d519c258cdf686ae5b7a97/hironxjsk_hiraoka_diff.txt linkの宣言順番とかが変わってるみたいでdiffが大きく、チェックが難しいのですが、簡単な修正でdiffが小さくなるようにできたりしないでしょうか。 簡単じゃなければ教えてください。 一応、hironxjskで:start-grasp/:stop-grasp/:angle-vectorができるのは確認したのと、PR2のeusモデルのテストは通るので、最悪モデルファイル変わっていても大丈夫そうではあるのですが・・・。

確認方法

mkdir -p ~/ros/ws_hiraoka_before/src
cd ~/ros/ws_hiraoka_before/src
wstool init
wstool merge https://gist.githubusercontent.com/pazeshun/3c653fae2159ec1372c42f91ff88328e/raw/29db1aff911494ce587fd352a79dd771b7d7a467/ws_hiraoka_before.rosinstall
wstool update
cd ..
catkin build hironx_tutorial

mkdir -p ~/ros/ws_hiraoka_after/src
cd ~/ros/ws_hiraoka_after/src
wstool init
wstool merge https://gist.githubusercontent.com/pazeshun/a6910235b56fb833fd0cd39392618015/raw/38a5330c8420cbd40692e8c44e88b9ca9cf521de/ws_hiraoka_after.rosinstall
wstool update
cd ..
catkin build hironx_tutorial

cd ~/ros
diff -u ws_hiraoka_before/src/rtmros_tutorials/hrpsys_ros_bridge_tutorials/models/hironxjsk.l ws_hiraoka_after/src/rtmros_tutorials/hrpsys_ros_bridge_tutorials/models/hironxjsk.l > ~/hironxjsk_hiraoka_diff.txt

source ~/ros/ws_hiraoka_after/devel/setup.bash
rostest pr2eus make-pr2-model-file-test.launch

[pazeshun]

@Naoki-Hiraoka HIRONXJSK_controller_config.yamlには以下のdiffが出ていて、明らかに間違ってそうなのですが、どうでしょうか。 https://gist.githubusercontent.com/pazeshun/dbc7d2c6a3897c76e4f4dd957b3b9c7d/raw/c3b58149841595541ddb54b59f4ba59a8747bf26/HIRONXJSK_controller_config_hiraoka_diff.txt

確認方法

前のコメントで作った環境で、

cd ~/ros
diff -u ws_hiraoka_before/src/rtmros_tutorials/hrpsys_ros_bridge_tutorials/models/HIRONXJSK_controller_config.yaml ws_hiraoka_after/src/rtmros_tutorials/hrpsys_ros_bridge_tutorials/models/HIRONXJSK_controller_config.yaml > ~/HIRONXJSK_controller_config_hiraoka_diff.txt

[Naoki-Hiraoka]

ご確認くださりありがとうございます。

挙動には影響を与えませんが、今回改めて変更内容を確認したときに気になった箇所をリファクタリングしました。https://github.com/jsk-ros-pkg/jsk_model_tools/pull/250/commits/6cb9aa669a5d168f82dd53e7c98624163570a41b

@Naoki-Hiraoka HIRONXJSK_controller_config.yamlには以下のdiffが出ていて、明らかに間違ってそうなのですが、どうでしょうか。 https://gist.githubusercontent.com/pazeshun/dbc7d2c6a3897c76e4f4dd957b3b9c7d/raw/c3b58149841595541ddb54b59f4ba59a8747bf26/HIRONXJSK_controller_config_hiraoka_diff.txt

間違いでした。https://github.com/start-jsk/rtmros_common/pull/1130/commits/c4aec1b3b36a8c965ba70107817bde93160fa882 で修正しました。

@Naoki-Hiraoka このPRとhttps://github.com/start-jsk/rtmros_common/pull/1130 を使ってhironxjsk.lを生成すると、使わない場合と比べて以下のようなdiffが生じます。 https://gist.githubusercontent.com/pazeshun/7952f16ede3aa9229e08a314c60bc030/raw/05858c10d557564a03d519c258cdf686ae5b7a97/hironxjsk_hiraoka_diff.txt linkの宣言順番とかが変わってるみたいでdiffが大きく、チェックが難しいのですが、簡単な修正でdiffが小さくなるようにできたりしないでしょうか。

行数が多いので全ては見れていませんが、リンク名だけ目視で確認したところ、リンクの宣言順番は変化していますが、全体は変わっていないようです。

このPull Requestで変更した部分では無いのですが、 https://github.com/jsk-ros-pkg/jsk_model_tools/blob/6749740bdb803cdaeb4d453e086b805f05b6c030/euscollada/src/collada2eus_urdfmodel.cpp#L777-L781 や https://github.com/jsk-ros-pkg/jsk_model_tools/blob/6749740bdb803cdaeb4d453e086b805f05b6c030/euscollada/src/collada2eus_urdfmodel.cpp#L1862-L1866 で、linkのshared_ptrをキーとするmapにlinkが格納されていて、mapのイテレータを回したときにどのような順番になっているかによって、linkの宣言順番が決まっています。恐らくmapの順番は、linkのメモリを確保するときにどのアドレスに確保されたかに依存すると思われます。

そのため、今回変更した部分はlinkの宣言順番と直接関係しない部分なのですが、今回変更した部分で行うメモリ確保・開放がlinkのアドレスに何らかの影響を与えて、linkの宣言順番が変化してしまっているのではないかと思います。

今後も何か変更するたびにlinkの宣言順番が変化してしまうのを防ぐためには、shared_ptrをキーとするmapのイテレータを回してlinkを宣言する代わりに、例えばlink名の文字列をキーとするmapのイテレータを回して宣言するという方法に変える、といった方法が考えられます。このPull Requestとはまた別の話になるかと思いますが、この変更を行うPull Requestを出すことは可能です。

[pazeshun]

@Naoki-Hiraoka

そのため、今回変更した部分はlinkの宣言順番と直接関係しない部分なのですが、今回変更した部分で行うメモリ確保・開放がlinkのアドレスに何らかの影響を与えて、linkの宣言順番が変化してしまっているのではないかと思います。

今後も何か変更するたびにlinkの宣言順番が変化してしまうのを防ぐためには、shared_ptrをキーとするmapのイテレータを回してlinkを宣言する代わりに、例えばlink名の文字列をキーとするmapのイテレータを回して宣言するという方法に変える、といった方法が考えられます。このPull Requestとはまた別の話になるかと思いますが、この変更を行うPull Requestを出すことは可能です。

なるほど、宣言順番を固定化するように変更するにしても、今回は順番が変わってしまうことが避けられなさそうな感じですね。 変更しなくていいかと思います。

挙動には影響を与えませんが、今回改めて変更内容を確認したときに気になった箇所をリファクタリングしました。6cb9aa6

@Naoki-Hiraoka HIRONXJSK_controller_config.yamlには以下のdiffが出ていて、明らかに間違ってそうなのですが、どうでしょうか。 https://gist.githubusercontent.com/pazeshun/dbc7d2c6a3897c76e4f4dd957b3b9c7d/raw/c3b58149841595541ddb54b59f4ba59a8747bf26/HIRONXJSK_controller_config_hiraoka_diff.txt

間違いでした。start-jsk/rtmros_common@c4aec1b で修正しました。

@Naoki-Hiraoka このPRとstart-jsk/rtmros_common#1130 を使ってhironxjsk.lを生成すると、使わない場合と比べて以下のようなdiffが生じます。 https://gist.githubusercontent.com/pazeshun/7952f16ede3aa9229e08a314c60bc030/raw/05858c10d557564a03d519c258cdf686ae5b7a97/hironxjsk_hiraoka_diff.txt linkの宣言順番とかが変わってるみたいでdiffが大きく、チェックが難しいのですが、簡単な修正でdiffが小さくなるようにできたりしないでしょうか。

行数が多いので全ては見れていませんが、リンク名だけ目視で確認したところ、リンクの宣言順番は変化していますが、全体は変わっていないようです。

今回の変更後のdiffは以下のようになりました。 hironxjsk.l: https://gist.githubusercontent.com/pazeshun/7952f16ede3aa9229e08a314c60bc030/raw/d6166cf5d9cf6f7faa631c2ed184a76a4135c61a/hironxjsk_hiraoka_diff.txt HIRONXJSK_controller_config.yaml: https://gist.githubusercontent.com/pazeshun/dbc7d2c6a3897c76e4f4dd957b3b9c7d/raw/0fb096c5346f57f7e5eccdcd42f97443d210906f/HIRONXJSK_controller_config_hiraoka_diff.txt

HIRONXJSK_controller_config.yamlについては治ったようで、リンクの宣言順番に関するdiffしかありません。ありがとうございます。 hironxjsk.lについては、全体をざっと見ましたが、日付やファイルパス、リンクの宣言順番に関するdiff以外のdiffは発見できませんでした。 追加で、以下のように、URDFは変わっていないことを確認しました。

diff -u ws_hiraoka_before/src/rtmros_tutorials/hrpsys_ros_bridge_tutorials/models/HIRONXJSK_SENSORS.urdf ws_hiraoka_after/src/rtmros_tutorials/hrpsys_ros_bridge_tutorials/models/HIRONXJSK_SENSORS.urdf

また、Hiro実機に接続されたPC上でこのPRとrtmros_commonのPRが入ったworkspaceを作り、実機で立ち上げるlaunchをそのworkspaceから起動して、正常に起動することと、rviz上でエラーなくロボットモデルや点群が見えること、roseusから:angle-vector/:start-grasp/:stop-graspできることを確認しました。 以上から、恐らく大丈夫であろうと判断します。

なお、以下のPR2のeusモデルのテストも通りました。

source ~/ros/ws_hiraoka_after/devel/setup.bash
rostest pr2eus make-pr2-model-file-test.launch

[k-okada]

このPull Requestとはまた別の話になるかと思いますが、この変更を行うPull Requestを出すことは可能です。

mapの順番が違うから結果がも違う，というのが，案外近い将来に起こりそうなので， URDFDOM_1_0_0_API の場合だけでいいので，対応してたORを作ってください．

https://thispointer.com/stdmap-tutorial-part-2-stdmap-and-external-sorting-criteria-comparator/ でいいのかな． @Naoki-Hiraoka

[Naoki-Hiraoka]

mapの順番が違うから結果がも違う，というのが，案外近い将来に起こりそうなので， URDFDOM_1_0_0_API の場合だけでいいので，対応してたORを作ってください．

https://thispointer.com/stdmap-tutorial-part-2-stdmap-and-external-sorting-criteria-comparator/ でいいのかな．

https://github.com/jsk-ros-pkg/jsk_model_tools/pull/252 に作成しました。変更点が少なかったため、URDFDOM_1_0_0_APIの場合とそうで無い場合の両方を変更しました。