[hrpsys_ros_bridge/HrpsysSeqStateROSBridgeImpl.cpp]fix cop_link_info

[Naoki-Hiraoka]

HrpsysSeqStateROSBridgeからROSにpublishされるトピックrfsensor_cop lfsensor_copのframe_idが、トピックで送られる圧力中心の座標系と異なっています。

トピックで送られる圧力中心は、各limbのend_coords系で表現されており、Stabilizerで算出されています。
（m_COPInfo）
https://github.com/fkanehiro/hrpsys-base/blob/181013726aafbaa05c9d4a1c0089d613c37bb048/rtc/Stabilizer/Stabilizer.cpp#L1267-L1306

一方で、frame_idには各6軸センサが実際に取り付けられているリンクの名前が入っています。

rtmros_common/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp

Line 840 in 0beacf4

copv.header.frame_id = cop_link_info[tmpname].link_name;

rtmros_common/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp

Line 347 in 0beacf4

ci.link_name = sensor_info[sensor_name].link_name; // Link name for tf frame

rtmros_common/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp

Line 201 in 0beacf4

si.link_name = links[k].segments[0].name;

この結果、frame_idを参照してtfで変換しcopを描画するjsk-ros-pkg/jsk_control/jsk_footstep_controllerのfootstep_visualizer.pyで、CoPが後方にずれて描画されてしまう問題が発生しています。

frame_idを圧力中心の座標系に合わせてrleg_end_coords、lleg_end_coordsに変更しました。

[k-kimura]

HRP2JSK（7号機）実機でもfootstep_visualizerのCoP，ZMPの位置が正しく表示されていることを確認しました．

[k-kimura]

PRのタイトルは
[hrpsys_ros_bridge/HrpsysSeqStateROSBridge.cpp]
となっていますが，変更したファイルは
HrpsysSeqStateROSBridgeImpl.cpp
でしょうか．

[Naoki-Hiraoka]

PRのタイトル及びcommitメッセージをHrpsysSeqStateROSBridgeImpl.cppに修正しました。

HRP2JSKNTS(17号機)でfootstep_visualizerのCoP，ZMPの位置が正しく表示されていることを確認しました．

[Naoki-Hiraoka]

end-coordsの扱いについて、
#585
と関係します

[YoheiKakiuchi]

直し方はこれでいいと思いますが、せっかくなのでソースにコメント入れましょう。

https://github.com/fkanehiro/hrpsys-base/blob/aa0d514d4e33ba1ac1f5bd914b2b6ce5d61994f6/rtc/Stabilizer/Stabilizer.cpp#L1289-L1295
ここの、
// ee-local total moment and total force at ee position
このコメントより、データはend-effector のlocal で表されていると分かる。

[Naoki-Hiraoka]

hrpsysではエンドエフェクタの位置を動的に変更できるようにしてあり https://github.com/fkanehiro/hrpsys-base/blob/dd2cc153a536e6741afc6b6f642c0d87459ecd3d/idl/AutoBalancerService.idl#L317 https://github.com/fkanehiro/hrpsys-base/blob/dd2cc153a536e6741afc6b6f642c0d87459ecd3d/idl/StabilizerService.idl#L221 JAXONの4つ足歩行時に変更されていた実績があります。

ところが、ROSのレイヤでは、end_coordsは、現状はyamlに書いた座標をURDFに書き込み、robot_state_publisherからtfを出す仕様になっているため、位置を動的に変更することができません。#585

ROSのレイヤでもend_coordsの位置を動的に変更できないと、このPRは破綻してしまうため、容易には解決できず3年近く間放置してしまっておりました