ロボットの各種情報を可視化

[Yusuke-Yamasaki-555]

タイトル通り。ロボットのZMPやCoM、姿勢などをRviz2やProcessingなどで可視化したい。
ロボットのモデルを表示するならURDFへの移行が必要だろうが、各種情報だけなら不要だと思われる。

[Yusuke-Yamasaki-555]

他の可視化手段として、matlabでのプロットや、rqtでの描画が挙げられる。

オンラインで情報の可視化をおこなうのであれば、rqtやRviz2が適任だろう。
Logをファイルに吐いて後でプロットするなら、Pythonのmatlpotlibやmatlab、octaveが使える。
matlpotlibならnodeで描画を自動化できる。1sごとなどであれば、オンラインで描画もできるのではないだろうか。

Rviz2を用いる場合は、ロボットのモデルがあったほうが良いと思われ。調べる必要がある。

[Yusuke-Yamasaki-555]

加速度センサとGyroセンサをグラフにプロットしてみた。

各センサのオフセット（512, acceのzだけ640。センサ値出力から判断）は減算済み。ロボットモデル（proto）の方でセンサの座標が回転していたので、それも修正。

• 加速度センサ：z軸基準に、-1.5708[rad]回転
• Gyroセンサ：z軸基準に、-3.1416[rad]回転
  (source: https://github.com/cyberbotics/webots/blob/master/projects/robots/robotis/darwin-op/protos/Darwin-op.proto)
  ただ、lookupTableのセンサ値に対する影響がまだわかっていない。ので、そこは知る必要がある。

後は、PositionSensorの値からIKを解いて軌道を得られれば、ひとまず可視化したいデータ全て可視化できたことになるかな。

[Yusuke-Yamasaki-555]

リアルタイムにplotしたいなら、やっぱrqtか？rqt-plotとかで。
けどrqtのcustom pluginって書きづらいんだよなぁ。
ただprocessingでやるのは学習コストが高いし時間かかるしオリジナルになっちゃうんど。

[Yusuke-Yamasaki-555]

• rviz2を使う場合、webotsでproto, rviz2でurdfに分かれる。
• webots_ros2でpluginを指定するurdfからwebots内ロボットの各センサデータをPubできる。
• rviz2にurdfを読み込ませて各モータに動作命令が送れればおk。tf2が必須ならちょい面倒。
  • joint_state_publisherで使われているjoint_state TopicをPubしてやればRviz2上のモデルも動かせるっぽい。（公式の記事を参考）
• センサデータを表示するためのプラグインを探すなり作るなりすれば良い感じになる。
• rqtでセンサデータのみリアルタイム表示するのもありだが、launchで起動ができないかも。

[Yusuke-Yamasaki-555]

[Yusuke-Yamasaki-555]

rosbagを使う案

• リアルタイムでLogを表示するのも有りだが、rosbagで記録して、後でRviz2でロボットの動作とセンサ情報をまとめて確認できる方法のほうが良いかも。
  • 前例も多いし。

まずはオフラインでのlog取りでひとまず良いのではないか、という話。オンラインの恩恵があるのは、シミュレーションや実機での動作時間が長い場合。
加えて、オンラインでのlog表示をしても、最終的にlogを記録しておく必要があるのは変わらない。
優先順位として、オフラインの方法を確率した後に、optionとしてオンラインでの表示を考えたほうが良いかも。

[Yusuke-Yamasaki-555]

あと、Rviz2を使うとなると、Webotsはあんまりかも。URDFとProtoで別れちゃうし。

ただ、ROS2公式のドキュメントも充実してきたり（Rviz2との連携は無いが）、Rviz2との連携で先駆者の日本語での記事がでたりとしているので、できなくは無いだろうなとは思う。

が、やはり現段階でRviz2と相性が良いのはGazeboだと思うし、情報も多いかも。

[Yusuke-Yamasaki-555]

rviz2への表示ができた。
ROBOTIS公式GitHubで公開されているxacroモデルをURDFに変換、package.xmlとCMakeLists.txtとLaunchファイルをROS 2化することで達成。
公式xacroモデルのリポジトリはlicense Apache2.0で公開されているので、改変と再配布は許されるはず（要再確認）。

公式xacroモデル：https://github.com/ROBOTIS-GIT/ROBOTIS-OP2-Common
参考サイト：

• https://nutritionfoodtech.com/2022/10/14/urdf%E3%82%92rviz2%E3%81%A8gazebo%E3%81%A7%E8%A1%A8%E7%A4%BA%E3%81%99%E3%82%8B-ros2-foxy/#toc3
• https://zenn.dev/tasada038/articles/940ef193a4a28a

次は、xacro->urdfの手動変換を挟まずにrviz2に表示することを目指す。
あと、基準座標が胴体にある問題をどうにかする。床を足裏高さに合うようにすればイケるか？
-> Rviz2のGridのOffsetをいじればイケる。Z軸を足裏に合わせれば良い。

[Yusuke-Yamasaki-555]

xacro->urdfの手動変換を挟まずにrviz2に表示することができた。
Launchファイルの書き換えと、既存のxacro内に記述されていた xacro:property をCommentOutすることで対処。

[Yusuke-Yamasaki-555]

あとはこちらから/joint_state Topicを出してやればRviz2内のモデルが動く希ガス。で、WebotsもGazeboもそのTopicを読むようにすれば一番キレイになる。これに関しては確証は無いが。

以下、launch起動中にros2 topic echo /joint_stateした時の、1回分の/joint_state。

---
header:
  stamp:
    sec: 1692959222
    nanosec: 727243100
  frame_id: ''
name:
- head_pan
- head_tilt
- l_sho_pitch
- l_sho_roll
- l_el
- r_sho_pitch
- r_sho_roll
- r_el
- l_hip_yaw
- l_hip_roll
- l_hip_pitch
- l_knee
- l_ank_pitch
- l_ank_roll
- r_hip_yaw
- r_hip_roll
- r_hip_pitch
- r_knee
- r_ank_pitch
- r_ank_roll
position:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
velocity: []
effort: []
---

ここでの注意点は、Webots（というよりproto）での関節名と全く異なり、関節の名前の順番も異なるということ。Webots_ros2 Plugin側で対応させる必要がある。
Gazeboは同じxacroを読ませるので、特に問題はない。

[Yusuke-Yamasaki-555]

/joint_states topicをrqt_plotで可視化してみた様子。
velocityとeffortが空なのは、joint_state_publisher_guiがpositionしかpublishしないから。任意のnodeから任意にpublishするときに速度を指定してやればいい。
このrqt_plotを使えば姿勢とかをプロットできる。まぁ現状、オフラインで記録するようにはしているが、Topicで出すことでrosbag2にも対応できるということで。

rviz2_jointstate_rqtplot.mp4

で、gazeboはちと沼っているので、まずはRviz2とWebotsの連携に手を付ける（ここまでを発表までに納められればいいかな？）。
方法は比較的簡単で、Webots側で/joint_states topicを読むように書き換え & WalkingPatternGeneratorがPubする型を /joint_states にすればOKという算段。

TODO:

• Rviz2とWebotsの連携
• Topic通信時のQoSの見直し（TopicのLoss問題に対処）
• 関節角度の観測値よりIKを解いて重心軌道を得てPlot

TODO以降

以上の２つができたら、このissueはRviz2での可視化をもっと掘る方向に進める予定。歩行安定化制御系とアーキテクチャのほうが済んでからになりそうだが。

[Yusuke-Yamasaki-555]

ん、自作nodeからの/joint_states TopicのPubから、Rviz2内のモデルを動かすことに成功。位置と速度を両方変化させることができた。動画では位置の変化のみ。
モデルとTopicが途中ｔｍｔりしている理由はわからんが、QoS周りで変わるかな？

あと、Rviz2から慣性に関するERRORが吐かれている。URDFの問題な希ガス。動いているし、覚えておくがひとまず放置。

rviz2_jointstate_from-node.mp4

[Yusuke-Yamasaki-555]

自作nodeからの /joint_states Topic Publishが途中で止まる理由、このnodeだけで１スレッドの使用率が100%だからなのでは？

つまり、処理落ちってことか？
プログラムが駄目な気がするな。マルチスレッドで動くようにする必要があるのか？

（一番上のタスクが自作node）

[Yusuke-Yamasaki-555]

やっぱりこれは仕様で、並行処理をさせる必要があるかも。

joint_state_publisher_gui (/joint_states topic をPubするGUIインタフェ) はカクつかずに動かせるのだが、これの実装では、並行処理をさせている。（source codeの288行目）
https://github.com/ros/joint_state_publisher/blob/ros2/joint_state_publisher_gui/joint_state_publisher_gui/joint_state_publisher_gui.py

参考：Pythonでの並行処理：https://qiita.com/studio_haneya/items/a3485ea837e17e37bae9

C++での並列処理：https://qiita.com/birdwatcher/items/2d89374782a56be04aa1
並列処理と並行処理は違う、はず？
まあ１つのプロセスが複数のスレッドを使ってくれるなら何でも良いや

[Yusuke-Yamasaki-555]

ああいや、違うな。100%には張り付かないな。張り付く理由はcreate_wall_timerの周期を極端に短くするとそうなるだけかも。

でも周期を長くしても同様にカクつくので、？

並列処理も不要だと思われ。GUIのほうはGUIとTimerでスレッドを分けてただけっぽい。
やっぱ俺のコードが悪いんだろうな

[Yusuke-Yamasaki-555]

Rviz2とWebotsの連携、joint_state_publisher_guiを用いた方法なら行けた。
いや、自作nodeでもいけるのだが、やはりカクつく。こっちはカクつかない。

rviz2_jointstate_webots_gui.mp4

自作nodeでカクつくのを改善できたらOKなんだがなぁ。

[Yusuke-Yamasaki-555]

OKOK、カクつくBUG、修正完了。

原因は、/joint_states の header.stamp への値の与え方が駄目だった。

auto now_time = rclcpp::Clock().now();
// pub_msgs_->header.stamp.sec = now_time.seconds();  // BAD
// pub_msgs_->header.stamp.nanosec = now_time.nanoseconds();  // BAD
pub_msgs_->header.stamp = now_time;  // GOOD

ココを修正するだけで、以下のように自作nodeからでもカクつかずに動いてくれるようになった。（全関節角度・角速度一定。Webots側は両脚にのみ指令値出力）

rviz2_jointstate_webots_node.mp4

これができたので、あとはWalkingPatternGeneratorからのPubの型を/joint_statesに変更してやれば、一先ず完了ってところかな？

[Yusuke-Yamasaki-555]

良い、良い感じ。

Rviz2 と Webots を連携させて、動歩行をさせることができた。QoSの設定はDefault（Depthは５）に変更した。
歩行も、自作QoSとmessageでのPub/Subと比較して、安定しているように思える。若干倒れかかるのはParameter調整不足。どれだけのLossがあるかは計測したほうが良いとは思うが、一先ずは良いだろう。

rviz2_jointstate_webots_dynamic-walking.mp4

今後のRviz2は、着地位置とか、前進の反映とかをしたいと思う。
Plot周りは、Rqt_plotで。ただPlotしたいものが多いので、Rqt_pluginを作成して自作しても良いだろう。