Open Yusuke-Yamasaki-555 opened 1 year ago
Yusuke-Yamasaki-555
commented
1 year ago 他の可視化手段として、matlabでのプロットや、rqtでの描画が挙げられる。
オンラインで情報の可視化をおこなうのであれば、rqtやRviz2が適任だろう。 Logをファイルに吐いて後でプロットするなら、Pythonのmatlpotlibやmatlab、octaveが使える。 matlpotlibならnodeで描画を自動化できる。1sごとなどであれば、オンラインで描画もできるのではないだろうか。
Rviz2を用いる場合は、ロボットのモデルがあったほうが良いと思われ。調べる必要がある。
Yusuke-Yamasaki-555
commented
1 year ago 加速度センサとGyroセンサをグラフにプロットしてみた。
各センサのオフセット（512, acceのzだけ640。センサ値出力から判断）は減算済み。ロボットモデル（proto）の方でセンサの座標が回転していたので、それも修正。
後は、PositionSensorの値からIKを解いて軌道を得られれば、ひとまず可視化したいデータ全て可視化できたことになるかな。
Yusuke-Yamasaki-555
commented
1 year ago リアルタイムにplotしたいなら、やっぱrqtか？rqt-plotとかで。 けどrqtのcustom pluginって書きづらいんだよなぁ。 ただprocessingでやるのは学習コストが高いし時間かかるしオリジナルになっちゃうんど。
Yusuke-Yamasaki-555
commented
1 year ago Yusuke-Yamasaki-555
commented
1 year ago Yusuke-Yamasaki-555
commented
1 year ago rosbagを使う案
まずはオフラインでのlog取りでひとまず良いのではないか、という話。オンラインの恩恵があるのは、シミュレーションや実機での動作時間が長い場合。 加えて、オンラインでのlog表示をしても、最終的にlogを記録しておく必要があるのは変わらない。 優先順位として、オフラインの方法を確率した後に、optionとしてオンラインでの表示を考えたほうが良いかも。
Yusuke-Yamasaki-555
commented
1 year ago あと、Rviz2を使うとなると、Webotsはあんまりかも。URDFとProtoで別れちゃうし。
ただ、ROS2公式のドキュメントも充実してきたり（Rviz2との連携は無いが）、Rviz2との連携で先駆者の日本語での記事がでたりとしているので、できなくは無いだろうなとは思う。
が、やはり現段階でRviz2と相性が良いのはGazeboだと思うし、情報も多いかも。
Yusuke-Yamasaki-555
commented
1 year ago 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 参考サイト：
次は、xacro->urdfの手動変換を挟まずにrviz2に表示することを目指す。
あと、基準座標が胴体にある問題をどうにかする。床を足裏高さに合うようにすればイケるか？
-> Rviz2のGridのOffsetをいじればイケる。Z軸を足裏に合わせれば良い。
Yusuke-Yamasaki-555
commented
1 year ago xacro->urdfの手動変換を挟まずにrviz2に表示することができた。 Launchファイルの書き換えと、既存のxacro内に記述されていた xacro:property をCommentOutすることで対処。
Yusuke-Yamasaki-555
commented
1 year ago あとはこちらから/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
commented
1 year ago /joint_states topicをrqt_plotで可視化してみた様子。 velocityとeffortが空なのは、joint_state_publisher_guiがpositionしかpublishしないから。任意のnodeから任意にpublishするときに速度を指定してやればいい。 このrqt_plotを使えば姿勢とかをプロットできる。まぁ現状、オフラインで記録するようにはしているが、Topicで出すことでrosbag2にも対応できるということで。
で、gazeboはちと沼っているので、まずはRviz2とWebotsの連携に手を付ける（ここまでを発表までに納められればいいかな？）。 方法は比較的簡単で、Webots側で/joint_states topicを読むように書き換え & WalkingPatternGeneratorがPubする型を /joint_states にすればOKという算段。
以上の２つができたら、このissueはRviz2での可視化をもっと掘る方向に進める予定。歩行安定化制御系とアーキテクチャのほうが済んでからになりそうだが。
Yusuke-Yamasaki-555
commented
1 year ago ん、自作nodeからの/joint_states TopicのPubから、Rviz2内のモデルを動かすことに成功。位置と速度を両方変化させることができた。動画では位置の変化のみ。 モデルとTopicが途中ｔｍｔりしている理由はわからんが、QoS周りで変わるかな？
あと、Rviz2から慣性に関するERRORが吐かれている。URDFの問題な希ガス。動いているし、覚えておくがひとまず放置。
Yusuke-Yamasaki-555
commented
1 year ago 自作nodeからの /joint_states Topic Publishが途中で止まる理由、このnodeだけで１スレッドの使用率が100%だからなのでは？
つまり、処理落ちってことか？ プログラムが駄目な気がするな。マルチスレッドで動くようにする必要があるのか？
（一番上のタスクが自作node）
Yusuke-Yamasaki-555
commented
1 year ago やっぱりこれは仕様で、並行処理をさせる必要があるかも。
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
commented
1 year ago ああいや、違うな。100%には張り付かないな。張り付く理由はcreate_wall_timerの周期を極端に短くするとそうなるだけかも。
でも周期を長くしても同様にカクつくので、？
並列処理も不要だと思われ。GUIのほうはGUIとTimerでスレッドを分けてただけっぽい。 やっぱ俺のコードが悪いんだろうな
Yusuke-Yamasaki-555
commented
1 year ago Rviz2とWebotsの連携、joint_state_publisher_guiを用いた方法なら行けた。 いや、自作nodeでもいけるのだが、やはりカクつく。こっちはカクつかない。
自作nodeでカクつくのを改善できたらOKなんだがなぁ。
Yusuke-Yamasaki-555
commented
1 year ago 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側は両脚にのみ指令値出力）
これができたので、あとはWalkingPatternGeneratorからのPubの型を/joint_statesに変更してやれば、一先ず完了ってところかな？
Yusuke-Yamasaki-555
commented
1 year ago 良い、良い感じ。
Rviz2 と Webots を連携させて、動歩行をさせることができた。QoSの設定はDefault（Depthは５）に変更した。 歩行も、自作QoSとmessageでのPub/Subと比較して、安定しているように思える。若干倒れかかるのはParameter調整不足。どれだけのLossがあるかは計測したほうが良いとは思うが、一先ずは良いだろう。
今後のRviz2は、着地位置とか、前進の反映とかをしたいと思う。 Plot周りは、Rqt_plotで。ただPlotしたいものが多いので、Rqt_pluginを作成して自作しても良いだろう。
タイトル通り。ロボットのZMPやCoM、姿勢などをRviz2やProcessingなどで可視化したい。 ロボットのモデルを表示するならURDFへの移行が必要だろうが、各種情報だけなら不要だと思われる。