滑らかにサーボを動かすためのROSのプログラミング

Text by Ben Martin
Translated by kanai

Robot Operating System（ROS）は、オープンソースのロボティックプラットフォームだ。これを使うことで、ロボットが物を見たり、マッピングしたりナビゲートしたり、または最新のアルゴリズムで周囲の環境に作用できるようになる。複雑なロボットを作りたいなら、すでに用意されているROSコードが役に立つ。ROSは最小限の利用が可能だ。Raspberry Piレベルのコンピューターからインストールできる。

ROS入門編として、サーボのコントロールを見てみよう。サーボの欠点として、できるだけ高速にコマンドに従おうとするところがある。そのため、急に頭が動いてバランスを崩し倒れる、なんていうことがよくある。だがROSを使えば、正弦曲線的な動きが可能になるので、ロボットが安定する。これは、ROSで行えるため、コントロール用のコードを書き換える必要がない。また、サーボとROSをつなぐコードも、サーボのハードウェアも変更する必要がない。さらにそのコードは使い回しが可能だ。

ROSは、UbuntuまたはDebianとの相性がいいので、コンパイルの必要がない。このビルドでは、LinuxマシンにUbuntuを走らせ、ホビー用サーボ、Arduino、普通のリード線を使っている。ROSはUbuntuマシン上で起動し、メッセージがUSBを介してArduinoに送られる。バイナリーのROSパッケージをインストールしてあれば、コンソールプログラム（gnome-terminal、またはkonsoleなど）に以下のコマンドを追加することで、Arduino環境はROSライブラリーを認識できるようになる。

cd ~/sketchbook/libraries rm -rf ros_lib rosrun rosserial_arduino make_l ibraries.py .

Arduino のプログラム

次に、ArduinoにSketchをアップロードして、ローレベルのサーボ制御を実行させ、Linuxマシンからの操作を可能にする。これは、サーボの位置を制限範囲内のパーセンテージ（0.0〜1.0）で指定する。明示的な角度ではなくパーセンテージを使う理由は、Arduinoのコードに正確な角度制限をさせるためであり、角度で指定したときの衝突を避けるためだ。

見て分かるとおり、ROSを使うと通常のループ関数は非常にシンプルになる。ループ関数は、データをsubscribeするだけで、どのArduinoコードでも同じだ。セットアップではROSを初期化して、それぞれのROSメッセージのサブスクライバーのsubscribe（購読）を呼び出す。それぞれのサブスクライバーはArduinoのRAMを占拠する。Sketchで何をするかによるが、6個から12個が限度だ。

#include <Arduino.h>
#include <Servo.h>

#include <ros.h>
#include <std_msgs/Float32.h>

#define SERVOPIN 3

Servo servo;
void servo_cb( const std_msgs:: Float32& msg )
{
const float min = 45;
const float range = 90;
float v = msg.data;

if( v > 1 ) v = 1;
if( v < 0 ) v = 0;

float angle = min + (range * v);
servo.write(angle);
}
ros::Subscriber<std_msgs::Float 32> sub( “/head/tilt”, servo_cb );

ros::NodeHandle nh;

void setup()
{
servo.attach(SERVOPIN);

nh.initNode();
nh.subscribe(sub);
}

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

次に、ArduinoからROSの世界に話しかけられるようにする。そのもっとも簡単な方法が、ロボットローンチファイルだ。下のファイルはとてもシンプルだが、ここにローンチファイルを追加することで、非常に複雑なロボットもひとつのコマンドでスタートさせることができる。

$ cat rosservo.launch

<launch>
<node pkg="rosserial_python" type="serial_node.py" name= "osservo" respawn="true"output="screen">
<param name="port" value="/dev/ttyUSB0" />
</node>
</launch>

$ roslaunch ./rosservo.lanch

rostopicコマンドは、ROSメッセージをロボットのどの部分に送れるかを見えるようにするものだ。下のコードを見てわかるとおり、Arduinoから「/head/tilt」が使えるようになっている。メッセージは「rostopic」を使って送る。-1というオプションは、メッセージを1回だけpublish（発行）して、/head/tiltにひとつの浮動小数点数を送るように伝えるものだ。

$ rostopic list
/diagnostics
/head/tilt
/rosout
/rosout_agg

$ rostopic pub -1 /head/tilt std_msgs/Float32 0.4
$ rostopic pub -1 /head/tilt std_msgs/Float32 0.9

この段階では、ROSに数値を発行する方法を知っているものはすべて、サーボのコントロールに使える。0から1に変えると、サーボはフルスピードで動くようになる。それはそれで問題はないのだが、実際には、だんだん速くなってフルスピードに達し、そしてだんだん遅くなって目的の角度に止まってほしい。サーボが急激に動くと、ロボットの動作はぎこちなくなり、周囲の人たちはビックリしてしまう。

Anotherノードで滑らかに

Houndbot

Terry

このTerryもHoundbotもROSロボットだ。6061アルミ合金のパーツで作られている。私の目標は、これらをできる限り自律化することだ。

下のPython のスクリプトは「/head/tilt/smooth」のメッセージを聞き、「/head/tilt」に多くのメッセージを発行して、サーボを目的の角度まで、徐々に速く、そして徐々に遅くして回転させるためのメッセージを発行している。「moveServo_cb」は、「/head/tilt/smooth」にメッセージが届いたときに必ず呼び出される。そのコールバックは、-90から+90までの10度ごとにひとつの数値を生成して、角度の配列に追加する。「sin()」はその角度を受けて、-1から+1まで値を徐々に増やしていく。それに1を足すとレンジは0から-2となり、それを2で割って、0から+1までのカーブの数値配列ができあがる。そしてm配列の中を一覧して、その都度、メッセージを発行し、そのたびにレンジrの中をわずかに先に進み、1*rまたはフルレンジに至る。

#!/usr/bin/env python

from time import sleep
import numpy as np

import rospy
from std_msgs.msg import Float32

currentPosition = 0.5
pub = None

def moveServo_cb(data):
global currentPosition, pub

targetPosition = data.data
r = targetPosition - currentPosition
angles = np.array( (range(190)) [0::10]) - 90
m = ( np.sin( angles * np.pi/ 180. ) + 1 ) /2

for mi in np.nditer(m):
pos = currentPosition + mi*r
print “pos: “, pos
pub.publish(pos)
sleep(0.05)

currentPosition = targetPosition
print “pos-e: “, currentPosition
pub.publish(currentPosition)

def listener():
global pub
rospy.init_node(‘servoencoder’, anonymous=True)
rospy.Subscriber(‘/head/tilt/smooth’, Float32, moveServo_cb)
pub = rospy.Publisher(‘/head/tilt’, Float32, queue_size=10)
rospy.spin()

if __name__ == ‘__main__’:
listener()

滑らかなサーボの動きをテストするには、Pythonスクリプトをスタートさせて、メッセージを「/head/tilt/smooth」に発行させる。すると滑らかな動きが確認できる。

$ ./servoencoder.py
$ rostopic pub -1 /head/tilt/smooth std_msgs/Float32 1
$ rostopic pub -1 /head/tilt/smooth std_msgs/Float32 0

ROSの中の名称をマッピングし直すこともできる。「/head/tilt/smooth」を「/head/tilt」にリマップすれば、プログラムは正弦曲線的に数値が変化することを意識せずにサーボに命令を出せるようになる。

その先へ

ここでは簡単なサーボの制御だけを解説したが、ROSにはもっと多くの機能がある。ロボットの障害物になっているものが何かを知りたければ、すでにROSがサポートしているKinectを使えばよい。ナビゲーションスタックがそのデータを使ってマッピングをしていても、短いPythonスクリプトをフィードしてサーボを動かし、ロボットにいちばん近いオブジェクトを追跡するよう命令することが可能だ。そう、その目が実際にオブジェクトを追いかけるのだ。

ROSを使って、私は2台のロボット、TerryとHoundbotを作った。Terryは屋内用のロボットで、2つのKinectを搭載している。1つはナビゲーション専用。もうひとつは深度のマッピング用だ。Arduinoを6つ使っていて、TerryはROSを使ったウェブインターフェイスから、またはPS3リモコンから直接操作できる。

Houndbotは屋外で使うようにデザインした。これにはラジコン、GPS、コンパス、ROS制御の耳がある。今はナビゲーション用にPS4のツインカメラを搭載しようとしている。太陽の光でKinectが使えないからだ。重量は20キログラム。先日、サスペンションを追加したのだが、そのために、アルミ合金のカスタムパーツを自作することになった。