Refboxのディレクトリにある、referee.conf に各種コンフィグが保存されています。

ssl-refbox/referee.conf at master · RoboCup-SSL/ssl-refbox · GitHub

(Verの古いやつだと、refree.confじゃなくてsingle.confになってます)

で、ADDRESSってやつとかPROTOBUF_PORTってやつを任意に書き換えてあげればOK。

2016-03-21

【メモ】decision_making パッケージの rqt_decision_graphが動かない問題

plugin.xmlに設定されているパスが間違っている

<library path="src">

これを

<library path="src/rqt_decision_graph">

こう修正

あと、間違ったパスの状態でrqtを起動すると間違ったパスの情報がrqtに残ってしまうらしいので

rm ~/.config/ros.org/rqt_gui.ini
rqt

これで削除

2015-09-29

navigationスタックで学ぶpluginlibの使い方

ROS pluginlib

navigationスタックはpluginlibによる実装がなされていて、拡張しようと思ったらpluginlibの知識が必要ということなので、勉強しています。

今回は、navigationスタックのソースを追っかけていきながら、pluginlibの基本的な使い方について解説します。自分も勉強中の身なので、間違っているところがあれば指摘をおねがいします。

pluginlibとは

pluginlib - ROS Wiki

pluginlibとは、roscppで書いたコードに、プラグインを実装するための機能です。プラグインが何なのか、自分もまだはっきりとはわかっていませんが、基本的な概念としては、

インタフェースのみを定義したクラスを用意し、そのクラスの中身の実装を外部のライブラリによって行うことによって、ユーザによってクラスの実装を入れ替えることができる仕組み

のようです。

プラグイン - Wikipedia

イメージとしては、こんな感じでしょうか。

f:id:tilt_silvie:20150928231636p:plain

で、pluginlibは、roscppで書いたコードのクラスにこのプラグインのしくみを持たせるための機能です。

navigationスタックは、BaseGlobalPlanner、BaseLocalPlannerおよびRecoveryBehaviorがプラグインとして実装されています。

今回は、pluginlibのwikiにかかれている内容を、BaseLocalPlannerのプラグインの一つであるdwa_local_plannerのソースをおっかけながら確認していきます。

解説

プラグイン構成

wikiとソースコードをおっかける前に、プラグインの構成を整理しておきます。

move_baseとBaseLocalPlanner、その実装であるプラグインは下図のような関係になっています。

f:id:tilt_silvie:20150928233033p:plain

move_baseに、nav_core::BaseLocalPlannerというインターフェースが用意されており、その実装として dwa_local_planner::DWAPlannerROSと、base_local_planner::TrajectoryPlannerROSが用意されています。

インタフェースを定義しているパッケージは、nav_coreというパッケージで、move_baseパッケージとは別物になっています。通常はインタフェースを定義するパッケージとそれを使うパッケージは同一だと思います。たぶん。

プラグインの登録

pluginlibのwiki 3.1 Registering/Exporting a Plugin の内容です。

プログラムをプラグインとして登録するには、登録したいプログラムのソースコード中（どこでもいいらしい）に、

PLUGINLIB_EXPORT_CLASS(プラグインとして登録するクラス名, インタフェースクラス名)

というマクロを記述する必要があります。

dwa_local_plannerではどうなっているかというと…

navigation/dwa_planner_ros.cpp at jade-devel · ros-planning/navigation · GitHub

dwa_planner_ros.cppの50行目に、確かにありますね。

PLUGINLIB_EXPORT_CLASS(dwa_local_planner::DWAPlannerROS, nav_core::BaseLocalPlanner)

プラグイン説明ファイル（？）

pluginlibのwiki 3.2 The Plugin Description File の内容です。

プラグインの説明ファイルをxmlで作る必要があります。

内容は、だいたいこんな感じ。

<library path="ライブラリのパス">
  <class type="プラグインとして登録するクラス名" base_class_type="インタフェースクラス名">
  <description>
  説明文
  </description>
  </class>
</library>

dwa_local_plannerではどうなっているかというと…

navigation/blp_plugin.xml at jade-devel · ros-planning/navigation · GitHub

blp_plugin.xmlがそのようです。

<library path="lib/libdwa_local_planner">
  <class name="dwa_local_planner/DWAPlannerROS" type="dwa_local_planner::DWAPlannerROS" base_class_type="nav_core::BaseLocalPlanner">
    <description>
      A implementation of a local planner using either a DWA approach based on configuration parameters.
    </description>
  </class>
</library>

<class name=〜〜〜ってやつが追加されてますが、これは必要なのかな…？　よくわかりません。

ROSのパッケージシステムにプラグインを登録する

pluginlibのwiki 3.3 The Plugin Description File の内容です。

ROSのシステムにプラグインの認識をしてもらうために、プラグインパッケージのpackage.xmlファイルに以下の追記が必要です。

<export>
  <インタフェースが定義されているパッケージ名 plugin="${prefix}/プラグイン説明ファイルの名前" />
</export>

それと、これ。

  <build_depend>インタフェースが定義されているパッケージ名</build_depend>
  <run_depend>インタフェースが定義されているパッケージ名</run_depend>

dwa_local_plannerではどうなっているかというと…

navigation/package.xml at jade-devel · ros-planning/navigation · GitHub

47行目から49行目にかけてありますね。

    <export>
        <nav_core plugin="${prefix}/blp_plugin.xml" />
    </export>

build_dependとrun_dependは、それぞれ30行目、41行目に、nav_coreパッケージが記述されています。

定義されているプラグインを検索する

pluginlibのwiki 3.4 Querying ROS Package System For Available Plugins の内容です。

端末上で rospack plugins --attrib=plugin インタフェースが定義されたパッケージを実行することで、定義されているプラグイン一覧を表示できます。

以下の例では、nav_coreパッケージに定義されているプラグイン一覧を表示します。

rospack plugins --attrib=plugin nav_core

プラグインを使う.

pluginlibのwiki 4. Using a Plugin の内容です。

いままでは、プラグイン側の話でしたが、今回はプラグインを使う側の話です。

定義されたプラグインを使うには、以下のようにします。

#include <pluginlib/class_loader.h>
#include <インタフェースクラスが定義されたヘッダ>

//... some code ...

pluginlib::ClassLoader<インタフェースクラス名> poly_loader("インタフェースが定義されたパッケージ名", "インタフェースクラス名");

try
{
  boost::shared_ptr<インタフェースクラス名> poly = poly_loader.createInstance("プラグインクラス名");

  //何か処理
}
catch(pluginlib::PluginlibException& ex)
{
  //handle the class failing to load
  ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what());
}

流れとしては、pluginlib::ClassLoaderでインタフェースクラスをロードして、それにcreateInstance()メソッドで実体を定義してやる、という感じでしょうか。

さて、navigationのソースコードですが、プラグインを使っているのはmove_baseパッケージです。

navigation/move_base.h at jade-devel · ros-planning/navigation · GitHub

navigation/move_base.cpp at jade-devel · ros-planning/navigation · GitHub

move_base.hの205行目に、pluginlib::ClassLoaderの定義がありますね。

      pluginlib::ClassLoader<nav_core::BaseLocalPlanner> blp_loader_;

で、move_base.cppの53行目でこれを初期化しています。この書き方を知らない人は、"c++ メンバイニシャライザ"でググってください。

    blp_loader_("nav_core", "nav_core::BaseLocalPlanner"),

129行目では、createInstance()していますね。

      tc_ = blp_loader_.createInstance(local_planner);

おわりに

長い記事になってしまってすいません。

pluginlibに関する最低限のことはこれで理解出来たと思います。

既存のBaseLocalPlannerに満足が行かないので、これからがんばってあたらしいBaseLocalPlannerのプラグインをつくろうと思います。

2015-09-27

costmap_2dの障害物情報のクリアについて

ROS

勘違いしてました。

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

REP 117 -- Informational Distance Measurements (ROS.org)

2015-09-18

C++でのROSのノード開発メモ

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***を使う

tf: tf Namespace Reference

Subscribe時のコールバック関数を、クラスのメンバメソッドとする場合

Using subscriber/callback function inside of a class C++ - ROS Answers: Open Source Q&A Forum

roscpp_tutorials/Tutorials/UsingClassMethodsAsCallbacks - ROS Wiki

2015-05-24

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

Smach ROS

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

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

ja/smach - ROS Wiki

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