MoveIt! Benchmark Toolsの使い方

概要

MoveIt!に各プランナーの性能評価を行うツールが付属しているので，それの使い方をメモ．

準備

warehouse_rosを導入．

git clone https://github.com/ros-planning/warehouse_ros.git
cd warehouse_ros
git branch indigo-devel origin/indigo-devel
git checkout indigo-devel

cd <catkin_ws>
rosdep install -i -r --from-paths src
catkin_make

Benchmarkを実行する環境を設定する

各node立ち上げ

roslaunch motoman_gazebo sia5_empty_world.launch gui:=false
roslaunch motoman_moveit sia5_moveit_planning_execution.launch 
roslaunch motoman_sia5_moveit_config warehouse.launch # moveit_warehouse_database_path:=~/moveit_dbがデフォルトで設定されている．

Motion Planning RViz pluginでの設定

WarehouseのConnectを押す．

つながった！

いつも通りSceneを追加する．

追加された．

Save Sceneを押す．

名前変える．今回は「nishidalab-test-space」．

Stored StatesでSave StartとSave Goalのボタンを使って，初期姿勢と目標姿勢を保存してあげる．名前はわかりやすいように「start」と「goal」にしました．

Stored Scenesのタブに戻ってSave Queryを押す．このときの名前は「Motion_Plan_Request」にしました．

これで設定は終わり．ちゃんとこれらの設定が保存されているか気になる場合．一回Rvizを閉じてから，
WarehouseのConnect→Stored ScenesのLoad Scene→Load Queryを押していつも通りモーションプランニングボタンを押す． そのときちゃんと設定した初期姿勢から目標姿勢までのプランニングをしてくれたら成功．

Benchmark実行

benchmark_config.cfgを作りました．このファイルでどんなベンチマークをとるかを定義します． このファイルはmotoman_project/motoman_sia5_moveit_config/config/にあります．

[scene]
name=nishidalab-test-space
query=Motion-Plan-Request
runs=2
timeout = 10
output = /home/ry0/result.log

[plugin]
name=ompl_interface/OMPLPlanner
planners=SBLkConfigDefault ESTkConfigDefault LBKPIECEkConfigDefault BKPIECEkConfigDefault KPIECEkConfigDefault RRTkConfigDefault RRTConnectkConfigDefault RRTstarkConfigDefault TRRTkConfigDefault PRMkConfigDefault PRMstarkConfigDefault
runs = 10

roslaunch motoman_sia5_moveit_config run_benchmark_ompl.launch # motoman_sia5_moveit_config/config/benchmark_config.cfgはデフォルト値でセットしてあります．
                                                               # 違う場所にある場合は引数で与えてください(cfg:=ほげほげ)

待つ．

データ処理，可視化

moveit_benchmark_statistics.pyは私が試した環境ではPDF表示に問題があったので少しコードを変更しました． 157行目のdef plot_attribute(cur, planners, attribute, typename):関数．

def plot_attribute(cur, planners, attribute, typename):
    """Create a box plot for a particular attribute. It will include data for
    all planners that have data for this attribute."""
    plt.clf()
    ax = plt.gca()
    labels = []
    measurements = []
    nan_counts = []
    is_bool = True
    for planner in planners:
        cur.execute('SELECT * FROM `%s`' % planner)
        attributes = [ t[0] for t in cur.description]
        if attribute in attributes:
            cur.execute('SELECT `%s` FROM `%s` WHERE `%s` IS NOT NULL' % (attribute, planner, attribute))
            measurement = [ t[0] for t in cur.fetchall() ]
            cur.execute('SELECT count(*) FROM `%s` WHERE `%s` IS NULL' % (planner, attribute))
            nan_counts.append(cur.fetchone()[0])
            cur.execute('SELECT DISTINCT `%s` FROM `%s`' % (attribute, planner))
            is_bool = is_bool and set([t[0] for t in cur.fetchall() if not t[0]==None]).issubset(set([0,1]))
            measurements.append(measurement)
-           labels.append(planner.replace('planner_geometric_','').replace('planner_control_',''))
+           name_short = planner.replace('planner_OMPL_', '')
+           name_short = name_short.replace('kConfigDefault', '')
+           labels.append(name_short)
    if is_bool:
        width = .5
        measurements_percentage = [sum(m)*100./len(m) for m in measurements]
        ind = range(len(measurements))
        plt.bar(ind, measurements_percentage, width)
-       xtickNames = plt.xticks([x+width/2. for x in ind], labels, rotation=30)
+       xtickNames = plt.xticks([x+width/2. for x in ind], labels, rotation=20)
        ax.set_ylabel(attribute.replace('_',' ') + ' (%)')
    else:
        if int(matplotlibversion.split('.')[0])<1:
            plt.boxplot(measurements, notch=0, sym='k+', vert=1, whis=1.5)
        else:
            plt.boxplot(measurements, notch=0, sym='k+', vert=1, whis=1.5, bootstrap=1000)
        ax.set_ylabel(attribute.replace('_',' '))
        xtickNames = plt.setp(ax,xticklabels=labels)
        plt.setp(xtickNames, rotation=25)
    ax.set_xlabel('Motion planning algorithm')
    ax.yaxis.grid(True, linestyle='-', which='major', color='lightgrey', alpha=0.5)
    if max(nan_counts)>0:
        maxy = max([max(y) for y in measurements])
        for i in range(len(labels)):
            x = i+width/2 if is_bool else i+1
            ax.text(x, .95*maxy, str(nan_counts[i]), horizontalalignment='center', size='small')
    plt.show()

[ INFO] [1481806849.435471055, 6642.557000000]: Benchmarks complete! Shutting down ROS...
[moveit_benchmark_p344209r_32464_7001629492371034295-2] process has finished cleanly

の表示が出たらCtrl+Cで閉じる．

rosrun moveit_ros_benchmarks moveit_benchmark_statistics.py ~/result.1.log
rosrun moveit_ros_benchmarks moveit_benchmark_statistics.py -p ~/result.pdf

PDFを見る．自分が手を加えたプランナーは....とか見れる．
おわり．

参考サイト

http://docs.ros.org/indigo/api/moveit_tutorials/html/doc/benchmarking_tutorial.html
http://picknik.io/moveit_wiki/index.php?title=Environment_Representation/Rviz#Saving.2FImporting_From_a_Database https://github.com/ros-planning/moveit_ros/issues/206