Webots講座5-2023:スーパーバイザ・コントローラ (Python)

Webots講座の5回目です.引き続きWebotsの自動運転シミュレータを使っていきます.Webotsにはロボットを動作させるコントローラの他に,スーパーバイザ(Supervisor)コントローラがあります.スーパーバイザは管理者という意味で,ロボットではなく人間が行う作業に相当することは,このスーパーバイザ・コントローラで実現できます.例えば,ロボットの真の位置や速度を取得したり,初期位置に戻すなどの作業です.スーパーバイザ・コントローラを使いロボットカーの真の自己位置を取得する方法とシミュレーションを再実行する方法を学びましょう.

レファレンス

API

  • class Node: ノードクラス
    • def getField(self, fieldName): ノードからフィールドのハンドラーを取得する.
    • def restartController(self): ロボット用のコントローラを再実行する.
  • class Supervisor (Robot): SupervisorクラスはRobotクラスを継承する.
    • def simulationReset(self): シミュレーションをリセットする.

 

真の自己位置の取得方法

  • 前回まで学んだコントローラはロボットの制御プログラムで,スーパーバイザ・コントローラは人間の操作に相当するものなので別物です.そのため,別プロセスで動かし並列動作させますが,コードとしては並列動作させるAPIなどは使いません.ロボット用コントローラのプログラムに関しては前回のrobot_car2.pyと同じものを使います.
  • スーパーバイザ・コントローラを使うために,まず,そのコントローラを新規追加します.そのコードはこの例では次のサンプルコードを使います.このプログラムでは,スーパーバイザAPIを使って,ロボットの真の位置を取得し,シミュレーションを再実行します.
    まず,2行目でスーパーバイザのAPIを使うために,コントローラモジュールからスーパーバイザクラスをインポートし,9行目でスーパーバイザクラスのインスタンスを生成します.12行目のsupervisor.getFromDef(“MyRobotCar”)は,スーパーバイザの対象となるノード(この例ではBmwX5)に対応するノードをシーンツリーから探し返します.なお,引数のvehicleは,オブジェクトのDEFネームでありシーンツリーのオブジェクト名(この例ではBmwX5)とは異なりますので注意しましょう.17,18行目でオブジェクトの位置と姿勢を取得するために,robot_node.getField(“translation”)でtranslationフィールドとrobot_node.getField(“rotation”)でrotationフィールドを取得しています.21,38行目のtrans_field.getSFVec3f()のSFは単一フィールド,Vec3F:は3次元ベクトルを表し,translationフィールドから真の位置を取得し,22行目のrot_field.getSFRotation() でrotationフィールドから真の姿勢を取得しています.
    32から36行目はシミュレーションとコントローラをリセットするコードです.15秒経過したら,34行目のsupervisor.simulationReset()でシミュレーションをリセットします.シミュレーションをリセットしてもコントローラはリセットされないので,35行目のrobot_node.restartController() でコントローラを再実行しています.このプログラムでは15秒毎にシミュレーションを再実行することになります.
  • サンプルコード
 ''' supervisor_robot_car.py '''
from controller import Supervisor # コントローラモジュールからスーパーバイザクラスをインポート
from vehicle import Driver        # vehcleモジュールからDriverクラスのインポート
from math import sqrt             # mathモジュールからsqrtクラスをインポート
import sys                        # sysモジュールをインポート

TIME_STEP = 10   
    
supervisor = Supervisor() # スーパーバイザクラスからインスタンスを生成

# 以下の処理はシミュレーションループの前に一度だけ呼び出す
robot_node = supervisor.getFromDef("MyRobotCar")

if robot_node is None:
    sys.stderr.write("The node is not found in the current world file\n")
    sys.exit(1)
trans_field = robot_node.getField("translation")  # translationフィールドを取得
rot_field   = robot_node.getField("rotation")     # rotationフィールドを取得

t0 = supervisor.getTime()             # 時刻の取得
init_pos = trans_field.getSFVec3f()   # translationフィールドから位置を取得
init_rot = rot_field.getSFRotation()  # rotationフィールドから姿勢を取得
print("init_pos=", init_pos)
print("init_rot=", init_rot)

print("*** Start the supervisor controller ***")

# シミュレーションループ
while supervisor.step(TIME_STEP) != -1:    
    t = supervisor.getTime() - t0

    if t >= 15:   # 15経過後
        print("*** Reset the simulation ***")
        supervisor.simulationReset()
        robot_node.restartController()         
        t0 = supervisor.getTime()
        
    values = trans_field.getSFVec3f()  #  translationフィールドから値(位置)を取得
    print("%f[s] Real position:(%g, %g, %g)[m]" % (t, values[0], values[1], values[2]))
  • 次に,スーパーバイザ・コントローラを割り当てるRobotノードをシーンツリーに追加します.ノードは下図の赤丸で囲んだボタンをクリックします.
  • 「ノードを追加」窓が現れるので,[Base nodes]から[Robot]ノードをクリックして,[追加]をクリックするとシーンツリーにRobotノードが追加されます.
  • Robotノードのcontrollerフィールドに説明したスーパーバイザー用のコントローラを割り当てます.supervisorフィールドTRUE,ロボットのコントローラと同期を取るためにsychronizationフィールドTRUEに設定します.シーンツリーのフィールドは次のように設定します.
    • controller: スーパーバイザ用のコントローラ(この例では”supervisor_robot_car”)
    • supervisor: TRUE
    • synchronization: TRUE

ハンズオン

  1. Webots入門講座3-2023:自動運転シミュレータ (Python)で実施した要領で,新規プロジェクトsupervisor_robot_carを作成して,ワールドcity.wbtをワールドsupervisor_robot_car.wbtに上書き保存する.
  2. Webots入門講座3で作成したrobot_car2.pyと同じプログラムも使うので,上で作成したプロジェクトsupervisor_robot_carに新規コントローラrobot_car2.pyを作成する.作成された雛形コードの中身を全部消して,Webots入門講座3のrobot_car2.pyのコードをコピーする.
  3. 次に,スーパーバイザ・コントローラを割り当てるRobotノードをシーンツリーに追加し,各フィールドを次のように設定する.
    • controller: スーパーバイザ用のコントローラ(この例では”supervisor_robot_car”)
    • supervisor: TRUE
    • synchronization: TRUE
  4. サンプルコードをコントローラプログラムとして,シミュレーションを実行して動作を確認しよう.コンソール(Cosole -All)には,ロボットのコントローラとスーパーバイザ・コントローラの出力が図のように表示されます.これを見るとこのシミュレータに搭載されているGPSは精度が高く,真値とさほど変わらないことがわかりますね.こんなGPSを使いたいものです.
  5. Webots入門講座3のハンズオンとこのサンプルプログラムを改変して,Webots入門講座3のコース1周走らせて,真値とGPSの値をプロットしてみよう.さらに,真値とどのぐらい誤差があるか定量的にも評価しましょう.

終わり

コメント

タイトルとURLをコピーしました