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

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

レファレンス

• Supervisor Programming (Cyberbotics Ltd.)
• Webots for Automobiles (Cyberbotics Ltd.)
• GPSサンプルコード
  • WEBOTS_HOME/projects/samples/devices/worlds/gps.wbt
    • WEBOTS_HOMEはwebotsをインストールしているディレクトリ．

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-2022：自動運転シミュレータ (Python)で実施した要領で，新規プロジェクトsupervisor_robot_carを作成して，ワールドcity.wbtをワールドsupervisor_robot_car.wbtに上書き保存する．
2. Webots入門講座３で作成した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入門講座３のハンズオンとこのサンプルプログラムを改変して，Webots入門講座３のコース１周走らせて，真値とGPSの値をプロットしてみよう．さらに，真値とどのぐらい誤差があるか定量的にも評価しましょう．

終わり