オドメトリ算出式及びパラメータ(w_rate , gain)の修正

[kei1107]

オドメトリ算出式の修正

src/vmecanum_odom.cppのオドメトリ算出式が誤っていると思われるため以下の修正を加えました。

-    rover_odo.angular.z = (atan(wheel_speed[F_L]/dCenter2Wheel) + atan(wheel_speed[F_R]/dCenter2Wheel) + atan(wheel_speed[R_L]/dCenter2Wheel) + atan(wheel_speed[R_R]/dCenter2Wheel))/4.0;
+    rover_odo.angular.z = (wheel_speed[F_L] + wheel_speed[R_L] + wheel_speed[R_R] + wheel_speed[F_R])/(4.0 * dCenter2Wheel);

参考

• 文献 (式 2.18)
• メカナムホイールを有する他のロボットの計算式 : https://github.com/ridgeback/ridgeback/blob/melodic-devel/ridgeback_control/src/odometry.cpp#L94-L101

w_rateの修正

Gazebo設定ファイル ( urdf/vmecanumrover.xacro )を確認してタイヤ半径（車輪＋バレル込み）が 0.0785であると判断したため、 ソース及びlaunchのw_rateを 0.0785に修正しました。

gainの修正

一時的にgazebo_p3d_pluginを導入し、以下1[m/s]の速度指令値を与えたときのGazebo上での実速度を確認したところ約1.2[m/s]となっておりました。 その為、現在のgain 16.5 を 16.5 * 1.0/1.2 = 13.75に修正しました。　

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

[kei1107]

https://github.com/vstoneofficial/mecanumrover_samples/commit/a0c5f80de205fc41909c0d4d3843e0703c009712 で追修正されているのでcloseします。