うごくものづくりのために

技術的な備忘録がメインです。

rosserial を用いてROSとArduinoの間の通信を行う

ROS

久方ぶりの記事です。

今回はタイトル通り、rosserialを用いてROSとArduinoを接続します。 ついでに、カスタムメッセージを用いた通信についても解説します。

rosserialとは

ja/rosserial - ROS Wiki

rosserialは、シリアル通信を用いて、ROSと組み込みボード、あるいはセンサノード等と通信するためのパッケージです。 シリアル通信をするだけでは無く、ROSとその他のもの達との通信プロトコルも提供します。 ArduinoでいうところのFirmataみたいなものです。Firmata使ったこと無いけど。

rosserialを使うと、シリアル通信で接続されたボードが、ROS側からはROSノードのように見えます。 ボードと通信するには、rosserialによってできたノードに対して、いつものようにトピックをPublish/SubscribeすればOKです。

まずは動かす

インストール

rosserial_arduino/Tutorials/Arduino IDE Setup - ROS Wiki

まずは、rosserialのパッケージをインストールします。

sudo apt-get install ros-indigo-rosserial-arduino
sudo apt-get install ros-indigo-rosserial

次に、Arduino IDEに、rosserialのライブラリを導入します。 Arduino IDEをまだインストールしてないひとは、ダウンロードしてインストールしてください。

Arduino IDEのインストールフォルダの下、librariesフォルダに移動して、make_libraries.pyを実行すると、ros_libフォルダができます。 ros_libフォルダの中には、rosserialのライブラリと各種メッセージのヘッダが含まれています。

cd <sketchbook>/libraries
rm -rf ros_lib
rosrun rosserial_arduino make_libraries.py .

二行目は、もし既にros_libフォルダがあった際、一旦それを削除するためのものです。

最後に、Aruduino IDEを起動して、サンプルスケッチの中にros_lib関連のものが入っていることを確認してください。

通信テスト

rosserial_arduino/Tutorials/Hello World - ROS Wiki

Arduino側の準備ですが、 Arduino IDEから、サンプルのros_lib/HelloWorldを選択し、Arduinoに書き込むだけです。

PC側は、まずroscoreを立ち上げておきます。その後、以下のコマンドを打ちます。

rosrun rosserial_python serial_node.py _port:=/dev/ttyUSB0

これで、ArduinoがROSノードとして認識されます。/dev/ttyUSB0は適宜読み替えてください。

最後に、ArduinoがPublishしてるトピックを確認します。

rostopic echo chatter

f:id:tilt_silvie:20150405180757p:plain

Arduinoへの書き込み時にPermission Deniedが出る場合

Arduinoが接続されているシリアルポート(/dev/ttyUSB0とか)に権限を与えてやりましょう。

sudo chmod 666 /dev/ttyUSB0

ただし、Arduinoを接続するたびに権限を与え直す必要があります。 それが面倒だ、って人は、そのための方法もあるのでググってください。

独自メッセージの作り方

まずは、ROSで普通にメッセージを定義します。以下のリンクの手順通り。

ja/ROS/Tutorials/CreatingMsgAndSrv - ROS Wiki

メッセージを作ったあとは、一応rosmsg show hogepiyo等をして、きちんと出来てることを確認しましょう。

作ったメッセージの導入は、最初にrosserialをArduino IDEに導入した時と同じ手順でいけます。

cd <sketchbook>/libraries
rm -rf ros_lib
rosrun rosserial_arduino make_libraries.py .

ros_lib以下に、作ったメッセージのヘッダファイルができてればOK。

rosserialの通信プロトコル詳細が気になる方へ

こちらをどうぞ。

rosserial/Overview/Protocol - ROS Wiki

さすがに誤り訂正はしてくれないですが、一応チェックサムが入ってるので信頼性はそこそこかと。

Navigationスタックを使う (1) - 障害物情報のセット

ROS Navigation

Navigationスタックは、ロボットの移動制御を行うための大規模なスタックです。 このスタックに、目標地点、障害物、マップ、オドメトリの情報をそれぞれ与えると、それらを統合して、台車の速度指令を出力してくれます。

内部的には、グリッドセルに区切ったコストマップ（障害物情報を含んだマップ）の生成、広域な経路生成、ロボット周辺のローカルな経路生成、移動制御などを、別々のパッケージが分担して行っています。 もちろん、それぞれのパッケージのパラメータは調整できるので、種々のロボットに合わせて適切な制御を行うことができます。

navigation - ROS Wiki

これから、このNavigationスタックを SSLロボットで使えるように調整していきます。

navigation/Tutorials/RobotSetup - ROS Wiki

障害物情報のセット

navigation/Tutorials/RobotSetup/Sensors - ROS Wiki

現在のところ、navigationスタックは障害物の情報として、sensor_msgs/LaserScan 型か sensor_msgs/PointCloud 型のメッセージを受け付けるようです。 SSLではLRFを用いて障害物検知をしているわけではないので、PointCloud型のメッセージとして障害物情報をパブリッシュします。

PointCloud型は、以下のような構成になっています。

sensor_msgs/PointCloud Documentation

まず、headerの中身から解説します。

  • seq
    シーケンス番号です。パブリッシュされるたびに勝手にインクリメントされるので、いじらなくてよし。

  • stamp
    タイムスタンプです。秒単位かナノ秒単位での現在時刻を入れておきましょう。

  • frame_id
    障害物の座標を指定する際の基準となる、tf上の座標空間の名前を指定します。 例えば、ワールド座標系で障害物の座標を指定する際は、frame_id = 'world' とでもしておきます。

次に、pointsについてです。

pointsには、障害物となる点の集まりを登録します。SSLの場合は、主としてロボットの外周が、この点の集まりとして表現されると思います。 pointsは、Point32型の配列となっています。 Point32型は１つの点の座標を表現するための型で、x,y,zをfloat32型で持っています。pointsの要素一つ一つに、障害物点の座標を登録します。

最後に、channelsについてですが、これはいまいちよくわかっていません。 どうやら、pointsに登録した点一つ一つの性質を登録するものみたいです。例えば、色情報であったり、点の強さ？であったり。 正体がわかったらまた追記しておきます。

"RefereeBox"のインストール方法

RoboCupSSL

前の記事で、RefereeBoxのインストールに失敗して悩んでいましたが、解決方法がわかりましたので、インストール手順を記しておきます。

最後のほうに、全部自動でインストールしてくれるシェルスクリプトも書いておきます。

ソースファイルのダウンロード、解凍

以下のURLからダウンロードします。

https://github.com/Hawk777/ssl-refbox/archive/robocup-2014.tar.gz

解凍は適当にやってください。

main.ccの改変

前の記事でバイナリが実行できなかったのは、referee.confというファイルが存在しなかったためでした。 どうやら、referee.confというファイルは、single.confという名前に変更されているようです。

そこで、main.ccを少し改変してやります。

具体的には、39行目の

std::string config_filename = Glib::filename_from_utf8(u8"referee.conf");

これを、

std::string config_filename = Glib::filename_from_utf8(u8"single.conf");

こうしてやります。

make
sudo apt-get install libprotobuf-dev protobuf-compiler libgtkmm-2.4-dev make g++
make
実行

./sslrefbox

というわけで、無事に動きました。

f:id:tilt_silvie:20141012223509p:plain

自動インストールのシェルスクリプトはこちら。

wget https://github.com/Hawk777/ssl-refbox/archive/robocup-2014.tar.gz
tar zxvf robocup-2014.tar.gz
cd ssl-refbox-robocup-2014
sed -i -e "s/referee.conf/single.conf/g" main.cc
sudo apt-get install libprotobuf-dev protobuf-compiler libgtkmm-2.4-dev make g++
make

"RefereeBox"のインストールをしたかった

RoboCupSSL

Small Size Robot League - referee

ソースファイルは、Source (.tar.gz) version 2014 finalからダウンロード。

解凍後のフォルダ内にREADME-LINUX.txtというファイルがあるので、それを参考にしながらインストールを進めます。

sudo apt-get install libprotobuf-dev protobuf-compiler libgtkmm-2.4-dev make g++
make

これにてインストール（というかコンパイル？）完了。

。。。

さて、無事にmakeは通ったのですが、バイナリが実行できない…

f:id:tilt_silvie:20141012010101p:plain

なんだかよくわからないので、ひとつ昔のバージョンのプログラムをダウンロードして、同様にインストールしてみようと思います。

Source (.tar.gz) version 2013 final

今度はmakeでコケた…

f:id:tilt_silvie:20141012012459p:plain

つらみが高まってきたので、今日はこのへんにしておきます。 原因が特定できて、問題が解決したらまた記事を書きます。

tfは遅い？

RoboCupSSL grSim tf ROS

現在、grSimとROSの結合作業をしています。

とりあえず、grSimからマルチキャストで送信される各ロボットの座標を、tfでブロードキャストしようと思ったのですが、問題発生。

おおよそ60Hzで座標データが飛んでくるので、その周期に合わせてブロードキャストしようと思い、コードを書きました。 しかし、2つ以上のデータを60Hzでブロードキャストしようとすると、一番最後にブロードキャストした以外のフレームの更新頻度が極端に落ちます。 一番最後にブロードキャストしたフレームは60Hz近くで更新されるのですが、それ以外が10Hzとか3Hzとか。

いろいろと原因を探ってみたのですが、いまいちよく分からず。 帯域が逼迫しているわけでもなさそうですし、tf.sendTransform()自体もそんな時間は食っていないようです。

一つのノードからは、高い周波数で複数のフレームをブロードキャストできないのかもしれません…。困ったな。 何か打開策を考えます。

#!/usr/bin/env  python
import  rospy
import  tf
import  socket
import  messages_robocup_ssl_wrapper_pb2

##################

multicast_if_addr='192.168.0.10'
multicast_group='224.5.23.2'
multicast_port=10020

my_addr='0.0.0.0'
server_address=(my_addr, multicast_port)

sock=socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind(server_address)

mreq=socket.inet_aton(multicast_group)+socket.inet_aton(multicast_if_addr)
sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq)

#make protobuf instance
ssl_wrapper = messages_robocup_ssl_wrapper_pb2.SSL_WrapperPacket()



def loadGeometry():
    br = tf.TransformBroadcaster()
    
    while not rospy.is_shutdown():   
        data    = sock.recv(1024)
        ssl_wrapper.ParseFromString(data)   #Parse sent data
        
        for i in range(0, 3):
            x   = ssl_wrapper.detection.robots_blue[i].x / 1000.0
            y   = ssl_wrapper.detection.robots_blue[i].y / 1000.0
            th  = ssl_wrapper.detection.robots_blue[i].orientation
                 
            br.sendTransform((x, y, 0),
                   tf.transformations.quaternion_from_euler(0, 0, th),
                   rospy.Time.now(),
                   "blue" + str(i),
                   "world")
        
        
        
if  __name__ == '__main__':
    rospy.init_node('grsim_geometry_loader')
    
    try:
        loadGeometry()
    except  rospy.ROSInterruptException:
        rospy.loginfo('Exception occured')
        pass

f:id:tilt_silvie:20140928164259p:plain

PythonとGoogleProtocolBuffersを用いてgrSimのデータを取得する

Python RoboCupSSL grSim

GoogleProtocolBuffers for Pythonのインストール

grSimをインストールした時 にダウンロードした GoogleProtocolBuffers のフォルダ内に、Python用セットアップファイルが同梱されているので、それを用いてインストールします。

まずは、このフォルダに移動。 今回は、Downloadsフォルダ内に解凍しているので、以下のパスとなっています。 適宜変えてください。

cd ~/Downloads/protobuf-2.5.0/python/

このフォルダ内にあるreadme.txtを参考にしながら、インストールを進めていきます。

python setup.py build
python setup.py test #このテストを全部通らないとまずい

python setup.py install #自分はsudoで実行する必要がありました

grSimのprotoファイルをPythonにコンパイル

まずは、適当に作業フォルダを作ります。

mkdir -p ~/Documents/SSL_develop/temp_proto/
cd ~/Documents/SSL_develop/temp_proto/

grSimが通信に用いるprotoファイルは、grSim/src/proto/ 内にあります。

このファイルたちを作業フォルダへとコピーします。

cp -r ~/Documents/SSL_develop/grSim/src/proto ./proto
ls ./proto

そのうち、 messages_robocup_ssl_*** というファイルが、grSimが吐き出すシミュレータデータのprotoファイルですので、それらをprotocコンパイラでPythonへと変換します。

protoc -I=./proto --python_out=./ ./proto/messages_robocup_ssl_detection.proto
protoc -I=./proto --python_out=./ ./proto/messages_robocup_ssl_geometry.proto
protoc -I=./proto --python_out=./ ./proto/messages_robocup_ssl_refbox_log.proto
protoc -I=./proto --python_out=./ ./proto/messages_robocup_ssl_wrapper.proto
ls

これで、messages_robocup_ssl***pb2.pyというファイルが4つできていれば成功です。

grSimのデータを取得

以下のページに、GoogleProtocolBuffersの使い方がまとまっています。 このページの下の方にある、"Reading A Message"というサンプルコードを参考に、 昨日書いたマルチキャストのソースコードを改造していきます。

Protocol Buffer Basics: Python - Protocol Buffers — Google Developers

#!/usr/bin/env python

import  socket
import  messages_robocup_ssl_wrapper_pb2

multicast_if_addr='192.168.0.10'  

multicast_group='224.5.23.2'
multicast_port=10020

my_addr='0.0.0.0'
server_address=(my_addr, multicast_port)

sock=socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind(server_address)

mreq=socket.inet_aton(multicast_group)+socket.inet_aton(multicast_if_addr)
sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq)

#make protobuf instance
ssl_wrapper = messages_robocup_ssl_wrapper_pb2.SSL_WrapperPacket()

while True:

    data    = sock.recv(1024)
    ssl_wrapper.ParseFromString(data)
    
    print   '-------------------------------------------'
    print   ssl_wrapper

あとは、grSimを起動したあとに、このコードを実行！

f:id:tilt_silvie:20140927151417p:plain

座標データやらなんやら取得できたーっ！

Pythonでマルチキャストのデータを読む

Python grSim

RoboCupSSLでは、チーム間共有のビジョンサーバがフィールド上のカメラ情報を統合し、各ロボットの座標などのデータを取得してくれます。 そのデータは、UDPマルチキャストによって送信されてきます。 その情報を使って、各チームのAIはいろいろと戦略を練って、ロボットへ指示を与えるわけです。

自分が、マルチキャストってなんぞい？っていう状態から、なんとかPythonを用いてサーバ（正確にはシミュレータ）からのデータを受信できるようになったのでメモしておきます。

マルチキャストって何

ネットワークのおべんきょしませんか？ テクニカルエンジニア(ネットワーク)、Cisco CCNA/CCNP対策に！

上のページを参考にしました。

ネットワーク上の複数の端末に、同じ情報を同時に送信するための仕組みです。 同様に、全体に同時送信するための仕組みで、ブロードキャストというのもあるのですが、 マルチキャストはブロードキャストより通信のオーバーヘッドが少ないです。

で、マルチキャストでデータを送信するためには、データを送信する対象となるグループを作る必要があります。 逆に、マルチキャストで送信されるデータを受信するためには、そのグループに参加する必要があります。

あんまり詳しくはわかっていないので、解説はこのへんで適当に終わらせておきます。 詳細な情報はぐぐってみてください…

Pythonでマルチキャストのデータを受信する

このページを参考にしてソースを書きました。（というかほぼコピペ）

pythonでマルチキャストを受信する - karasuyamatenguの日記

今回は、前回インストールしたgrSim を使って、grSimが吐いてくれるマルチキャストのデータをPythonで拾おうと思います。

#!/usr/bin/env python
import socket

#LANアダプタに割り当てられているIPアドレス
multicast_if_addr='192.168.0.10'

#マルチキャストアドレスとポート(grSim参照)
multicast_group='224.5.23.2'
multicast_port=10020

my_addr='0.0.0.0' #これは変えない
server_address=(my_addr, multicast_port)

sock=socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind(server_address)

mreq=socket.inet_aton(multicast_group)+socket.inet_aton(multicast_if_addr)
sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq)

while True:

    data, address=sock.recvfrom(1024) #本当は受信したデータを表示させたいけど、バイナリなので表示できない
    print 'Received'

一応"Received"が端末に表示されまくったので、受信できているでしょう。 バイナリデータからちゃんとしたデータに直すために、次はPythonでGoogleProtocolBuffersを使ってみようと思います。