ROS制御のレゴ・メカナム・ホイール・カー (2) ソフト編

SENSYN ROBOTICS（センシンロボティクス） Advent Calendar 2019 の 19日目 担当の @nishimoto_ です。

ROS制御のレゴ・メカナム・ホイール・カー (1) 組み立て編の後編のソフト編です。

はじめに

後編ではソフトウェアの設定や記述をします。動いているところを見たい方は、ページの最後までスクロールしましょう。

ラズパイへのOSの導入、ROSのインストール、モータードライバーの制御、PS3コントローラとの接続、シャットダウンスイッチの設定、という内容になっております。

仕様 & 目標

PS3ゲームコントローラで操縦できるラジコンを作ります。すべての制御はROS(バージョンはkinetic)を使います。

Raspberry PI3の環境設定

UbuntuMate

Raspberry PI 3 B+ に Ubuntu Mateを入れて立ち上げます。手順はRaspberry Pi 3B+(Ubuntu Mate 16.04LTS)にROS kinetic をインストールしてturtlesimを動かすの通りです。ラズパイ用設定済みのimgがリンク先にあるので、ダウンロードしてSDカードに焼けば完了です。

最初はRasbianでROSを入れて動かすことをトライしていましたが、数々の障害にくじけてしまいました。Ubuntu最高。

ROSのインストール

ROSの公式Wiki Ubuntu install of ROS Kinetic の通りインストールします。OSがUbuntuなのでスムーズに進むはずです。

PS3コントローラの設定

PS3コントローラの無線接続

Joystick(PS3) - FaBo DonkeyCar Docs -の最初から [PLAYSTATION(R)3 Controller]# quit まで実施すれば繋がります。上手くいかないときはコントローラのリセットボタンを押しましょう。

接続が上手くいったかの動作確認はPS3コントローラ (DUALSHOCK3; SIXAXIS)をROSに接続する（無線編） - Qiita - の「動作確認」を実施しましょう。

PS3を接続するとマウスポインターが動いてしまう対策

接続に成功すると画面のマウスポインタが突然動き始めます。そのままでは作業ができないので、【Ubuntu】ジョイスティック(PS3コントローラ)がマウスとして認識されてしまう問題 - 極楽とんぼのロボット製作記 - の手順で解決しましょう。私はこの xinput set-prop 'Sony PLAYSTATION(R)3 Controller' 'Device Enabled' 0 コマンドをエイリアスに登録し、すぐに打てるようにしました。

PS3コントローラのアサインの表

どのボタンやスティックの変化がどの配列にアサインされているかは、ROS講座07 joyプログラム - Qiita - を見ました。

サーボ・モータ・ドライバを動かすための準備

I2Cを有効にする

sudo raspi-config でi2cをenableに設定します。その後、 i2cdetect -y 1 で認識されていればOKです。

I2Cに接続したPCA9685を制御するROSパッケージのインストール

GitHub - dheera/ros-pwm-pca9685: ROS package for PCA9685 16-channel PWM driver, used in motor and LED applications -で git clone と catkin_make しましょう。

メカナム・ホイール・カーの動作原理と実装

大きな車輪の外側に複数の小さな車輪が自由に回転するように、かつ斜め45°に取り付けられたメカナム・ホイール、これを搭載した車両を思い通りに動かすには特別な計算式が必要です。

Drive Kinematics: Skid Steer & Mecanum (ROS Twist included) - Robots For Roboticists - にかかれている式を用いて、以下のように実装しました。

mecanum_base_driver.h (/mecanum_base_driverノード)

#define DISTANCE_LEFT_TO_RIGHT_WHEEL 0.195  // 左右の車輪の間隔
#define DISTANCE_FRONT_TO_REAR_WHEEL 0.15  // 前後の車輪の間隔
#define WHEEL_SEPARATION_WIDTH DISTANCE_LEFT_TO_RIGHT_WHEEL / 2
#define WHEEL_SEPARATION_LENGTH DISTANCE_FRONT_TO_REAR_WHEEL / 2
#define WHEEL_RADIUS 0.032  // 車輪の半径
#define INV_WHEEL_RADIUS 31.25  // 1/(64/2/1000)=31.25。float計算対策
#define WHEEL_SEPARATION_PARAM 0.1725  // (195/1000)/2+(150/1000)/2 = 0.1725

#define ROTATION_GAIN 5

mecanum_base_driver.cpp (/mecanum_base_driverノード)

float wheel_front_left  = INV_WHEEL_RADIUS * (msg.linear.x - msg.linear.y - WHEEL_SEPARATION_PARAM * msg.angular.z * ROTATION_GAIN);
float wheel_front_right = INV_WHEEL_RADIUS * (msg.linear.x + msg.linear.y + WHEEL_SEPARATION_PARAM * msg.angular.z * ROTATION_GAIN);
float wheel_rear_left   = INV_WHEEL_RADIUS * (msg.linear.x + msg.linear.y - WHEEL_SEPARATION_PARAM * msg.angular.z * ROTATION_GAIN);
float wheel_rear_right  = INV_WHEEL_RADIUS * (msg.linear.x - msg.linear.y + WHEEL_SEPARATION_PARAM * msg.angular.z * ROTATION_GAIN);

ROTATION_GAINの係数は元ネタのサイトにはありません。Joystickの命令と実際の動きを見て、前後左右の平行移動と回転の移動のバランスを取ります。

このmecanum_base_driverノードでは、サーボ・モータ・ドライバのノードにモータの回転速度をpublishしますが、その際に右側のモータ2個の回転方向を反転させておきます。

cmd_.data[0] = (int)(total_gain * wheel_front_right * (-1) + OFFSET);  // front right servo
cmd_.data[1] = (int)(total_gain * wheel_rear_right * (-1) + OFFSET);  // rear right servo
cmd_.data[2] = (int)(total_gain * wheel_rear_left + OFFSET);  // rear left servo
cmd_.data[3] = (int)(total_gain * wheel_front_left + OFFSET);  // front left servo

total_gainはlaunchファイル内のros paramにてゲイン係数を設定しています。実際の動きを見て調整しましょう。

RosのNode

ノードはこのように構成しました。

/joy_nodeはROS標準で入っているジョイスティックの操作を検出するパッケージです。/joyトピックのうちスティック操作量を自作の/joy_cmd_publisherノードが/cmd_vel(twist型)トピックに変換して出力します。それを/mecanum_base_driverノードがサーボ制御コマンド/commandに変換し、/pca9685_nodeに渡すと動きます。

/mecanum_base_driverノードが/joy_nodeから直接/joyトピックを受け取っていますが、これは△や□のボタンを押したときに特別な動きをするためです。

∞マーク走行

DJIのRobomaster S1のデモにて∞マークのかたちで走る様子が見れるのですが、それを実現しました。ジョイステックをぐるっと1周回し、反対方向にもう1周回す、という動作を自動で繰り返します。

mecanum_base_driver.h (/mecanum_base_driverノード)

#define PI 3.1415926535
#define INFINITY_DEVIDE 250.0  // 5sec * 50Hz

mecanum_base_driver.cpp (/mecanum_base_driverノード)

  double rad = (double)((int)infinity_rotaion_tick_ % (int)INFINITY_DEVIDE) * 2 * PI / INFINITY_DEVIDE;
  float linear_x;
  float linear_y;

  if(((int)infinity_rotaion_tick_ / (int)INFINITY_DEVIDE) % 2 == 0){
    linear_x = (float)sin(rad + PI/2);
    linear_y = (float)cos(rad + PI/2);
  }
  else{
    linear_x = (float)sin(-1 * rad + PI/2);
    linear_y = (float)cos(-1 * rad + PI/2);
  }

  float wheel_front_left  = INV_WHEEL_RADIUS * (linear_x - linear_y);
  float wheel_front_right = INV_WHEEL_RADIUS * (linear_x + linear_y);
  float wheel_rear_left   = INV_WHEEL_RADIUS * (linear_x + linear_y);
  float wheel_rear_right  = INV_WHEEL_RADIUS * (linear_x - linear_y);

  infinity_rotaion_tick_++;

シャットダウン・スイッチの設定

ラズパイでシャットダウンボタンを付ける(ついでに起動ボタン) - Qiita - を参考に、23番PINとGNDに接続したスイッチを押せばシステムをシャットダウンしてくれるように設定しました。

この記事の通りに設定したのですが、1点、サービスファイルを作成するときに ExecStart =/usr/bin/python3.5 /home/pi/pyhome/shutdownd.py と書かないといけないので注意です。

完成

左ジョイスティックの操作で、どの方向でも移動できる：

右ジョイスティックの操作で、回転(超信地旋回)：

左右ジョイスティックの操作を組み合わせて、奇妙な踊り：

∞マーク走行： △ボタンを押すだけで、自動走行です。

おわりに

ラジコン操縦できるロボットを作ってみました。メカナム・ホイールの操縦は楽しいですね。

作った車両はtwist型の速度命令を受信すれば動くものですので、turtlebotのようにどんどんROS上で拡張していくことができます。また、メカ部分はすべてレゴで作っていますので形を自由に変えたり、好きなものを載せてどんどん拡張することができます。ゆえに個人的には、 レゴxROS は最強のロボットプログラミングの学習環境だと思います。