2023年11月24日〜12月1日に行われたAmazonブラックフライデーセールにて，RoboMaster S1にそっくりなラジコンが激安で販売されていることが𝕏（旧Twitter）上で話題になっていました．
このビッグウェーブに乗るしかないと思い，私も即ポチりました．
わずか3,000円でサスペンション付きのメカナム足回りが手に入ると思うと驚きです．
例のアレ(とDUALSHOCK4)買いました
— Akinosuke Tsutsumi (@akki_nitkk) November 30, 2023
このロボット3000円ってマジ？ pic.twitter.com/cFAyRUA7ZV
注文した2日後には届いたので早速組み立てを行いました．
しかし，このあと学会やロボコンの大会で忙しく1ヶ月ほど寝かしてしまい...
年末にようやく時間を作って動かすことができました．
ラジコン制御
今回はROS2とmicro-ROSを用いて足回りを動かしてみました．
上物も動かしたかったのですが，回路を取り付ける際に煩わしかったので一旦取り外しました．
使用したマイコンはM5Stack Core2です．
マイコンに給電するモバイルバッテリーを搭載するスペースを確保するのが面倒だったため，バッテリー内蔵でmicro-ROSも使えるこちらを使用しました．
始めは「RaspberryPi Zero 2W」でモータを駆動しつつカメラ積んで画像処理などしようかと考えていたのですが，そもそもROS2がスムーズに動いてくれるか怪しかったため，将来的にミニPCを搭載することにして採用を見送りました．
ソースコードはこちらです．
PlatformIOのプロジェクトになっていますのでメインの処理はsrc/main.cppに記述されています．
micro-ROS関連の処理にはコメントを付けていますのでそちらを見れば何をしているかは理解できるかと思います．
本記事では制御システム概要ともう少し細かい部分を解説していきます．
今回はjoy_nodeから/joyをパブリッシュし，teleop_twist_joyノードで/cmd_velメッセージに変換してからmicro-ROS経由でマイコン側のradicon_nodeに指令を送信しています．
なぜわざわざ変換する処理を挟んでいるかというと，/joyメッセージはsensor_msgs/Joy型ですが，この型はメンバとして動的なサイズのVectorを持つため，マイコン側で受信することが難しいからです．
一応，micro-ROS公式にはSequence型として動的なサイズのメッセージを受信する方法が紹介されていますが，私は正常に受信できたことがありません...
そこで動的なサイズのメンバを持たないgeometry_msgs/Twist型の/cmd_velメッセージに変換してから送ることでこの問題を回避しています．
変換ノードを記述するのが面倒なのでteleop_twist_joyを利用していますが，もちろん自前で用意してもいいです．
...と言うよりは，teleop_twist_joyはOPTIONボタンかPSボタンを押しながらでないとジョイスティックの入力が反映されない仕様のため，自前で用意して好みの仕様にしたほうがいいです．（単純なノードになるためROS2初学者の方は作ってみるといい勉強になるかと思います）
始めに，setup関数で実行しているピンの初期化やmicro-ROSのセットアップについて注意点だけ見ていきます．
int pwm_pin[8] = {14, 26, 25, 13, 32, 27, 19, 33};
void setup(void) {
/*中略(Lcd等のセットアップ)*/
// PWM波生成に使用するタイマーのセットアップとピン割当て
for(int i = 0; i < 8; i++) {
ledcSetup(i, 20000, 8); // i番チャンネルを20kHz・分解能8bitで初期化
ledcAttachPin(pwm_pin[i], i); // i番チャンネルに配列pwm_pinのi番目のピンを割当て
ledcWrite(i, 0); // モータを停止させる
}
// micro-ROSをセットアップする関数
// Wi-FI経由で通信する際の引数はSSID, パスワード, 接続先のPCのIPアドレス, ポート番号(適当でOK)
set_microros_wifi_transports("SSID", "PASSWORD", "IP_ADDRESS", 2000);
/*中略(micro-ROSのセットアップ)*/
// executorを作成
// 引数は executorの構造体, サポートのコンテクスト, ハンドル数, allocator
// ハンドル数：マイコンで処理するtopic, service, timerなどのコールバックの数
RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
// executorにsubscriptionを追加
// 引数は executor, subscription, msg, コールバック関数, 呼び出し方法(?)
RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &twist_msg, &twist_callback, ON_NEW_DATA));
}
まずはモータードライバ(MD)に入力するPWM波を生成するためのピン設定を行っています．
サンプリング周波数を今回使用したモータードライバの許容最大PWM周波数である20kHz，分解能を8bit(0~255)として，8つのピンそれぞれに1つのチャンネルをfor文で回して割り当てています．
また，電源を入れてからmicro-ROSの接続が確立されるまではモータが止まっていてほしいので，念のためモータを停止させる処理を入れています．
次に，set_microros_wifi_transports関数に，接続するWiFiのSSIDとパスワード，そしてmicro-ROSのAgentを起動するPCのIPアドレスを渡して接続を試みます．
それぞれの引数(文字列)は各自の環境に合わせて書き換える必要があります．
接続できたらmicro-ROSのセットアップを行います．
rclc_executor_init関数の引数に渡すコールバックの数について，なぜかメッセージを受信できないと思ったらこの引数の値が間違っていたというミスが起こりがちなのでご注意ください．
また，subscriptionを宣言するときに，今回はtwist_callbackというコールバック関数を呼び出すよう割り当てています．
/cmd_velメッセージを受信した際に呼び出されるtwist_callback関数の中身を見ていきます．
// MDに入力するduty比(0~255)を格納する配列(0:右前, 1:左前, 2:左後, 3:右後)
int duty[4] = {0};
void twist_callback(const void * msgin)
{
// 受信したメッセージを格納
const geometry_msgs__msg__Twist * msg = (const geometry_msgs__msg__Twist *)msgin;
// 各モータのduty比を計算
duty[0] = msg->linear.x * 127 + msg->angular.z * 127;
duty[1] = msg->linear.x * -127 + msg->angular.z * 127;
duty[2] = msg->linear.x * -127 + msg->angular.z * 127;
duty[3] = msg->linear.x * 127 + msg->angular.z * 127;
// 各モータを回転させる
for(int i = 0; i < 4; i++) {
if (duty[i] > 0) {
// Duty比が正ならモータを正転
ledcWrite(i*2, duty[i]);
ledcWrite(i*2+1, 0);
} else if(duty[i] < 0) {
// Duty比が負ならモータを逆転
ledcWrite(i*2, 0);
ledcWrite(i*2+1, -1*duty[i]);
} else {
// Duty比が0ならモータを停止
ledcWrite(i*2, 0);
ledcWrite(i*2+1, 0);
}
}
}
まず受信したメッセージをローカルの変数に格納しています．
基本的に受信したメッセージの内容にアクセスする際は，コールバック関数内であればこのローカルの変数，コールバック関数外であれば事前に宣言しておいたメッセージ格納用変数を利用することが多いようです．
次に足回りの各モータに入力するPWM波のDuty比を計算しています．
4輪メカナムの運動学モデルに従って，左前と左後のDuty比だけlinear.xの値に負のバイアスを掛けています．
また，今回は/cmd_velの各値の絶対値が1を超えないという前提で計算式を記述していますが，念のため配列dutyに格納されている値が0~255の範囲内かチェックするといった対策をしておいたほうがいいかもしれません．
参考までに，以下のように記述すればduty[0]に代入される値を-254~254の範囲に制限することができます．
duty[0] = min(254, max(-254, msg->linear.x * 127 + msg->angular.z * 127));
最後にモータを回転させる処理をfor文で回して実行しています．
配列dutyに格納されている値が正であれば正回転，負であれば逆回転させています．
ledcWrite関数の第2引数は正の値しか受け付けないため，dutyに格納されている値が負の場合のみ-1を掛けて渡しています．
なお，今回はPWM-PWM型のMDを使用しているためledcWrite関数を2回実行する形になっていますが，DIR-PWM型のMDを使用する場合は記述が変わってきますのでご注意ください．
ラジコン回路
MDはCytron社製のMaker Driveを使用しています．
M5Stack Core2で自由にPWM波を出力できるGPIOはちょうど8つぐらいのようなので，単純な拡張でこれ以上モータを増やすのは少々難しそうです...
本体付属のバッテリーはちょうど2.54mmピッチのピンヘッダーに刺さるため，そこからMDに電力を供給させています．
早速動かしてみた
プログラムを書き込み，回路を組んでラジコンを動かしてみました！
例のラジコンを #ROS2 + micro-ROSで動かしてみました！
— Akinosuke Tsutsumi (@akki_nitkk) December 29, 2023
teleop_twist_joyパッケージで/joyを/cmd_velに変換してM5Stack Core2に送っています
ソースコードの細かい部分や回路については後日Qiitaにまとめる予定です👀
▼ソースコードありますhttps://t.co/5ieHSDFkWq pic.twitter.com/7doNqQK03S
おわりに
今後はミニPCとLiDAR積んで魔改造していこうと考えています！
USBカメラ(もしくはRealSense)あたりを取り付けて上物の照準を何かに自動追従させるとかも楽しそうです．
この記事が少しでも皆様のものづくりの役に立てれば幸いです．
もし質問等あれば気軽にお問い合わせください．