costmap_2dの障害物情報のクリアについて
勘違いしてました。
costmap_2d/hydro/obstacles - ROS Wiki
costmap_2dは、センサ情報としてPointCloudまたはLaserScan型のメッセージを受け取ります。
RoboCup SSLでは、フィールド上部のカメラからロボット及びボールの絶対座標が常に取得できるので、 その値を使ってロボットの輪郭をなぞったPointCloudを渡してやればいいだろう、と思っていました。
が、うまくいかず。 障害物は確かに認識されるのですが、障害物が動いてPointCloudのデータが変化した後も、 前の場所に障害物の情報が残ってしまう、という現象が起きました。
これでしばらくハマりましたが、おそらく原因がわかりました。
costmap_2dが障害物をクリアする条件は、
以前観測された点が、直近に観測された点とロボット本体の間にある場合
のようです。
ですから、障害物情報を消去したい場合は、ロボット本体と観測の間に障害物が入るような観測を 与えてやる必要があります。
costmap_2dは、RoboCupSSLのようなワールド座標に固定されたセンサ情報は前提としておらず、 ロボット本体に乗っかったセンサのみを扱うように設計されているみたいです。
障害物情報を消去する方法としてもうひとつ、 move_baseのサービスである、"clear_costmap"を呼ぶという方法もあります。
しかしながら、"clear_costmap"サービスは、高速に頻繁に呼ばれることを想定していないみたいです。 あんまり頻繁に呼びすぎるとクラッシュするとか。
move_base crashed when call clear_costmaps service - ROS Answers: Open Source Q&A Forum
参考
publishing "0" points in a point cloud - ROS Answers: Open Source Q&A Forum
C++でのROSのノード開発メモ
Catkin設定方法
How to do common tasks — catkin 0.6.15 documentation
ソースファイルの追加
CMakelist.txtの、add_executable()にファイル名を追加する
add_executable(node_name src/filename1.cpp src/filename2.cpp)
node_nameの部分は、このファイルのノード名にする
ヘッダファイルの追加
CMakelist.txtの、include_directories()にインクルードディレクトリを追記し、そこに入れる
include_directories( "include" ${catkin_INCLUDE_DIRS} )
tf
Quaternion --> Eulerの変換
Transform Quaternion - ROS Answers: Open Source Q&A Forum
#include <tf/transform_datatypes.h> // ... tf::Quaternion q(quat.x, quat.y, quat.z, quat.w); tf::Matrix3x3 m(q); double roll, pitch, yaw; m.getRPY(roll, pitch, yaw); std::cout << "Roll: " << roll << ", Pitch: " << pitch << ", Yaw: " << yaw << std::endl;
geometry_msgs::Quaternionの作り方
createQuaternionMsgFrom***
を使う
Subscribe時のコールバック関数を、クラスのメンバメソッドとする場合
Using subscriber/callback function inside of a class C++ - ROS Answers: Open Source Q&A Forum
roscpp_tutorials/Tutorials/UsingClassMethodsAsCallbacks - ROS Wiki
状態機械を作る"Smach"パッケージの使い方(1)
ロボットに限らずですが、状態機械を用いることで、様々なタスクの遷移をスマートに表現することができます。
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
こんなのが出たら成功です。
smach_viewerを使う
rosrun smach_viewer smach_viewer.py
こんな感じで状態遷移図が表示されれば成功!
navigationスタックを理解できそう
やっとぼんやりながら正体が掴めつつあるので、メモを随時残していきます
2016/10/11 追記
座標系のところに、REP 105(座標系についてまとめたROSの文章)の一部翻訳を乗せました。(正確ではない可能せがあります)
wikiのこれ(下画像)の階層構造
だいたいこんな感じ(だと思います)
- 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
- BaseGlobalPlanner Plugins
costmap_2d
- voxel_grid
(move_base_msgs)
map_server
amcl
- fake_localization
- robot_pose_ekf
move_baseが親玉
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の座標系を確認する: 花岡ちゃんに花束を
以下の3つの座標を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が便利そう
実際に運用するときは、robot_pose_ekfでいけそう めちゃ便利そう
ただし、非ホロノミック移動体(自動車や、マイクロマウスのような二輪移動ロボット)のみが対象ぽい
センサ情報のセッティング
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が吐いてくれるが、今回は使わないので自分で吐く)
rosserial を用いてROSとArduinoの間の通信を行う
久方ぶりの記事です。
今回はタイトル通り、rosserialを用いてROSとArduinoを接続します。 ついでに、カスタムメッセージを用いた通信についても解説します。
rosserialとは
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
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) - 障害物情報のセット
Navigationスタックは、ロボットの移動制御を行うための大規模なスタックです。 このスタックに、目標地点、障害物、マップ、オドメトリの情報をそれぞれ与えると、それらを統合して、台車の速度指令を出力してくれます。
内部的には、グリッドセルに区切ったコストマップ(障害物情報を含んだマップ)の生成、広域な経路生成、ロボット周辺のローカルな経路生成、移動制御などを、別々のパッケージが分担して行っています。 もちろん、それぞれのパッケージのパラメータは調整できるので、種々のロボットに合わせて適切な制御を行うことができます。
これから、この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型は1つの点の座標を表現するための型で、x,y,zをfloat32型で持っています。pointsの要素一つ一つに、障害物点の座標を登録します。
最後に、channelsについてですが、これはいまいちよくわかっていません。 どうやら、pointsに登録した点一つ一つの性質を登録するものみたいです。例えば、色情報であったり、点の強さ?であったり。 正体がわかったらまた追記しておきます。
"RefereeBox"のインストール方法
前の記事で、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
というわけで、無事に動きました。
自動インストールのシェルスクリプトはこちら。
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