2015-05-24

状態機械を作る"Smach"パッケージの使い方(1)

Smach ROS

ロボットに限らずですが、状態機械を用いることで、様々なタスクの遷移をスマートに表現することができます。

ROSの"Smach"パッケージを使えば、状態機械をPython スクリプトによって比較的簡単に実現できます。

Smachのチュートリアルページは全部英語だし、しかも順番がグチャグチャで訳わからんことになっているので、こちらで整理しておきます。

準備

チュートリアル用パッケージの作成

cd ~/catkin_ws/src/
catkin_create_pkg smach_tutorial rospy smach smach_ros

smach_viewerの導入

Smachで作成した状態機械を視覚的に確認できる、"smach_viewer"を導入します。ただし、ROS Indigoだと、デフォルトではsmach_viewerがうまく動かないので、簡単な改造をします。

まずは導入。

sudo apt-get install ros-indigo-smach-viewer

そして、改造します。

ROS Indigo :Cannot show Graph View on smach_viewer - ROS Answers: Open Source Q&A Forum

/opt/ros/indigo/lib/python2.7/dist-packages/xdot/xdot.pyの480行目を、以下のように書き換えます

管理者権限が必要なので、sudoでエディタを立ち上げて、書き換えてください。

# return int(self.read_code())     #元々のコード
return int(float(self.read_code())) #書き換えたコード

一番シンプルな状態機械を作る

smach/Tutorials/Getting Started - ROS Wiki

サンプルコードを、先ほど作ったsmach_tutorial以下に、state_machine_simple.pyとして保存してください。

このサンプルは、状態FOOに遷移するごとにカウンタをインクリメントし、カウンタが3を超えた時に状態機械を終了する、という動作をします。

#!/usr/bin/env python

import rospy
import smach
import smach_ros

# define state Foo
class Foo(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['outcome1','outcome2'])
        self.counter = 0

    def execute(self, userdata):
        rospy.loginfo('Executing state FOO')
        rospy.sleep(1)
        if self.counter < 3:
            self.counter += 1
            return 'outcome1'
        else:
            return 'outcome2'


# define state Bar
class Bar(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['outcome2'])

    def execute(self, userdata):
        rospy.loginfo('Executing state BAR')
        rospy.sleep(1)
        return 'outcome2'
        



# main
def main():
    rospy.init_node('smach_example_state_machine')

    # Create a SMACH state machine
    sm = smach.StateMachine(outcomes=['outcome4', 'outcome5'])

    # Open the container
    with sm:
        # Add states to the container
        smach.StateMachine.add('FOO', Foo(), 
                               transitions={'outcome1':'BAR', 
                                            'outcome2':'outcome4'})
        smach.StateMachine.add('BAR', Bar(), 
                               transitions={'outcome2':'FOO'})

    # Create and start the introspection server
    sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
    sis.start()

    # Execute SMACH plan
    outcome = sm.execute()
    rospy.spin()


if __name__ == '__main__':
    main()

やっていることは、

状態の作成
状態機械の作成
デバッグ出力の準備
状態機械の実行

の順となります。

サンプルのコードを上から順に見ていきます。

状態の作成

# define state Foo
class Foo(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['outcome1','outcome2'])
        self.counter = 0

    def execute(self, userdata):
        rospy.loginfo('Executing state FOO')
        rospy.sleep(1)
        if self.counter < 10:
            self.counter += 1
            return 'outcome1'
        else:
            return 'outcome2'

Fooという状態を定義しています。

initでは、outcomes=['outcome1','outcome2']という部分で、この状態が取りうる出力を定義しています。シングルクォーテーションで囲まれた部分が、出力となります。一般的には、タスクの状態(success, abort, fail等)になると思います。

executeでは、この状態に遷移した際に実行する処理を書いています。任意の処理が終わったあとは、returnで、outcomesに定義した出力結果のうちの一つを返します。

Barに関しても同様です。

状態機械の作成

# Create a SMACH state machine
    sm = smach.StateMachine(outcomes=['outcome4', 'outcome5'])

状態機械を定義しています。状態機械が取りうる最終的な出力を、outcomesとして登録しています。

# Open the container
    with sm:
        # Add states to the container
        smach.StateMachine.add('FOO', Foo(),
                               transitions={'outcome1':'BAR',
                                            'outcome2':'outcome4'})

状態機械に対して状態を登録しています。

transitionsでは、出力結果と遷移先の状態のペアを登録します。

今回の例では、状態FOOが出力outcome1を出力した時、状態BARに遷移します。状態FOOが出力outcome2を出力した時は、状態機械の出力としてoutcome4を出力し、状態機械の実行を終了します。

デバッグ出力の準備

    # Create and start the introspection server
    sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
    sis.start()

ここでは、smach_viewerを用いて状態機械を確認するために、状態機械の状態（ややこしいね…）を外部に出力する手続きをしています。

状態機械の実行

    # Execute SMACH plan
    outcome = sm.execute()

最後に、状態機械を実行します。

実行してみる

それでは、作成したサンプルプログラムを実行してみましょう。

rosrun smach_tutorial state_machine_simple.py

f:id:tilt_silvie:20150524214119p:plain

こんなのが出たら成功です。

smach_viewerを使う

rosrun smach_viewer smach_viewer.py

f:id:tilt_silvie:20150524214306p:plain

こんな感じで状態遷移図が表示されれば成功！

2015-04-20

navigationスタックを理解できそう

ROS

やっとぼんやりながら正体が掴めつつあるので、メモを随時残していきます

2016/10/11 追記

座標系のところに、REP 105(座標系についてまとめたROSの文章)の一部翻訳を乗せました。（正確ではない可能せがあります）

wikiのこれ（下画像）の階層構造

f:id:tilt_silvie:20150927185827p:plain だいたいこんな感じ（だと思います）

navigation
- move_base
  - navcore
    - BaseGlobalPlanner Plugins
      - navfn
      - global_planner
      - carrot_planner
    - BaseLocalPlanner Plugins
      - base_local_planner
      - dwa_local_planner
    - RecoveryBehavior Plugins
      - rotate_recovery
      - clear_costmap_recovery
      - move_slow_and_clear
  - costmap_2d
    - voxel_grid
  - (move_base_msgs)
- map_server
- amcl
- fake_localization
- robot_pose_ekf

move_baseが親玉

move_base - ROS Wiki

move_baseに、種々のプラグインが実装されて、経路生成を行っている

global_costmap および local_costmap
global_planner
local_planner
recovery_behaviour

global_costmap および local_costmap

プラグイン実体は、 costmap_2d

http://wiki.ros.org/costmap_2d

障害物の登録

costmap_2dは、LaserScan or PointCloud型のメッセージを障害物として設定することができる

navigation/Tutorials/RobotSetup - ROS Wiki

コレの中ほどに説明あり

マップレイヤー

Static Map Layer costmap_2d/hydro/staticmap - ROS Wiki

変わらないマップ map_serverで生成できると思う

Obstacle Map Layer costmap_2d/hydro/obstacles - ROS Wiki

PointCloud or PointCloud2 or LaserScan型のtopicを勝手に読んで障害物としてくれるらしい

障害物のサイズも変更できる（というかロボットの安全領域を広げる？）ので、おそらくロボットの中心座標をPointCloudとしてぶちこめばOK

Infration Layer costmap_2d/hydro/inflation - ROS Wiki よくわかってないけど、たぶんロボットの安全領域を広げる的なやつ

global_planner

プラグイン実体は、move_base/base_global_plannerというパラメータとして与える

デフォルトだと、navfn/NavfnROSになっている

navfn(ダイクストラ)よりも、global_planner(NF1, A*とか？)のほうが良さそう

local_planner

プラグイン実体は、move_base/base_local_plannerというパラメータとして与える

デフォルトだとbase_local_planner/TrajectoryPlannerROSになっている

base_local_plannerよりも、dwa_local_plannerのほうがいいらしい (dwa_local_plannerはbase_local_plannerの焼き直しだとか)

dwa_planner vs. base_local_planner - ROS Answers: Open Source Q&A Forum

recovery_behaviors

障害物にぶつかってスタックしたときに回復するためのやつ

SSLではたぶん必要ないので、move_base/recovery_behavior_enabledパラメータをfalseにしてディスエーブルしよう

座標系

Navigation stackの座標系を確認する: 花岡ちゃんに花束を

以下の３つの座標をtfする

map : 一番上のレイヤ　真のワールド座標

odom : オドメトリ mapレイヤの下にくる　オドメトリと真のワールド座標のずれを表現する

base_link : オドメトリでのロボット位置

ワールド座標上でのロボットの真の位置は、

mapレイヤとodomレイヤのずれ + odomレイヤ上でのbase_linkの位置として表現できる

2016/10/11 追記分

REP 105 -- Coordinate Frames for Mobile Platforms (ROS.org)

base_link

"base_link"と呼ばれる座標系は移動ロボットの土台に固定されます。 "base_link"は任意の位置、方向の土台に与えられます。 REP 103[1]に好ましい方向の与え方が明記されています。

odom

"odom"と呼ばれる座標系は世界に固定された系です。 "odom"上の移動ロボットの姿勢は時間とともにドリフトします。このドリフトのため"odom"を長い期間の絶対座標として用いることは出来ません。しかしながら、"odom"上でのロボットの姿勢は連続であることが保障されます。これは、"odom"上でのロボットの姿勢が離散的に飛ぶことなく、スムーズに接続されることを表します。

一般的なセットアップでは、"odom"フレームは、車輪オドメトリやビジュアルオドメトリ、慣性センサなどによって計算され、与えられます。

"odom"は短時間には正確な座標系として使えますが、ドリフトがあるため長時間のリファレンスとすることは厳しいです。

map

"map"は世界に固定された系で、Z軸は上向きです。 "map"に対して相対的なロボットの姿勢は、時間とともに大きくドリフトすべきではありません。 "map"は不連続です。これは、"map"上のロボットの姿勢が離散的に飛ぶ可能性があることを表します。

一般的なセットアップでは、位置推定を行うコンポーネントが、センサの観測に基づいて"map"上でのロボットの姿勢を計算しなします。そのため、ドリフトは都度修正されますが、新たなセンサ情報が来た際に"map"上のロボットの位置は離散的に移動する可能性があります。

"map"は、長時間の絶対座標として用いることができます。しかし、離散的なジャンプが発生するため、センシングやアクチュエーションのために用いる短時間の正確な座標としては適切ではありません。

追記ここまで

~~シミュレータ上で動かすときはfake_localizationが便利そう~~

~~fake_localization - ROS Wiki~~

実際に運用するときは、robot_pose_ekfでいけそうめちゃ便利そう

ただし、非ホロノミック移動体（自動車や、マイクロマウスのような二輪移動ロボット）のみが対象ぽい

robot_pose_ekf - ROS Wiki

センサ情報のセッティング

tf

navigation/Tutorials/RobotSetup/TF - ROS Wiki

これはSSLには必要ないかも

吐かないといけないもの

ロボット上でのレーザーセンサの取り付け位置

sensors (障害物検知)

navigation/Tutorials/RobotSetup/Sensors - ROS Wiki

吐かないといけないもの

障害物情報としてのLaserScan か PointCloud

odom (オドメトリ)

navigation/Tutorials/RobotSetup/Odom - ROS Wiki

吐かないといけないもの

map->odomのtf

odom->base_linkのtf (本来は、mapとodomのtfからamclが吐いてくれるが、今回は使わないので自分で吐く)

2015-04-05

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

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

2014-11-02

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

ROS

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に登録した点一つ一つの性質を登録するものみたいです。例えば、色情報であったり、点の強さ？であったり。正体がわかったらまた追記しておきます。

2014-10-12

"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

2014-10-12

"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

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

2014-09-28

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