octomapがRviz上でうまく確認できない問題について

[nishida-lab]

octomapがRviz上でうまく確認できない問題について

[MoriKen254]

すみません、octomapが更新されない件、もう少し教えていただけますか？私が理解できておらず^^;

当方の環境だと、RvizのPointCould2で /kinect2/hd/points を可視化すると、下記の状況でその情報がちゃんと更新されているようなのです。

• ロボットを動かしたとき
  • points がロボットの追従する
• gazeboで新しい物体をkinectの視野内に入れたとき（台の外、台の上どちらでも）
  • 新しい物体が存在するところにpointsが出現する

これ以外の現象を観察して、更新されていないという問題が発生しているのでしょうか？

それとも、そもそもoctomapとは、/kinect2/hd/pointsとは異なるものを指しており、その情報を見て更新されていないという問題が発生するのでしょうか？もしそうである場合、どのような操作をすればRviz上で確認できるか教えていただけますか？

すみませんが、よろしくお願いします。

[RyodoTanaka]

@mum254
お返事が遅くなり、申し訳ありません。
現在問題になっているのは、/kinect2/hd/pointsではなく、Moveit!をRviz上で立ち上げた時に出現するボクセル状グリッド（Octomap）が更新されないという点です。
つまり、

octomapとは、/kinect2/hd/pointsとは異なるものを指しており、その情報を見て更新されていないという問題が発生する

ということです。
この症状を確認する方法は以下の通りです。

1. sia5 + kinect のGazeboを起動
2. pnanning_executionの立ち上げ

このとき表示されるボクセルグリッドが、/kinect2/hd/pointsと同じように見た目には更新されないということです。

以上、よろしくお願いします。

[MoriKen254]

@RyodoTanaka
ありがとうございます。助かります。ちなみに、下記の手順で良いですか？
roslaunch motoman_gazebo sia5_nishida_lab.launch
roslaunch motoman_sia5_moveit_config moveit_planning_execution.launch

それから、planning_executionとは、何を指しますか？ move_groupノードを起動した時に有効になる何かと解釈してよいでしょうか？ ※すみません、探してみたのですが、見つけられませんでした。

また、Rvizで表示させるためには、具体的にどの項目にチェックを入れればよいでしょうか？デフォルトの設定だと、/kinect2/hd/pointsしか表示されないようで^^;

よろしくお願いします。

[RyodoTanaka]

@mum254
わかりづらい説明、失礼しました。
起動の手順についてご説明します。

1. roslaunch motoman_gazebo sia5_nishida_lab.launch
2. roslaunch motoman_moveit sia5_nishida_lab_moveit_planning_execution.launch

以上の操作で、以下のような状況をRvizで確認できるはずです。
この時に表示されるボクセルグリッド（octomap）が、/kinect2/hd/pointsと同じように更新されなくて困っている、というのが現在の問題です。よろしくお願いします。

[RyodoTanaka]

@mum254
先ほどの情報に追加ですが、
最新のmasterブランチにて、
sia5 + kinectv2 のみのモデルも追加しました。
以下の手順でloanchでき、このモデルでも同様の問題が確認できます。
また、こちらのモデルの方が軽いので、もしよければこちらでご確認ください。
以下に手順と動作画面を示します。

1. roslaunch motoman_gazebo sia5_with_kinectv2.launch
2. roslaunch motoman_moveit sia5_with_kinectv2_moveit_planning_execution.launch
   上記コマンドで、以下のような状況が確認できるはずです。
   
   以上、よろしくお願いします。

[Ry0]

Learning_ROS_for_Robotics_Programming_2nd_editionで正しくOctomapの更新が行われている動画です．この状況をどうしてもmotomanなど自分たちのところで再現できていません．
https://youtu.be/muTJ-VZzkY8
move_groupノードを立ち上げたあとに物体を追加しても，きちんと更新されています．

[RyodoTanaka]

@mum254
@Ry0

動きました！！！！！！！！！！

まずは、動いた様子をアップロードしましたので、こちらからご覧ください。

原因

Rviz上のMotionPlanningプラグインにおいて、Planning Scene Topicという項目があるのですが、このトピック名が/move_group/monitored_planning_sceneになっていないことが原因でした。因みに、うまく動かなかった時のトピック名は/planning_sceneです。
これにより、

1. Octomapの更新ができるようになった。
2. Currentが更新されるようになった。
   上記二つの問題が一度に解決されました！
   なので、本issueの最初で述べていた、

また、この症状は、motomanだけcurrent姿勢が更新されないことに酷似していると感じます。
故に、原因はmoveitのvisualisationとmotomanに何か齟齬がある為だと考えられます。

という推理はあたっていたということです。

なぜ、原因究明に手間取ったのか

今回の原因であったトピックですが、/planning_sceneトピックには、情報が入っていないわけではなく、MotionPlanningプラグインが起動した時のScene情報が入っており、それが原因で発見が遅れてしまいました。

おわりに

問題が完全に解決しました！これでROSBOOK_ARMと同じ環境を手に入れることができたので、次はMotomanに平行チャックを付けた時と、ジャミンググリッパ（プラグインを書く必要あり）を付けた時で色々と比べてみたいと思います。
ご協力ありがとうございました！

[MoriKen254]

@RyodoTanaka
ありがとうございます！こちらでも確認しました！
いやー，私も今日帰ってからずっとこれやっててさっぱりだったんです．すっきりしました！

これは設定のミスなんじゃないかって，launch ファイルやyaml を全部ひっくり返して見てましたが，間違いが見つからないはずです．

ファイルのミスが見当たらないので，見た目側が問題なのかなーとか思いつつ，ちょうど下記ように整理していたとろで，更新がかかりました．せっかくここまで書いたので，情報共有のために投下しておきますね．^^;

• octmap自体は更新されていて，表示だけされてないということは分かった．
• Rviz上のMotion PlanningのStatusタブでrosbook_armではChanged goal stateが更新されているのに，motomanでは更新されていないことがおかしいと思った．
• rostopic list | grep plan で出てきたトピックが怪しそうなので，rosbook_armとmotomanそれぞれについて，rostopic echo をやって違いを見つけようとした．
• rostopic echo /move_group/planning_scene_monitor/parameter_updates をすると，publish_planning_scene_hzの値が4.0となっており，rosbook_armでは確かにコレくらいの周期で「StatusタブのChanged goal state」が更新されているのに，motomanでは更新されていない．
  • planninig_sceneをpublishしている先がどこかにあるはずで，ここの不一致が疑われるが，これはどこなんだろうなぁ？

てな感じで，私は迷子になっていました^^;　お見事です！

この辺り，仕様のドキュメントがないんですかね．google group でも相当悩んでるっぽかったんで，意外とみんな気づいていないかもしれません．
https://groups.google.com/forum/#!topic/moveit-users/MFXZWp7_iSQ

とにかく，とても勉強になりました！

[RyodoTanaka]

@mum254
ありがとうございます！
私も、全部ひっくり返して確認していたのですが、何もわからず、途方にくれていました！
いずれにせよ、良かったです。
また、デバッグ手法の共有、ありがとうございます。
勉強になりました。

@mum254
@Ry0
ということで、本件は解決したので、閉じたいと思います。
ありがとうございました！