vmecanumroverのオドメトリ値がおかしい

atinfinity commented 3 years ago

vmecanumroverが発行するオドメトリ（rover_odo）を確認すると値がおかしいように見受けられます。

再現手順

シミュレータ起動

git clone https://github.com/vstoneofficial/mecanumrover_samples.git
cd ..
catkin_make -DCMAKE_BUILD_TYPE=Release
source devel/setup.bash
roslaunch mecanumrover_samples vmecanumrover.launch

rover_twist配信

rostopic pub /rover_twist geometry_msgs/Twist -r 1 -- '[1.0, 0.0, 0.0]' '[0.0, 0.0, -1.0]'

odom購読

rostopic echo rover_odo
linear: 
  x: 0.839887589216
  y: 0.0212067812681
  z: 0.0
angular: 
  x: 0.0
  y: 0.0
  z: -0.0362106633746
---
linear: 
  x: 0.843559861183
  y: 0.0116153210402
  z: 0.0
angular: 
  x: 0.0
  y: 0.0
  z: -0.0359048156017

参考情報

姉妹プロジェクトのhttps://github.com/vstoneofficial/megarover_samplesだとodomの値は問題なさそうです。

シミュレータ起動

roslaunch megarover_samples vmegarover_with_empty_world.launch

cmd_vel配信

rostopic pub /vmegarover/diff_drive_controller/cmd_vel geometry_msgs/Twist -r 1 -- '[1.0, 0.0, 0.0]' '[0.0, 0.0, 1.0]'

odom購読

rostopic echo /vmegarover/diff_drive_controller/odom
header: 
  seq: 3855
  stamp: 
    secs: 77
    nsecs: 371000000
  frame_id: "odom"
child_frame_id: "base_footprint"
pose: 
  pose: 
    position: 
      x: 0.930948063996
      y: 1.44233480988
      z: 0.0
    orientation: 
      x: 0.0
      y: -0.0
      z: -0.848943057715
      w: -0.528484327826
  covariance: [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.0]
twist: 
  twist: 
    linear: 
      x: 0.999810289086
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 1.00309066901
  covariance: [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.0]

vsfukatsu commented 3 years ago

メカナムローバーのオドメトリは、メカナムローバー Ver 2.1 ROS制御モード取扱説明書の/rover_odoに関する説明(31ページ付近)にある通り、本製品のサンプルパッケージに含まれるノード「pub_odom」にて行われます。
/launch/vmecanumrover.launchを実行後、rosrun mecanumrover pub_odomを実行すると、/odomにオドメトリがpublishされます。

atinfinity commented 3 years ago

@toshimori 説明が不十分で申し訳ありません。

このチケットで言及しているのは実機の系ではなく、Gazeboの系でrover_odoの値がおかしいのではないかということでした。rover_twistとrover_odoのtwistの値に大きな乖離があるのは意図通りでしょうか？

この質問の背景としては、Gazebo環境でvmegaroverを使ってgmappingを動かしてみたところ、車両をキー操作で動かす度にLRFのスキャンと地図がどんどんずれていくという現象がみられていて、その原因を探っていたところrover_odoの結果がおかしいのではないかということがわかりました（gmappingへ入力するodomへの変換はpub_odomで実施することは認識しています）。

vmegarover（メガローバー）ではGazeboから得られるodomをそのまま使っているようですが、vmecanumrover（メカナムローバー）ではホイール回転数をもとにvmecanum_odomでrover_odoを計算しているように見受けられます。

vsfukatsu commented 3 years ago

ご指摘につきまして把握しました。補足いただきありがとうございます。

メガローバーとメカナムローバーのオドメトリの差異につきまして、仕様上統一されておらず混乱を招き申し訳ありませんが、メカナムローバーのシミュレータでは実機がモータのエンコーダから得られた回転数からオドメトリを計算しているフローをそのまま再現しております。
そのため、誤差についてはメガローバーのシミュレータより顕著に見られると思われます。

ただ、ご提示いただいた数値より、実機の挙動からも割と大きめに乖離しておりましたので、こちらで検証いたしました。
/src/vmecanum_odom.cppの10行目をfloat w_rate=0.0625f;に差し替えると、今よりも実機に近い挙動となります(まだ若干の差異はあります)。

atinfinity commented 3 years ago

@toshimori ご提示頂いた仮修正を適用したところlinear.xは近い値になっていること確認できました。一方でangular.zに関しては乖離が大きい問題がまだ残っているようです。

rostopic echo rover_odo
linear: 
  x: 0.994802176952
  y: 0.010379716754
  z: 0.0
angular: 
  x: 0.0
  y: 0.0
  z: -0.0271421035008

atinfinity commented 3 years ago

@toshimori 検証目的ですが指令値通りに動いたと仮定してvmecanum_odom.cppでrover_odoを計算する際stateでなく指令値のcommand値を直接使ってみましたが現状の計算式で得られるangular.zの結果が明らかにおかしいように見受けられるので、角速度の計算式が誤っているのではないと考えています。今の実装の元になっている情報源はありますか？

vsfukatsu commented 3 years ago

検証いただきありがとうございます。angluar.zを含め、実機(Arduino)で車輪のエンコーダ値からgeometry_msgs::Twistの各種数値を変換するロジックをそのまま移植しておりますが、何か反映し忘れている情報などが無いか見直してみます。
上記は、実機をご購入いただいた方であれば、DLいただけるサンプルソースに含まれております。

atinfinity commented 3 years ago

@toshimori 本件進展ありましたらお教え頂ければ幸いです。

vsfukatsu commented 3 years ago

@atinfinity 返答が遅くなり申し訳ありません。原因について調査を進めておりますが、現時点では特定・改善出来ておりません。

根本的な解決ではありませんが、mecanumrover_samples/src/vmecanum_twist.cppの53行目

float mem_com_z = twist_last.angular.z*gain; //[rad/s]

について、

float mem_com_z = twist_last.angular.z*gain*1.25f; //[rad/s]

のように旋回の出力を補強することで、数値上は近い値に補正できます。

nyomaron commented 3 years ago

実機でも同様の現象が見られているように思います．

シミュレータの方では確認していませんが，vmecanum_twist内の float dCenter2Wheel = sqrt(ROVER_D*ROVER_D + ROVER_HB*ROVER_HB); について float dCenter2Wheel = ROVER_D + ROVER_HB; ではないでしょうか？

matsuyama-v commented 3 years ago

@nyomaron ご指摘ありがとうございます。確認しましたところ、 float dCenter2Wheel = ROVER_D + ROVER_HB; が正しい値でした。

制御式に誤りがあり申し訳ございません。実機のライブラリもあわせて更新させて頂きます。

vstoneofficial / mecanumrover_samples