自作した台車をrosserial（ROS）で制御する方法

背景

ROSとは、ロボットの制御に便利な機能を提供してくれるプログラムです。
rosserialとは、Arduinoなどのシリアル通信出来る装置をROSの仕組みで動かせるライブラリです。
Arduinoとは、電子工作しやすいように作られた、マイコンが載った基板の名前です。

rosserialを使うことにより、ROSをインストールしたPCとArduinoのそれぞれに台車のハードウェアに依存したプログラムを作ることなく、Arduino向けにだけ台車のハードウェアに依存したプログラムを書けば、ROSで台車を制御できるようになります。
自作した台車をrosserialで制御するまでに、ところどころ詰まるところがあったので、自分の言葉でまとめて方法を共有します。

使ったもの

ROS（Melodic）をインストールしたPC

「Raspberry Pi + Raspbian」と「ノートPC + Ubuntu」で動くことを確認しました。

ROSのインストール方法は下記のページが参考になると思います。

Ubuntu: Ubuntu install of ROS Melodic
Raspberry Pi: ROS(melodic)をRaspberry Pi + Raspbian(stretch)にインストールする方法

Arduino

atmega32u4が載っているPro Microを利用しました。
atmega32u4とは、Arduinoとして動くICの一つであり、Arduino Leonardo、Arduino Micro、Pro Microなどに使われています。

Arduinoで出力を制御できる台車

Raspberry PiとArduinoに下記の材料を加えて作成しました。

• モーターコントローラ
  TB6612FNGが使われているドライバを利用しました。
  aliexpress | tb6612fng
• ギアードモーター + タイヤ 2セット
  何RPMのものを買ったかは忘れてしまいましたが、aliexpressで下記のような商品を購入しました。
  12V ギアモーター
• キャスター
• シャーシ
  設計情報はこちらです。
  kagotos.svg
• バッテリー
  12Vを引き出せるジャンプスターターをバッテリーとして利用しています。
  
• 他ネジ、柱など

中央の丸くて青い装置はLIDARですが、今回は使いません。
興味のある方は、こちらの記事を見てみてください。

$99と$400のLIDAR（360度レーザー距離測定機）を動かしてみた

PlatformIOをインストール

この記事ではPlatformIOというプログラム開発環境を利用して、Arduinoにrosserialのプログラムを書き込みます。
Arduinoの開発環境としてはArduino IDEが有名ですが、コマンドでの操作やライブラリの管理はPlatformIOの方が優れていると個人的に思うのが、PlatformIOを利用する理由です。

PlatformIOのインストール手順に従い、下記のようなコマンドで、Python2系のpipを利用してインストールできます。

sudo apt install python-pip
sudo pip install platformio

platformioはpioで呼び出せます。
下記のコマンドでバージョンが表示されれば、インストール成功です。

pio --version

参考:PlatformIOを使うと、ライブラリの管理が楽そうだった話

プロジェクトを作成

この記事では、ホームディレクトリ直下にatmega_rosserialというプロジェクトを作り、それを編集します。
今回はatmega32u4が載っているPro MicroというArduino互換機を使ったので、それ向けのプロジェクトを作ります。

mkdir ~/atmega_rosserial
cd ~/atmega_rosserial
pio init --board sparkfun_promicro16

rosserialライブラリを準備

ダウンロードできるライブラリを利用する場合

最新版のコードでなくて良い場合は、Arduino向けのライブラリとして公開されているrosserialを利用できます。
試した範囲では問題無さそうだったので、自分はこれを使っています。

ライブラリを利用する場合、platform.inoに下記の記述を追加します。

~/atmega_rosserial/platform.ino

lib_deps = rosserial_arduino

動作確認のため、rosserialを読み込むプログラムを作ってコンパイルしてみます。

下記のプログラムを作成します。

~/atmega_rosserial/src/main.cpp

#include <Arduino.h>
#include <ros.h>

ros::NodeHandle nh;

void setup() {
}

void loop() {
}

下記のようにプロジェクトディレクトリでpio runコマンドを実施してエラー無く終了すれば、ライブラリの設定が成功しています。

cd ~/atmega_rosserial
pio run

ROSのコマンドでライブラリを配置する場合

インストールしているrosがサポートするrosserialのコードを利用したい場合、プロジェクトのlibディレクトリにrosコマンドでライブラリを配置すればrosserialを利用できるようになります。

ライブラリを配置するためのプログラムをインストールします。

sudo apt install ros-melodic-rosserial-arduino

インストールしたら、下記のコマンドを実行します。

rosrun rosserial_arduino make_libraries.py ~/atmega_rosserial/lib

動作確認は、前のダウンロードしたrosserialを利用した場合と同じプログラムでできます。

rosserial_pythonをインストール

ROSと連携して動かすArduinoのシリアルポートを解釈するためのプログラムです。
下記のコマンドでインストールできます。
インストール後はPCの再起動をお勧めします。

sudo apt install ros-melodic-rosserial-python

受け取ったTwistの値に応じてモーターを動かす操作するプログラムを作成

rosserialを動かせる状態になったら、Twistの値に応じてモーターの動作を変えるプログラムを作ります。
TwistとはROSで利用される通信の型の一つであり、ROSのシミュレーションの一つであるtutllesmや、既存のロボットの一つであるturtlebotなどで、移動制御に使われています。
既存事例に倣って、この記事でもTwistを移動制御に利用します。

モータの操作はハードウェアに依存すると思うので、Twistをどのように処理したら良いかの参考になりそうなプログラムを共有します。
実際に動かしているコードを見たい方は、githubで公開しているファイルを見てください。

~/atmega_rosserial/src/main.cpp

#define USE_USBCON // for atmega32u4

#include <Arduino.h>
#include <ros.h>
#include <geometry_msgs/Twist.h>

void messageCb(const geometry_msgs::Twist& twist) {
  const float linear_x = twist.linear.x;
  const float angle_z = twist.angular.z;
  if (linear_x > 0.0 && angle_z == 0.0) {
    // Go forward
  } else if (linear_x < 0.0 && angle_z == 0.0) {
    // Go back
  } else if (angle_z > 0.0) {
    if (linear_x == 0) {
      // Turn left
    } else if (linear_x > 0) {
      // Go left forward
    } else {
      // Go right back
    }
  } else if (angle_z < 0.0) {
    if (linear_x == 0) {
      // Turn right
    } else if (linear_x > 0) {
      // Go right forward
    } else {
      // Go left back
    }
  } else {
    // stop
  }
}

ros::NodeHandle nh;
ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &messageCb);

void setup() {
  nh.getHardware()->setBaud(115200);
  nh.initNode();
  nh.subscribe(sub);
}

void loop() {
  nh.spinOnce();
  delay(1);
}

プログラムの内容を大まかに解説します。

#define USE_USBCON // for atmega32u4

#include <ros.h>

atmega32uを使う場合、#define USE_USBCON を #include <rosserial.h>の前に記述する必要があります。
こうしないと、USBでシリアル通信しないモードで起動してしまい、下記のとうなエラーが出ます。
これの解決に数時間取られました。

[ERROR] [1530255988.547879]: Write timeout: Write timeout
[ERROR] [1530255998.509807]: Unable to sync with device; possible link problem or link software version mismatch such as hydro rosserial_python with groovy Arduino

void messageCb(const geometry_msgs::Twist& twist) {
  ..
}

ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &messageCb);

cmd_velとしてTwistを受け取ったら、messageCbが呼び出されます。
messageCbの引数として受け取るTwistの値に応じて、モーターの出力を変えています。

nh.getHardware()->setBaud(115200);

通信速度は速くて良いと思うので、115200bpsにしました。
この記述が無い場合、57600bpsになります。

書き込み

ProMicroをPCにUSB接続して、プロジェクトのディレクトリでアップロードコマンドを入力すると、ProMicroにプログラムが書き込まれます。

cd ~/atmega_rosserial
pio run -t upload

Twistを発行するプログラムをインストール

この記事ではrosserialへの情報送信に下記のライブラリを利用します。
teleop_twist_keyboard

teleop_twist_keyboardとは、プログラムはキーの入力によって、/cmd_velに対してTwistを発行してくれるプログラムです。
実行すると下記のような説明が表示され、そのキーを入力することで十字キーで操作するような感覚でTwistを発行できます。

Moving around:
   u    i    o
   j    k    l
   m    ,    .

下記の手順でインストール出来ます。
Ubuntu OSの場合

sudo apt install ros-melodic-teleop-twist-keyboard

上記コマンド実行後、rosrunコマンドで呼び出せない場合はOSの再起動が必要かもしれません。

ソースコードからビルドする場合は、このようなコマンドでインストールできます。

cd ~/ros_catkin_ws/src
git clone https://github.com/ros-teleop/teleop_twist_keyboard.git
cd ../
catkin_make 

実行

ターミナルを複数使い、これらのrosのプロセスを実行します。

1. roscore
   ROSのノード間の接続に必要なので、まず最初に立ち上げます。

   roscore

2. rosserial
   atmegaのrosserialをroscoreに接続します。先ほどのmain.cppがアップロードされたatmegaに接続できれば、/cmd_velのTwistを購読する状態になります。
   

   rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 _baud:=115200

3. teleop_twist_keyboard
   Twistを発行するために起動します。
   起動した画面でキーを入力すると、それに応じたTwistが/cmd_velに発行されます。
   

   rosrun teleop_twist_keyboard teleop_twist_keyboard.py

4. rostopic
   /cmd_velのTwistの値が期待通りであることを確認します。
   動作確認のためのプロセスなので、このコマンドを実行しなくてもteleop_twist_keyboardとrosserialは連携できます。
   

   rostopic echo /cmd_vel

telop_twist_keyboardを立ち上げたターミナルでのキーの入力（「i」で前進、「,」で後進、「j」で左旋回、「l」で右旋回など）に応じて、ロボットが期待する動きをすれば成功です。

まとめ

rosserialを通してTwistを受け取って動作する台車を作れました。

何かの参考になれば嬉しいです。

参考

rosserial arduino can't connect (Arduino micro)
rosserial_arduinoTutorialsHello World
rosserial_arduinoTutorialsBlink

更新履歴

2019.03.06
Subscriberの定義にメッセージの方定義が必要になったので、 「ros::Subscriber sub」を「ros::Subscriber<geometry_msgs::Twist> sub」に変更しました。
2020.03.06
rosserial_pythonのインストール手順が抜けていたので、追加しました。