このページは金沢工業大学大学院機械工学専攻ロボット知能工学特論のサポートページです。第１１週はWebotsの自動運転シミュレータにウェイポイントナビゲーションとデッドレコニングを実装します．

レファレンス

• Webots for Automobiles (Cyberbotics Ltd.)

ハンズオン

• 次の記事に従って順番にハンズオンを進めてください．プチプロジェクトがファイナルプロジェクトの中間マイルストーンになります．
  • Webots入門講座４：自分の位置を知ろう (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ノードをシーンツリーに追加し，そのcontrollerフィールドに説明したスーパーバイザー用のコントローラを割り当てます．supervisorフィールドをTRUE，ロボットのコントローラと同期を取るためにsychronizationフィールドをTRUEに設定します．シーンツリーのフィールドは次のように設定します．
  • controller: スーパーバイザ用のコントローラ（この例では”supervisor_robot_car”)
  • supervisor: TRUE
  • synchronization: TRUE

ハンズオン

1. Webots入門講座３：自動運転シミュレータ (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の値をプロットしてみよう．さらに，真値とどのぐらい誤差があるか定量的にも評価しましょう．

終わり

Webots入門講座の６回目です．引き続きWebotsの自動運転シミュレータを使っていきます．今回はカメラセンサの使い方とそれを使った簡単な自動運転を実現します．コースの中央にある黄色ラインを追従するサンプルプログラムを紹介します．また，プログラムが前回より長くなってくるのでクラス化します．

レファレンス

• Camera  (Cyberbotics Ltd.)
• Webots for Automobiles (Cyberbotics Ltd.)
• 参考にしたコード
  • Webots/projects/vehicle/controllers/autonomous_vehile/autnomous_vehicle.c

カメラの使い方

• カメラセンサの使い方は次のとおりです．

1. 1. カメラデバイスの取得
      • camera = driver.getDevice(“camera”)でカメラデバイスを取得する．
   2. カメラの有効化
      • camera.enable(TIME_STEP)メソッドでカメラを有効化する．
   3. カメラの各パラメータの取得
      • camera.getWidth(), camera.getHeight(), camera.getFov()などでカメラの画像処理に必要なパラメータを取得する．
   4. カメラ画像の取得
      • camera_image = camera.getImage()でカメラ画像を取得する．
      • cv_image = np.frombuffer(camera_image, np.uint8).reshape((self.camera_height, self.camera_width, 4))でopencv用の画像フォーマットに変換する．
   5. カメラ画像の処理
      • opencvのAPIなどを使い必要な画像処理を行う．

API

• class Camera (Device):
  • def enable(self, samplingPeriod):カメラを有効にする．samplingPeriodはサンプリング時間[ms]．
  • def disable(self):カメラを無効にする．
  • def getFov(self):カメラのFOV(Field of View, 視野角) [rad]を取得する．
  • def getHeight(self):カメラ画像の高さ[pixel]を取得
  • def getImage(self):カメラ画像を取得する．画像は各画素のB(blue), G(green), R(red), A(alpha)の４バイトの並びでコード化されている．画素は画像の左上から右下まで水平方法に格納されている．画像サイズ[Byte]は次の式で表される．
    • byte_size = camera_width *  camera_height * 4
  • def getImageArray(self):カメラ画像配列を取得する．このメソッドの戻り値はlist<list<list<int>>>となり，ピクセルのRGB値を直接扱えるがgetImageと比較すると，非常に処理が遅いので，お勧めしない．
  • def getSamplingPeriod(self):サンプリング時間[ms]を取得する．
  • def getWidth(self): カメラ画像の幅[pixel]を取得

サンプルコード

• このサンプルコードは次のコードをPythonとクラス化し入門講座に合うように改変しています．
  • Webots/projects/vehicle/controllers/autonomous_vehile/autnomous_vehicle.c
• ソースコードが長くなってくるので，RobotCarクラスを作り，必要な定数，変数やメソッドをまとめます．ソースコードをざっと説明します．まず，131～132行目はこのrobot_car_auto.pyをモジュールとして使うための表記で，126～128行目のmain関数でRobotCarクラスのインスタンスを作成し，runメソッドで実行しています．
• 98～122行目のrunメソッドでシミュレータ車道の中央にある黄色線を追従する制御を実現しています．100～122行目の処理を繰り返し実行し，104行目はシミュレーションのステップ時間が10[ms]に対して，GPSやカメラセンサのサンプリング時間は60[ms]と遅いので，センサのデータ取得をセンサのサンプリング時間に合わせています．これをしないとシミュレーションがとても遅くなってしまう上に無駄な処理をすることになります．109行目でカメラから画像データを取得し，111行目でOpenCVで処理できる画像フォーマットに変換しています．114行目のmaFilter()はハンズオンで移動平均フィルタを実装するためのメソッドですが，このサンプルでは何も処理をしていません．calcSteeringAngle(cv_image)で黄色ラインを追従するためのステアリング角を計算しています．
• 78～95行目のcalcSteeringAngle()メソッドでは，画像中の黄色ピクセルを検出して，その平均位置（水平方向）とcamera_fov[rad]からステアリング角を計算しています．
• 残りのコードはソースコードにコメントを入れているのでそれを参照してください．

"""robot_car_auto controller"""
import cv2
import numpy as np
from vehicle import Driver
from controller import GPS, Node

""" ロボットカークラス """
class RobotCar():
    SPEED   = 20        # 巡行速度[km/h]
    UNKNOWN = 99999.99  # 黄色ラインを検出できない時の値
    TIME_STEP   = 60    # センサのサンプリング時間[ms]
        
    """ コンストラクタ """
    def __init__(self):  
        # Welcome message
        self.welcomeMessage()    
      
        # Driver
        self.driver = Driver()
        self.driver.setSteeringAngle(0)
        self.driver.setCruisingSpeed(self.SPEED) 
    
        # GPS
        self.gps = self.driver.getDevice("gps")
        self.gps.enable(self.TIME_STEP)

        # Camera
        self.camera = self.driver.getDevice("camera")
        self.camera.enable(self.TIME_STEP)
        self.camera_width  = self.camera.getWidth()
        self.camera_height = self.camera.getHeight()
        self.camera_fov    = self.camera.getFov()
        print("camera: width=%d height=%d fov=%g" % \
            (self.camera_width, self.camera_height, self.camera_fov))

        # Display
        self.display = self.driver.getDevice("display")
        self.display.attachCamera(self.camera) # show camera image
        self.display.setColor(0xFF0000)
        
      
    """ ウエルカムメッセージ """    
    def welcomeMessage(self):
        print("*********************************************")
        print("*   Welcome to a simple robot car program   *")
        print("*********************************************")       
                

    """ 移動平均フィルタを実装しよう．"""
    def maFilter(self, new_value):
        """
        ここに移動平均フィルタのコードを書く
        """   
        return new_value

    
    """ ステアリングの制御： wheel_angleが正だと右折，負だと左折 """
    def control(self, steering_angle):
        LIMIT_ANGLE = 0.5 # ステアリング角の限界 [rad]
        if steering_angle > LIMIT_ANGLE:
            steering_angle = LIMIT_ANGLE
        elif steering_angle < -LIMIT_ANGLE:
            steering_angle = -LIMIT_ANGLE
        self.driver.setSteeringAngle(steering_angle)
    
    
    """ 画素と黄色の差の平均を計算 """
    def colorDiff(self, pixel, yellow):
        d, diff = 0, 0
        for i in range (0,3):
            d = abs(pixel[i] - yellow[i])
            diff += d 
        return diff/3


    """ 黄色ラインを追従するための操舵角の計算　"""
    def calcSteeringAngle(self,image):
        YELLOW = [95, 187, 203]   # 黄色の値 (BGR フォーマット)
        sumx = 0                  # 黄色ピクセルのX座標の和
        pixel_count = 0           # 黄色の総ピクセル数
        for y in range(int(1.0*self.camera_height/3.0),self.camera_height):
            for x in range(0,self.camera_width):
                if self.colorDiff(image[y,x], YELLOW) < 30: # 黄色の範囲
                    sumx += x                      # 黄色ピクセルのx座標の和
                    pixel_count += 1               # 黄色ピクセル数の和
   
        # 黄色ピクセルを検出できない時は no pixels was detected...
        if pixel_count == 0:
            return self.UNKNOWN
        else:
            y_ave = float(sumx) /(pixel_count * self.camera_width) 
            steer_angle = (y_ave - 0.5) * self.camera_fov
            # print("dev=%g y_ave=%g sumx=%d pixel_count=%d" % (dev, y_ave, sumx, pixel_count))
            return steer_angle
                
    """ 実行 """
    def run(self): 
        step = -1       
        while self.driver.step() != -1:
            step +=1 
            BASIC_TIME_STEP = 10 # シミュレーションの更新は10[ms]
            # センサの更新をTIME_STEP[ms]にする．この例では60[ms] 
            if step % int(self.TIME_STEP / BASIC_TIME_STEP) == 0:
                # GPS
                values = self.gps.getValues()       

                # Camera
                camera_image = self.camera.getImage()
                # OpenCVのPythonは画像をnumpy配列で扱うので変換する．            
                cv_image = np.frombuffer(camera_image, np.uint8).reshape((self.camera_height, self.camera_width, 4))

                # 操作量（ステアリング角度）の計算
                steering_angle = self.maFilter(self.calcSteeringAngle(cv_image))

                if steering_angle != self.UNKNOWN:
                    print("%d:Find the yellow line" % step)
                    self.driver.setCruisingSpeed(self.SPEED)  # 走行速度の設定                     
                    self.control(steering_angle)              # ステアリングの制御(比例制御の簡易版)
                else: 
                    print("%d:Lost the yellow line" % step)
                    self.driver.setCruisingSpeed(0) # 追従する黄色線をロストしたので停止する．

        
""" メイン関数 """ 
def main():  
    robot_car = RobotCar() 
    robot_car.run()  
    
""" このスクリプトをモジュールとして使うための表記 """
if __name__ == '__main__':
    main()

ハンズオン

1. Webots入門講座３：自動運転シミュレータ (Python)で実施した要領で，新規プロジェクトrobot_car_auto controller，コントローラrobot_car_auto controller.pyを作成し，サンプルコードを実行しましょう．
2. サンプルコードの巡行速度は20[km/h]です．これを60[km/h]，100[km/h]に変更してシミュレーションを実行してロボットカーの挙動を観察しましょう．どうなるかな？
3. 巡航速度を上げてもロボットカーが転倒しないように，カーブで減速するようにしよう．
4. サンプルコードは黄色ラインの中央を走行します．ロボットカーに交通法規を守らせましょう．テストコースが米国だと思い，黄色ラインの右側を走行するようにサンプルを変更しましょう．
   • ヒント：簡単な方法は，78～95行目のcalcSteerinigASngle()メソッドのあるパラメータを変更すれば実現できます．
5. 51～55行目のmaFilter()移動平均フィルタを実装しましょう．
6. このサンプルでのステアリング角の制御は比例制御です．よりスムーズに黄色ラインに追従できるようにPID制御に変更してみよう．

なお，サンプルを実行すると路上に置いてあるドラム缶に衝突します．これを避けるためのLIDARセンサの使い方は次回のWebots入門講座で学びます．お楽しみに！

終わり

この記事はWIP(Work in Progress)です．サンプルコードの説明をほとんどしていないので，時間を見つけて加筆します．

Webots入門講座の７回目です．引き続きWebotsの自動運転シミュレータを使っていきます．今回はLIDAR(Laser Detection and Ranging)センサの使い方とそれを使った簡単な障害物回避を実現します．

レファレンス

• Lidar  (Cyberbotics Ltd.)
• Webots for Automobiles (Cyberbotics Ltd.)
• 参考にしたコード
  • Webots/projects/vehicle/controllers/autonomous_vehile/autnomous_vehicle.c

LIDARの使い方

• LIDARセンサの使い方は次のとおりです．
  1.  デバイスの取得
     • lidar = driver.getDevice(“LIDARデバイス名”)でLIDARデバイスを取得する．この例ではLIDARデバイス名は”Sick LMS 291″．
  2.  有効化
     • lidar.enable(TIME_STEP)メソッドでLIDARを有効化する．
  3.  各パラメータの取得
     • lidar.getHorizontalResolutiion(), lidar.getMaxRange(), lidar.getFov()などでLIDARのデータ処理に必要なパラメータを取得する．
  4.  データの取得
     • lidar_data = lidar.getRageImage()でLIDARの距離データを取得する.
  5.  データ処理
     • 障害物までの方位や距離など必要なデータ処理を行う．

API

• class Lidar (Device):
  • def enable(self, samplingPeriod):有効化
  • def disable(self):無効化
  • def getFov(self):水平方向の視野角(Fov) [rad]を取得
  • def getHorizontalResolution(self):水平方向解像度の取得
  • def getMaxRange(self):最大検出距離の取得[m]
  • def getMinRange(self):最小検出距離の取得[m]
  • def getRangeImage(self):距離データ[m]の取得．1次元の浮動小数点のリストを返す．
  • def getSamplingPeriod(self):サンプリング時間の取得[ms]

サンプルコード

• このサンプルコードは次のコードをPythonとクラス化し入門講座に合うように改変しています．元のコードとは，障害物検出が異なっており，このコードでは，長さが進行方向20[m]，幅が車幅+衝突余裕の矩形領域内を障害物検出範囲としています．
  • Webots/projects/vehicle/controllers/autonomous_vehile/autnomous_vehicle.c

"""robot_car_lidar.py
Webots/projects/vehicle/controllers/autonomous_vehile/autnomous_vehicle.cを参考にしています．
"""
import cv2
import math
import numpy as np
from vehicle import Driver
from controller import GPS, Node

""" ロボットカークラス """
class RobotCar():
    SPEED   = 20        # 巡行速度
    UNKNOWN = 99999.99  # 黄色ラインを検出できない時の値
    FILTER_SIZE = 3     # 黄色ライン用のフィルタ
    TIME_STEP   = 30    # センサのサンプリング時間[ms]
    CAR_WIDTH   = 2.015 # 車幅[m]
    CAR_LENGTH  = 5.0   # 車長[m]    
    
        
    """ コンストラクタ """
    def __init__(self):  
        # Welcome message
        self.welcomeMessage()    
      
        # Driver
        self.driver = Driver()
        self.driver.setSteeringAngle(0)
        self.driver.setCruisingSpeed(self.SPEED) 
        self.driver.setDippedBeams(True) # ヘッドライト転倒 
        
        # LIDAR (SICK LMS 291)
        self.lidar = self.driver.getDevice("Sick LMS 291")
        self.lidar.enable(self.TIME_STEP)
        self.lidar_width = self.lidar.getHorizontalResolution()
        self.lidar_range = self.lidar.getMaxRange()
        self.lidar_fov   = self.lidar.getFov();       
        
        # GPS
        self.gps = self.driver.getDevice("gps")
        self.gps.enable(self.TIME_STEP)

        # Camera
        self.camera = self.driver.getDevice("camera")
        self.camera.enable(self.TIME_STEP)
        self.camera_width  = self.camera.getWidth()
        self.camera_height = self.camera.getHeight()
        self.camera_fov    = self.camera.getFov()
        print("camera: width=%d height=%d fov=%g" % \
            (self.camera_width, self.camera_height, self.camera_fov))

        # Display
        self.display = self.driver.getDevice("display")
        self.display.attachCamera(self.camera) # show camera image
        self.display.setColor(0xFF0000)
        
      
    """ ウエルカムメッセージ """    
    def welcomeMessage(self):
        print("*********************************************")
        print("*   Welcome to a simple robot car program   *")
        print("*********************************************")       
                

    """ 移動平均フィルタを実装しよう．"""
    def maFilter(self, new_value):
        """
        ここに移動平均フィルタのコードを書く
        """   
        return new_value

    
    """ ステアリングの制御： wheel_angleが正だと右折，負だと左折 """
    def control(self, steering_angle):
        LIMIT_ANGLE = 0.5 # ステアリング角の限界 [rad]
        if steering_angle > LIMIT_ANGLE:
            steering_angle = LIMIT_ANGLE
        elif steering_angle < -LIMIT_ANGLE:
            steering_angle = -LIMIT_ANGLE
        self.driver.setSteeringAngle(steering_angle)
    
    
    """ 画素と黄色の差の平均を計算 """
    def colorDiff(self, pixel, yellow):
        d, diff = 0, 0
        for i in range (0,3):
            d = abs(pixel[i] - yellow[i])
            diff += d 
        return diff/3


    """ 黄色ラインを追従するための操舵角の計算　"""
    def calcSteeringAngle(self,image):
        YELLOW = [95, 187, 203]   # 黄色の値 (BGR フォーマット)
        sumx = 0                  # 黄色ピクセルのX座標の和
        pixel_count = 0           # 黄色の総ピクセル数
        for y in range(int(1.0*self.camera_height/3.0),self.camera_height):
            for x in range(0,self.camera_width):
                if self.colorDiff(image[y,x], YELLOW) < 30: # 黄色の範囲
                    sumx += x                      # 黄色ピクセルのx座標の和
                    pixel_count += 1               # 黄色ピクセル数の和
   
        # 黄色ピクセルを検出できない時は no pixels was detected...
        if pixel_count == 0:
            return self.UNKNOWN
        else:
            y_ave = float(sumx) /(pixel_count * self.camera_width) 
            steer_angle = (y_ave - 0.5) * self.camera_fov
            # print("dev=%g y_ave=%g sumx=%d pixel_count=%d" % (dev, y_ave, sumx, pixel_count))
            return steer_angle
                

    """ 障害物の方位と距離を返す. 障害物を発見できないときはUNKNOWNを返す．
        ロボットカー正面の矩形領域に障害物がある検出する    
    """
    def calcObstacleAngleDist(self, lidar_data):
        OBSTACLE_HALF_ANGLE = 20.0 # 障害物検出の角度．中央の左右20[°]．
        OBSTACLE_DIST_MAX   = 20.0 # 障害物検出の最大距離[m]
        OBSTACLE_MARGIN     = 0.1  # 障害物回避時の横方向の余裕[m]
        
        sumx = 0
        collision_count = 0
        obstacle_dist   = 0.0
        for i in range(int(self.lidar_width/2 - OBSTACLE_HALF_ANGLE), \
            int(self.lidar_width/2 + OBSTACLE_HALF_ANGLE)):
            dist = lidar_data[i]
            if dist < OBSTACLE_DIST_MAX: sumx += i collision_count += 1 obstacle_dist += dist if collision_count == 0 or obstacle_dist > collision_count * OBSTACLE_DIST_MAX:  # 障害物を検出できない場合
            return self.UNKNOWN, self.UNKNOWN
        else:
            obstacle_angle = (float(sumx) /(collision_count * self.lidar_width) - 0.5) * self.lidar_fov
            obstacle_dist  = obstacle_dist/collision_count
            
            if abs(obstacle_dist * math.sin(obstacle_angle)) < 0.5 * self.CAR_WIDTH + OBSTACLE_MARGIN:
                return obstacle_angle, obstacle_dist # 衝突するとき
            else:
                return self.UNKNOWN, self.UNKNOWN


    """ 実行 """
    def run(self): 
        stop =  False
        step = 0       
        while self.driver.step() != -1:
            step +=1 
            BASIC_TIME_STEP = 10 # シミュレーションの更新は10[ms]
            
            if stop == True:
                print("Stop!")
                #self.driver.setBrakeIntensity(1.0) # ブレーキをかける．
                self.driver.setCruisingSpeed(0)     # 非常停止 
               
            # センサの更新をTIME_STEP[ms]にする．この例では60[ms] 
            if step % int(self.TIME_STEP / BASIC_TIME_STEP) == 0:
                # Liar
                lidar_data = self.lidar.getRangeImage()   
                
                # GPS
                values = self.gps.getValues()       

                # Camera
                camera_image = self.camera.getImage()
                # OpenCVのPythonは画像をnumpy配列で扱うので変換する．            
                cv_image = np.frombuffer(camera_image, np.uint8).reshape((self.camera_height, self.camera_width, 4))

                # 障害物回避
                obstacle_angle, obstacle_dist = self.calcObstacleAngleDist(lidar_data)
                STOP_DIST = 5 # 停止距離[m]
                
                if obstacle_dist < STOP_DIST:
                    print("%d:Find obstacles(angle=%g, dist=%g)" % \
                        (step, obstacle_angle, obstacle_dist))
                    stop = True
                else:                
                    # 操作量（ステアリング角度）の計算
                    steering_angle = self.maFilter(self.calcSteeringAngle(cv_image))

                    if steering_angle != self.UNKNOWN:
                        print("%d:Find the yellow line" % step)
                        self.driver.setCruisingSpeed(self.SPEED)  # 走行速度の設定                     
                        self.control(steering_angle)              # ステアリングの制御(比例制御の簡易版)
                        stop = False
                    else: 
                        print("%d:Lost the yellow line" % step)
                        self.driver.setCruisingSpeed(0) # 追従する黄色線をロストしたので停止する．
                        # self.driver.setBrakeIntensity(0.5) # 追従する黄色線をロストしたので停止する．
                        stop = True

        
""" メイン関数 """ 
def main():  
    robot_car = RobotCar() 
    robot_car.run()  
    
""" このスクリプトをモジュールとして使うための表記 """
if __name__ == '__main__':
    main()

ハンズオン

1. Webots入門講座３：自動運転シミュレータ (Python)を実施した要領で，新規プロジェクトrobot_car，コントローラrobot_car_lidar.pyを作成し，サンプルコードを実行しましょう．
2. サンプルコードを実行すると初めに遭遇するドラム缶２個は通り過ぎますが，３番目のドラム缶は正面にあるので急停止します．それを避けて，元の黄色ラインを追従するプログラムを何も調べずに自分で良く考えて実装しよう．
3. 障害物回避にポテンシャル法を実装してみよう．

終わり

このページは金沢工業大学大学院機械工学専攻ロボット知能工学特論のサポートページです。第１２週はWebotsの自動運転シミュレータに，カメラを使いコース中央にある黄色ラインを追従する制御プログラムとLIDARを使った障害物回避を実装します．

レファレンス

• Webots for Automobiles (Cyberbotics Ltd.)

ハンズオン

• 次の記事に従って順番にハンズオンを進めてください．デッドレコニングの実装がファイナルプロジェクトの中間マイルストーンになります．
  • Webots入門講座６：カメラの使い方と簡単な自動運転 (Python)
  • Webots入門講座７：LIDARの使い方と障害物回避 (Python)

終わり

Webots講座の８回目です．今までWebots入門講座という名称でしたが，今後発展的な内容も増えてくると思うのでWebots講座に変更しました．さて，引き続きWebotsの自動運転シミュレータを使っていきます．今回はキーボードによるマニュアル操作を導入します．これでゲームのように車を操作できますね．

レファレンス

• Keyboard (Cyberbotics Ltd.)
• Webots for Automobiles (Cyberbotics Ltd.)
• 参考にしたコード
  • Webots/projects/vehicle/controllers/autonomous_vehile/autnomous_vehicle.c

キーボードの使い方

• キーボードの使い方は次のとおりです．
  1.   有効化
     • keyboard.enable(TIME_STEP)メソッドでキーボードを有効化する．TIME_STEPはサンプリングの間隔[ms]．
  2.  入力キーの取得
     • メインループの中でkeyboard.getKey()を呼び出して入力されたキーを取得する.
     • pythonの場合は入力されたキー値はUnicodeになっているので，比較するキーの値もordメソッドで変換する必要がある．

API

• class Keyboard:
  # キーの値
  END, HOME, LEFT, UP, RIGHT, DOWN, PAGEUP,
  PAGEDOWN, NUMPAD_HOME, NUMPAD_LEFT, NUMPAD_UP,
  NUMPAD_RIGHT, NUMPAD_DOWN, NUMPAD_END, KEY, SHIFT,
  CONTROL, ALT
  • • def enable(self, samplingPeriod):キーボード入力を有効化する．
    • def disable(self):無効化する．
    • def getSamplingPeriod(self):サンプリング期間を取得する
  • def getKey(self):キーの値を取得する．

サンプルコード

• キーボード入力に関する部分だけ説明します．その他は以前とあまり変わっていません．
• キーボードを使うために7行目でcontrollerモジュールからKeyboardクラスをインポートしています．63行目でKeyboardクラスのインスタンスkeyboardを作っています．
• 179～203行目のcheckKeyboard()メソッドでキーボードからの入力されたキーの値（Unicode)とUP，Down, Left, Right, ’A’，’a’, ‘S’, ‘s’を比較して次の必要な処理をしています．このメソッドはrun()メソッドのメインループの235行目で呼び出されています．
  • UP: 速度指令値を5[km/h]増加
  • DOWN: 速度指令値を5[km/h]減速
  • LEFT: 操舵指令値を左に1[°]増加
  • RIGHT: 操舵指令値を右に1[°]増加
  • ‘A’, ‘a’：自動運転モード
  • ‘S’, ‘s’：速度指令値を0[km/h]に設定．停止．

"""robot_car controller."""
import cv2
import math
import numpy as np
from vehicle import Driver
from controller import GPS, Node
from controller import Keyboard    

""" ロボットカークラス """
class RobotCar():
    CRUSING_SPEED =  20     # 巡行速度[km/h]
    MAX_SPEED     = 250     # 最大速度[km/h]
    LIMIT_STEERING_ANGLE = 0.5 # 操舵角の最大値[rad]
    UNKNOWN       = 99999.99  # 黄色ラインを検出できない時の値
    FILTER_SIZE   = 3     # 黄色ライン用のフィルタ
    TIME_STEP     = 30    # センサのサンプリング時間[ms]
    CAR_WIDTH     = 2.015 # 車幅[m]
    CAR_LENGTH    = 5.0   # 車長[m]    
    DEBUG         = False # デバッグ用の情報表示
    
        
    """ コンストラクタ """
    def __init__(self):  
        # Welcome message
        self.welcomeMessage()    

        self.cmd_speed          = self.CRUSING_SPEED # 速度指令値
        self.cmd_steering_angle = 0                  # 操舵角指令地
        
        
        # Driver
        self.driver = Driver()
        self.driver.setSteeringAngle(self.cmd_steering_angle)
        self.driver.setCruisingSpeed(self.cmd_speed) 
        self.driver.setDippedBeams(True) # ヘッドライト転倒
        
        # LIDAR (SICK LMS 291)
        self.lidar = self.driver.getDevice("Sick LMS 291")
        self.lidar.enable(self.TIME_STEP)
        self.lidar_width = self.lidar.getHorizontalResolution()
        self.lidar_range = self.lidar.getMaxRange()
        self.lidar_fov   = self.lidar.getFov();       
        
        # GPS
        self.gps = self.driver.getDevice("gps")
        self.gps.enable(self.TIME_STEP)

        # Camera
        self.camera = self.driver.getDevice("camera")
        self.camera.enable(self.TIME_STEP)
        self.camera_width  = self.camera.getWidth()
        self.camera_height = self.camera.getHeight()
        self.camera_fov    = self.camera.getFov()
        print("camera: width=%d height=%d fov=%g" % \
            (self.camera_width, self.camera_height, self.camera_fov))

        # Display
        self.display = self.driver.getDevice("display")
        self.display.attachCamera(self.camera) # show camera image
        self.display.setColor(0xFFFFFF)
        self.display.setFont('Arial', 12, True)        
        # Keyboard
        self.keyboard = Keyboard()
        
        # Steer Angle
        self.manual_steering = 0.0
        
        # sdrive
        self.auto_drive = True        

      
    """ ウエルカムメッセージ """    
    def welcomeMessage(self):
        print("*********************************************")
        print("*   Welcome to a simple robot car program   *")
        print("*********************************************")  
        print("*** Auto Drive Mode ***")
        print("Input [A] key to Auto Drive Mode")     
             

    """ 移動平均フィルタを実装しよう．"""
    def maFilter(self, new_value):
        """
        ここに移動平均フィルタのコードを書く
        """   
        return new_value

    
    """ ステアリングの制御： wheel_angleが正だと右折，負だと左折 """
    def control(self, steering_angle):
        # ここにPID制御などをコーディングする．
        return steering_angle
    
    
    """ 画素と黄色の差の平均を計算 """
    def colorDiff(self, pixel, yellow):
        d, diff = 0, 0
        for i in range (0,3):
            d = abs(pixel[i] - yellow[i])
            diff += d 
        return diff/3


    """ 黄色ラインを追従するための操舵角の計算　"""
    def calcSteeringAngle(self,image):
        YELLOW = [95, 187, 203]   # 黄色の値 (BGR フォーマット)
        sumx = 0                  # 黄色ピクセルのX座標の和
        pixel_count = 0           # 黄色の総ピクセル数
        for y in range(int(1.0*self.camera_height/3.0),self.camera_height):
            for x in range(0,self.camera_width):
                if self.colorDiff(image[y,x], YELLOW) < 30: # 黄色の範囲
                    sumx += x                      # 黄色ピクセルのx座標の和
                    pixel_count += 1               # 黄色ピクセル数の和
   
        # 黄色ピクセルを検出できない時は no pixels was detected...
        if pixel_count == 0:
            return self.UNKNOWN
        else:
            y_ave = float(sumx) /(pixel_count * self.camera_width) 
            steer_angle = (y_ave - 0.5) * self.camera_fov
            # print("dev=%g y_ave=%g sumx=%d pixel_count=%d" % (dev, y_ave, sumx, pixel_count))
            return steer_angle             


    """ 障害物の方位と距離を返す. 障害物を発見できないときはUNKNOWNを返す．
        ロボットカー正面の矩形領域に障害物がある検出する    
    """
    def calcObstacleAngleDist(self, lidar_data):
        OBSTACLE_HALF_ANGLE = 20.0 # 障害物検出の角度．中央の左右20[°]．
        OBSTACLE_DIST_MAX   = 20.0 # 障害物検出の最大距離[m]
        OBSTACLE_MARGIN     = 0.1  # 障害物回避時の横方向の余裕[m]
        
        sumx = 0
        collision_count = 0
        obstacle_dist   = 0.0
        for i in range(int(self.lidar_width/2 - OBSTACLE_HALF_ANGLE), \
            int(self.lidar_width/2 + OBSTACLE_HALF_ANGLE)):
            dist = lidar_data[i]
            if dist < OBSTACLE_DIST_MAX: 
                sumx += i 
                collision_count += 1 
                obstacle_dist += dist 

        if collision_count == 0 or obstacle_dist > collision_count * OBSTACLE_DIST_MAX:  # 障害物を検出できない場合
            return self.UNKNOWN, self.UNKNOWN
        else:
            obstacle_angle = (float(sumx) /(collision_count * self.lidar_width) - 0.5) * self.lidar_fov
            obstacle_dist  = obstacle_dist/collision_count
            
            if abs(obstacle_dist * math.sin(obstacle_angle)) < 0.5 * self.CAR_WIDTH + OBSTACLE_MARGIN: 
                return obstacle_angle, obstacle_dist # 衝突するとき 
            else: 
                return self.UNKNOWN, self.UNKNOWN


    """ 速度の設定 """
    def setSpeed(self, speed): 
        if speed > self.MAX_SPEED:  # [km/h]
            speed = self.MAX_SPEED
        elif speed < - self.MAX_SPEED: 
            speed = -self.MAX_SPEED
        self.driver.setCruisingSpeed(speed) 


    """ ステアリング角度の手動変更 """ 
    def setSteeringAngle(self, angle): 
        if self.auto_drive == False: 
            print("*** Manual Drive Mode ***") 
            print("Input [A] key to Auto-Drive Mode")

        if angle > self.LIMIT_STEERING_ANGLE: # [rad]
            angle =  self.LIMIT_STEERING_ANGLE
        elif angle < -self.LIMIT_STEERING_ANGLE: 
            angle = - self.LIMIT_STEERING_ANGLE 

        self.driver.setSteeringAngle(angle) 

 
    """ キーボードの入力チェック """ 
    def checkKeyboard(self): 
        key = self.keyboard.getKey() 
        if key == self.keyboard.UP: 
            #print("UP key is pushed")
            self.cmd_speed += 5.0 
        elif key == self.keyboard.DOWN: 
            #print("DOWN key is pushed") 
            self.cmd_speed -= 5.0 
        elif key == self.keyboard.RIGHT: 
            #print("Right key is pushed") 
            self.auto_drive = False 
            self.cmd_steering_angle += math.pi/ 180.0 # 1[deg]をradianに変換
        elif key == self.keyboard.LEFT: 
            #print("LEFT key is pushed") 
            self.auto_drive = False 
            self.cmd_steering_angle -= math.pi/ 180.0 
        elif key == ord('A') or key == ord('a'): # ord関数は文字のUnicode値を返す
            self.auto_drive = True 
            #print("A/a key is pushed") 
            print("*** Auto Drive Msode ***") 
        elif key == ord('S') or key == ord('s'): # ord関数は文字のUnicode値を返す  
            self.auto_drive = False
            #print("S/s key is pushed ") 
            self.cmd_speed = 0 

    """ 描画更新 """ 
    def updateDisplay(self): # 3Dシミュレーションウインドウに文字を表示
        gps_speed = self.gps.getSpeed()* 3.6 # [km\h] 

        if gps_speed > self.MAX_SPEED:
            return
        
        self.display.setColor(0x000000)
        self.display.fillRectangle(10,10, 90,20)
        txt = "%.1f [km/h]" % float(self.gps.getSpeed()* 3.6)
        # print(txt)
        self.display.setColor(0xFFFFFF)
        self.display.drawText(txt, 18, 15)


    """ 実行 """
    def run(self): 
        step = 0
        
        # キーボー入力の有効化
        self.keyboard.enable(self.TIME_STEP)
        
        # main loop       
        while self.driver.step() != -1:
            step +=1 
            BASIC_TIME_STEP = 10 # シミュレーションの更新は10[ms] 
           
            # センサの更新をTIME_STEP[ms]にする．この例では60[ms] 
            if step % int(self.TIME_STEP / BASIC_TIME_STEP) == 0:
                # キーボード入力
                self.checkKeyboard()
            
                # Liar
                lidar_data = self.lidar.getRangeImage()   
                
                # GPS
                values = self.gps.getValues()                   
                
                # Camera
                camera_image = self.camera.getImage()
                # OpenCVのPythonは画像をnumpy配列で扱うので変換する．            
                cv_image = np.frombuffer(camera_image, np.uint8).reshape((self.camera_height, self.camera_width, 4))

                # 障害物回避
                obstacle_angle, obstacle_dist = self.calcObstacleAngleDist(lidar_data)
                STOP_DIST = 5 # 停止距離[m]
                
                if obstacle_dist < STOP_DIST:
                    if self.DEBUG == True:
                        print("%d:Find obstacles(angle=%g, dist=%g)" % \
                        (step, obstacle_angle, obstacle_dist))
                    self.cmd_speed = 0 # 障害物があるので速度指令値を0に設定
                else:
                    if self.auto_drive == False:
                        pass
                    else:                                   
                        # 操作量（ステアリング角度）の計算
                        steering_angle = self.maFilter(self.calcSteeringAngle(cv_image))

                        if steering_angle != self.UNKNOWN:
                            if self.DEBUG == True:
                                print("%d:Find the yellow line" % step)
                            self.cmd_speed = self.CRUSING_SPEED # 速度指令値の設定              
                            self.cmd_steering_angle = self.control(steering_angle) # ステアリングの制御(比例制御の簡易版)
                        else: 
                            if self.DEBUG == True:
                                print("%d:Lost the yellow line" % step)
                            self.cmd_speed = 0  # 追従する黄色線をロストしたので速度指令値を0に設定する．
                            # self.driver.setBrakeIntensity(0.5) # 追従する黄色線をロストしたので停止する．
               

                # 描画更新
                self.updateDisplay()
                
                # 速度指令
                self.setSpeed(self.cmd_speed)
                
                # 操舵指令
                self.setSteeringAngle(self.cmd_steering_angle)    

        
""" メイン関数 """ 
def main():  
    robot_car = RobotCar() 
    robot_car.run()  
 
    
""" このスクリプトをモジュールとして使うための表記 """
if __name__ == '__main__':
    main()

ハンズオン

7/17(土)，18(日)にKITオープンキャンパスが開催されます．ロボティクス学科のブースでは出村研究室が協働型ロボットSawyerを使い未来のコンビニをテーマとしたFuture Convenience Store Challenge(FCSC)のデモを行います．事前申込された方は是非，見に来てください．詳細は次のリンクをご覧ください．なお，新型コロナ対策のため予約制になっており新規受付は終了しています．

• KIT OPEN CAMPUS

お待ちしています．

Webots講座の9回目です．Webots講座4:自分の位置を知ろう(Python)でデッドレコニングを実装するハンズオンがありました．デッドレコニングを実装するには，そのロボットのホイールベースなどのパラメータがわらないと実装できません．WebotsではPROTOという仕組みがあり，そのPROTOファイルにロボットのパラメータが書かれています．では，引き続き自動運転シミュレータで使ってきたロボットカーBMW X5のPROTOファイルを見てみましょう．

レファレンス

• PROTO (Cyberbotics Ltd.)
• Tutorial 7: Your First PROTO (Cyberbotics Ltd.)
• AckermannVehicle (Cyberbotics Ltd.)
• Webots for Automobiles (Cyberbotics Ltd.)

PROTOファイル

• Webotsの自動運転シミュレータの車モデルはアッカーマン操舵機構で，その車(アッカーマンビークル）のパラメータはPROTOファイルでは次のとおりです．このうち，デッドレコニングにはフロント/リアの左右の車輪間隔となるtrackFront/trackRear，ホイールベースwheelbaseが必要な情報となります．
  • trackFront/trackRear: フロント/リアの左右の車輪間隔
  • wheelbase: ホイールベース．フロントとリア軸間の距離
  • minSteeringAngle/maxSteeringAngle: フロント車輪の最少/最大操舵角
  • suspension...: サスペンション諸元の定義
  • wheelsDampingConstant:　各車輪ジョイントのdampingConstant（減衰定数)．車の摩擦のシミュレーション計算に使われる．
  • maxSteeringTorque: フロントホイールのy軸にかかる最大トルク
  • extensionSlot: センサや車の形状などの他ノードを追加
  • boundingObject: 車の物理ジオメトリ．なお，物理ジオメトリは衝突の対象となる物体の属性．
  • physics: 車の物理パラメータ
  • radarCrossSection: レーダ反射断面積．0以上の場合，レーダセンサに検出される可能性がある．
  • recognitionColors: 空欄でなければ，カメラセンサにより認識される可能性がある．
  • wheelX: VehicleWheel を追加するスロット
  • axisDevicesX: 車輪ジョイントにBrake, PositionSensor,Motorなどのデバイスを追加するスロット
  • data:  Robot ノードのユーザ定義データ（文字列）
• なお，車の位置(0, 0, 0)はリアホイール軸の中心であり，extensionSlotのどのノードもこの中心からの相対位置になります．Transformノードはこの中心からのextensionノードを移動させます．また，AckermannVehicle PROTOを継承することで簡単に自分のPROTOを作成できます．

ハンズオン

• Webotsを起動．
• cityワールドを開く．
  • メニュー→ファイル→Open Sample World…→vehicles→city.wbt
• シーンツリーのBmwX5を右マウスボタンでクリックして，開いた項目から[view PROTO source]を選択する．
  
• テキストエディタにBmwX5のPROTOファイルBmwX5protoが表示される．長いので全部掲載できないがアッカーマン機構に関連する部分は67～69行目となる．
  
• Webots講座４：自分の位置を知ろう (Python)のプチプロジェクトにBMW X5のデッドレコニング実装があります．上のパラメータ等を使い実装してみよう！

終わり

Webotsが約7か月ぶりに2021bにバージョンアップしました．結構変わっているので開発が盛んなんですね．詳細は次のリンクをご覧ください．なお，上の図は新しく追加されたNVIDIAのJetbot．

• https://cyberbotics.com/doc/reference/changelog-r2021

終わり

KIT夏のオープンキャンパスが2021年7月17,18日に開催されました．ロボティクス学科出村研究室ブースではRethink Robotics社の協働型ロボットSawyerを使ったコンビニタスクのデモを実施しました．その説明動画(2:55)です．よく見るとSawyerは関節に直列にバネが入っているので動作を停止しても多少動いているのがわかります．関節が柔らかいので，その特性を生かしてハメ合い作業などを簡単に実現できます．なお，ハンドは研究室で開発しました．

Webots講座の10回目です．今までPythonを使っていましたが，C言語でプログラミングしてみましょう．

レファレンス

• Webots User Guide (Cyberbotics, Inc.)

環　境

• Windows 10
• Webots R2021b：この記事で使っているJetBotはR2021bから新たなロボットモデルとして追加されました．

はじめてのWebotsプログラミング

• 準　備
  • プロジェクトを格納するフォルダとして”C:\ユーザ\ユーザ名\webots”をエクスプローラで作成する．
• プロジェクトの作成
  • メニューの[ウィザード]→[新規プロジェクトのディレクトリ…]を選択する．ディレクトリはフォルダのこと．下の窓が開くので[次へ(N)]をクリックする．
    
  • 「ディレクトリの選択」窓になるのでmy_projectをhello_worldに変更して[次へ(N)]をクリックする．下のdemuraは私のユーザ名なので，自分のユーザ名に変更する．
    
  • 「World settings」窓になる．ワールドはシミュレータ上の仮想世界になり，ロボットはそのワールドで動作します．ここでは，ワールド名はhello_world.wbtに変更し，一番下の”Add a rectangle arena”にチェックを入れて[次へ(N)]をクリックする．ワールド名の最後のwbtはワールドファイルの拡張子となります．
  • 「結果」窓になるので[完了(F)]をクリックする．
    
  • 市松模様のボードRectangleArena（四角いアリーナ）が下図のように現れる．
    
  • 次に，このRectangleArenaの属性を変えてみよう！左欄の上から５番目にあるRectangleArenaをダブルクリックすると展開され属性が現れる．まず，床のサイズを変更しよう．floorSizeをクリックすると，下欄[Selection floorSize (Vector2)]のxとyに2を入力するとサイズが縦横２倍になる．同様にして，floorTileSizeやwallHeightなどを変えて遊んでみよう．変更したらメニューバーの[ファイル]→[ワードファイルを保存]を選択してワールドを保存する．
    
• ロボットの追加
  • 次にロボットをアリーナに登場させよう．ロボットはいろいろ用意されているが，ここではWebotsバージョン2021bから追加されたNVIDIAのJetBotに登場願おう．RectangelArenaをクリックして，その上にある[＋]をクリックすると[ノードを追加]窓が開くので，[PROTO nodes (Webots Projects)]→[robots]→[nvidia]→[jetbot]→[JetBot (Robot)]を選択して[追加]をクリックする．
    
  • 下図のように真ん中に黒い四角形のウインドウが現れてアリーナが良く見えない．黒い四角形の右上の×印をクリックしてウインドウを閉じる．
    
  • 下図のようにJetBotが半分アリーナに埋もれた状態で現れる．これを正常な姿勢にするために，シーンツリーの[JetBot “JetBot”]→[rotation]をクリックする．
    
  • シーンツリーの下に[Selection  rotation (Rotation)]のウインドウが開くので次の値を入力して，ロボットをx軸周りに-1.57[rad]回転する．これで正常な姿勢になった．メニューから[ファイル]→[ワールドファイルを保存]する．なお，下図の赤矢印はx軸，緑矢印はy軸，青矢印はz軸となっています．
    • x:1, y:0, z:0, angle:-1.57
      
• コントローラの作成
  • ワールドとロボットを作ったので，次はコントローラを作成する．Webotsではコントローラをプログラミングすることでロボットを動かすことができます．メニューバーから[ウィザード]→[新規ロボットのコントローラ…]を選択する．「新規コントローラの作成」窓が開くので[次へ(N)]をクリックする．
    
  • 「言語選択」窓が開くので”C”にチェックを入れ，[次へ(N)]をクリックする．
    
  • 「IDE selection」窓になるので，Webots (gcc/ Makefile)にチェックを入れて，[次へ(N)]をクリック．
    
  • 「名前の選択」窓になるので，コントローラの名前を入力する．ここでは”hello_world”にする．[次へ(N)]をクリック．
    
  • 「結果」窓になるので[完了(F)]をクリックする．
    
  • テキストエディタが開き，次のコントローラの雛形プログラムが自動的に作られる．
    
  • ここでは，上の雛形コードを下のように変更する．プログラミング言語で定番の初めてのコードといえば，”hello, world”ですね．変更したらエディタの上にあるフロッピーディスクの形をしたsaveアイコンをクリックして保存し，歯車の形をしたbuildボタンでコードをビルドする．なお，ビルドしただけではプログラムは動きません．

    #include <stdio.h> 
    #include <webots/robot.h>
    
    int main(int argc, char **argv) {
      wb_robot_init();
    
      while (wb_robot_step(64) != -1) {
        printf("hello, world\n");
      }
    
      wb_robot_cleanup();
      return 0;
    }
    

ソースコードを説明します．１行目で標準入出力用のヘッダファイル，2行目でWebotsに必要なヘッダファイルをインクルードします．5行目でWebotsを初期化し，11行目で終了処理をしています．7行目のwb_robot_step(64)はシミュレータのタイムステップを表し，1ループで64[ms]シミュレーションを進めるという意味です．この時間を小さくすると計算精度は高くなりますが，シミュレーションは遅くなります．逆に大きくするとシミュレーションは速くなりますが，精度が悪くなり，シミュレータと実世界でのロボットの動きが変わってしまいます．このコントローラはロボットを動かすのではなく，シミュレータが終わるまで，”hello, world”とコンソールに出力し続けるだけです．

• ロボットへのコントローラ組込み
  • 次にこのコントローラプログラムをロボットに組込みます．組み込んでから実行させないとプログラムは動作しません．左欄の[JetBot]→[controller]を選択し，下の[Selection controller (String)]欄の[選択…]をクリックするとコントローラの一覧が現れるので，その中から先ほど作成した”hello_world”をクリックして組み込みます．和すらないうちにワールドをメニューから保存します．
    
• 実　行
  • プログラムを実行するためには，まず，シミュレーション表示部分の上にある，[reset]ボタンを押して，シミュレーションをリセットして初期化します．次に，[run]ボタンを押すとプログラムが実行され，下欄の[Console – All]に “hello, world”と表示されます．今回のプログラムはコンソールに”hello world”と表示するだけの簡単なものです．次回は車輪型ロボットを自作してプログラムで動かしますのでお楽しみに！

ハンズオン

• 新しいプロジェクトを作成して，次のハンズオンを実施してください．
  • 市松模様のアリーナRectangleArenaのサイズを変更してみよう．
  • 好きなロボットをアリーナに登場させよう．
  • コンソールに”I am Happy Robot”と表示するプログラムを作成しよう．

終わり

Webots講座9までは自動運転シミュレータを使い，予め作られたロボットカー(BMW X5)にプログラミングしてきました．Webots講座11からは自作ロボットの作り方を学んでいきましょう．本記事はレファレンスのWebots User Guide Tutorial 6を参考に制作しました．作るロボットは自律移動ロボットでは定番の左右駆動輪の速度差によって進行方向が変わる差動駆動型ロボットです．ロボットの前後にキャスターとして球を本体に埋め込んでいます．

なお，Webotsのデフォルトの座標系はロボットの進行方向がz軸：青，左手方向がx軸：赤，上方向がｚ軸：緑となっています．

レファレンス

• Tutorial 6: 4-Wheeled Robot (Cyberbotics Ltd.）

ロボットの仕様

• サイズ：直径 0.35[m]，高さ 0.1[m]
• 重量：4.0 [kg]
• 駆動方式：差動駆動
• 車輪数：４（駆動輪２，受動輪2）
• 駆動輪直径：0.08[m]

プロジェクトの作成

•  まず，次のWebots作業用ディレクトリがなければエクスプローラで作成する．
  • C:\ユーザー\ユーザ名\webots
• Webotsを起動する．
• 新規プロジェクトを作成する．Webots講座２で実施した新規プロジェクトする方法でwheel1プロジェクトを作成する．
  • メニュー→ウィザード→新規プロジェクトディレクトリ
  • ディレクトリの選択：”C:\Users\ユーザ名\webots\wheel1″と入力し[次へ(N)]をクリックする．
    
  • World settings: ワールド名をwheel1.wbtにし，次のように全ての項目にチェックを入れ[次へ(N)]をクリックする．
    
  • 次のように表示されるので[完了(F)]をクリックする．
    
  • 3Dウインドウに市松模様のボードRectangleArena（四角いアリーナ）が下図のように現れる．
    

Robotノードの追加

• シーンツリーの最後にRobotノードを追加する．
  
• Robotノードのchildrenの次のノードを追加する．追加の方法はRobotノードのchildrenを右マウスボタンクリックして[新規の追加]を選択する．Solidはロボット前後のキャスター，HingeJointは駆動輪の回転軸，Shapeはロボット本体用である．
  • Solid
  • Solid
  • HingeJoint
  • HingeJoint
  • Shape
    
• Shape：ロボット本体用を次のように設定する．
  • DEF: BODY
  • appearance: PBRAppearance
    • baseColor: 1 1 0
    • metalness: 0
  • geometry: Cylinder
    • height: 0.1
    • radius: 0.175
      
• boundingObject: [新規の追加]→[USE]→Body (Shape)
  • USE: Body (Shape)
•  physics: [新規の追加]→[Base nodes]→Physics
  • Physics
    
• HingeJoint: 左車輪用
  • DEF: HINGE_LEFT
  • jointParameters: HingeJointParameters
    • axis 1 0 0
    • anchor -0.175 -0.03 0
  • device
    • PositionSensor: “position sensor”
    • RotationMotor: “motor_left”
  • endPoint: Solid
    • DEF: WHEEL
    • translation: 0.175  -0.03  0
    • rotation: 0 0 1 1.57
    • children: Shapeを追加
      • DEF：WHEEL
      • appearance: PBRAppearance
        • baseColor: 0 0 0
        • metalness: 0
      • geometry: Cylinder
        • height: 0.15
        • radius: 0.039
    • boundingObject: USE Wheel
    • physics: Physics
      • DEF: PHYSICS_WHEEL
        
• 右車輪用のHingeJointは上の左車輪用HingeJointノードに右マウスボタンをクリックしてコピペする．変更箇所は以下とDEF(定義)でLEFTとなっているところをRIGHTにする．
  • • jointParameters
      • axis 1 0 0
      • anchor -0.175 -0.03 0
    • endPoint
      • translation: -0.175  -0.03  0

• Solid: 前キャスター用
  • DEF：CASTER_FRONT
  • translation: 0 -0.05 0.15
  • children
    • Shape
      • appearance: PBRAppearance
      • geometry Sphere
        • radius: 0.02
  • boundingObject: Sphere
    • radius: 0.02
  • physics: Physics
• Solid: 後キャスター用．前キャスター用のDEF CASTER_FRONT Solidノードをコピペして以下の項目を変更する．
  • DEF: CASTER_REAR
  • translation: 0 -0.05 -0.15
    
• なお，ボールキャスターをシミュレーションするために，ボールジョイントで球と本体を拘束しても良いですが，簡単のためとシミュレーションが遅くなるので，このような実装にしています．

コントローラの作成

• • Webots講座10で実施した新規ロボットのコントローラ作成と同じ要領です．
  • メニューの [ウィザード]→[新規ロボットのコントローラ…]から以下を選択する．
    • 言語選択：C
    • IDE selection: Webots (gcc / Makefile)
    • コントローラの名前：wheel1
  • ソースコード：下のコードを上の開いたテクストエディタに貼り付けてビルドする．ソースコードの説明はコメントを参照して欲しい．

/*　wheel1.c　*/
#include 
#include <webots/motor.h>
#include <webots/robot.h>

/* main関数 */
int main() {
  WbDeviceTag motor_left, motor_right; /* デバイスの宣言 */

  wb_robot_init();  /* webotsの初期化 */  

  motor_left  = wb_robot_get_device("motor_left");  /* デバイスの取得 */
  motor_right = wb_robot_get_device("motor_right");   
  wb_motor_set_position(motor_left,  INFINITY);     /*　モータ位置の設定           */
  wb_motor_set_position(motor_right, INFINITY);     /* 速度制御時はINFINITYに設定 */

  while (wb_robot_step(64) != -1) {
    wb_motor_set_velocity(motor_left,  3.0);        /* 回転速度の設定 [rad/s]    */
    wb_motor_set_velocity(motor_right, 5.0); 
  } 
  
  wb_robot_cleanup(); /* webotsの終了処理 */

  return 0;
}

ロボットへのコントローラ組込み

• 次にこのコントローラプログラムをロボットに組込みます．組み込まないとプログラムは動作しません．左欄の[Robot “robot”]→[controller]を選択し，下の[Selection controller (String)]欄の[選択…]をクリックするとコントローラの一覧が現れるので，その中から先ほど作成した”wheel1”をクリックして組み込みます．ワールドを保存する．

実　行

• プログラムを実行するためには，まず，シミュレーション表示部分の上にある，[reset]ボタンを押して，シミュレーションをリセットして初期化します．次に，[run]ボタンを押すとプログラムが実行され，ロボットが反時計周りに大きく回ります．

ハンズオン

1. 上の説明に従って自作ロボットモデルをつくりプログラムを実行してみよう．ワールドファイルやコントローラプログラムの入ったwheel1プロジェクトフォルダは次のリンクからダウンロードできます．
   • wheel1-20200723.zip
2. 直径1[m]の円の軌跡を進むコントローラプログラムを作成しよう．
3. ５秒間前進して，１秒停止，５秒間後進して元の位置に戻るコントローラプログラムを作成しよう．ヒント：メインループは１回64[ms]です．
4. １辺が１ｍの正方形の軌跡になるように進むコントローラプログラムを作成しよう．wb_motor_set_velocity()の引数の回転速度[rad/s]と車輪の半径0.04[m]から移動速度を計算してください．
5. 上のプログラムでは速度指令値をもとに進んだ距離を計算しているので正確ではありません．車軸にはPositionSensorがついているので，この値を使い正確な正方形の軌跡に進むようなコントローラプログラムを作成しよう．

終わり

KITのウェブサイトで出村研究室の取り組みが紹介されました．

金沢工業大学ロボティクス学科と三協立山株式会社タテヤマアドバンス社が、ロボットが操作する次世代スライド棚の共同研究を開始。ロボット競技会World Robot Summit 2020での実証実験を計画 | ニュース | KIT 金沢工業大学

www.kanazawa-it.ac.jp

ICT教育ニュースにも出村研究室の取り組みが紹介されました．

金沢工業大学、産学連携でロボットが操作する次世代スライド棚の共同研究を開始

金沢工業大学は7日、同大学ロボティクス学科が三協立山タテヤマアドバンス社と、近未来のコンビニエンスストアでの使用を想定し、ロボットが操作する次世代スライド棚の共同研究を開始したと発

ict-enews.net

金沢工業大学ロボティクス学科出村研究室のHappy Robotチームは経済産業所とNEDOが主催する国際ロボット競技会WRS2020フューチャーコンビニエンスストアチャレンジの遠隔部門に参加します．

• World Robot Summit公式ウェブサイト

WRS2020 Junior競技カテゴリーの ホームロボットチャレンジ（リアルサイズ）でYumekobo JuniorチームがRobotics for Happiness賞を受賞しました．おめでとうございます！

なお，Yumekobo Juniorは金沢工業大学ロボティクス学科の教育研究活動の一環で，出村教授がアドバイザーを務めています．今回のYumekobo Juniorチームのメンバーは金沢市立工業高校2年1名，金沢錦丘高校1年2名，金沢二水高校1年1名の生徒４名で，中には小学生からYumekobo Juniorの活動に参加したり，RoboCup Juniorの全国大会に参加した生徒もいます．

長年の活動が結果になったといえるかもしれませんね．おめでとうございます！

WRS2020ジュニア競技カテゴリーホームロボットチャレンジ（リアルサイズ）に出場した金沢工業大学夢考房RoboCup@HomeプロジェクトKIT Happy Robotチームは２位でした．おめでとうござます！

1年生主体のチームなので入学してから約５か月の成果です．コロナ禍で活動が制限される中，あきらめず最後まで努力したのが結果につながったと思います．デモはパーフェクトでした．今後は，英語でのプレゼンに磨きをかければ全てパーフェクトになると思います．1年生主体のチームだけに今後が楽しみです．

本講義では、金沢工業大学基礎実技教育課程の西川教授から強力なサポートを受けてETロボコンをロボティクス学科向けにアレンジしたロボコンをテーマとして、組込みプログラミングの基礎を楽しく学びます。

講義日：木曜日３，４限

•  場所
  • 講義室：23号館221室
  • 演習室：コラボレーションスタジオ/パフォ―ミングスタジオ
• 成績
  • 個人点：５０点
    • 小テスト５回各１０点
  • チーム点：５０点
    • チーム技術資料：２０点
      • 中間提出：５点、本提出：１５点
    • プレゼンテーション（ポスター発表）：２０点
      • 中間提出：５点、本提出：１５点
    • 競技成績：１０点
•  １週目(9/25)：ガイダンス・開発環境のインストール
  • 担当教員、TA、ＳＡの紹介（講義室）
  • 授業の概要説明（講義室）
    • 本資料の説明
    • ETロボコンの説明
  • PD実践：LEGO Mindstorms EV3 開発環境のインストール（出村；講義室）
    • TA、SAは教材準備が終了したら講義室へ移動し、学生をサポートする。
• ２週目(9/30)：開発環境・LEGO組立演習
  • 演習の説明（全員：講義室）
    • 班の発表（１組、２組混合）
  • PD実践：プログラムのビルド・アップロード・実行（全員：演習室）
  • LEGO部品チェック
  • LEGO組立（全員：演習室）
    • 付属冊子の機体
  • 小テスト１（個人）
• ３週目(10/7)：センサ演習
  • 演習の説明（全員：講義室）
  • プログラミング演習センサ（全員：演習室）
  • 小テスト２（個人）
• ４週目(10/14)：センサ演習続き、モータ演習
  • 演習の説明（全員：講義室）
  • プログラミング演習（モータ）
  • 小テスト３（個人）
• ５週目(10/28)：モータ演習
  • 演習の説明（全員：講義室）
  • プログラミング演習（モータ演習２）
  • 小テスト４（個人）
• 6週目(11/4)：状態遷移図演習
  • 演習の説明（全員：講義室）
  • 状態遷移図演習
  • 小テスト５（個人）
• 7週目(11/11)：マルチタスク演習
  • 演習の説明（全員：講義室）
  • 状態遷移図演習
  • 競技１の説明
• 8週目(11/18)：競技準備
• 9週(11/25)：競技準備
• 10週(12/2)：競技１、チーム技術資料中間提出(チームレポート)
• 11週(12/9)：競技２説明（準備フォーミングスタジオ、競技場設置）
• 12週(12/16)：競技準備
• 13週(12/23)：トライアル（パフォーミングスタジオ、競技場設置）、プレゼン準備
• 14週(1/20)：競技会（パフォーミングスタジオ）
• 15週(1/27)：プレゼンテーション（ポスター発表）、チーム技術資料提出(チームレポート)、自己点検授業

1. 開発環境(MSYS2)のインストール
   • 以下のリンクの資料(PDF)をダウンロードして，その説明に従ってインストールする．
     • Windows 7/8/8.1/10 + MSYS2 環境構築方法（ETロボコン実行委員会提供）
   • 注意：上のインストール資料P17の手順①「下記のリンクからmkimageのバイナリをダウンロード」に指定されているURLにアクセスすると，私の環境では，”AccessDenied”と怒られ，ダウンロードできなかった．その場合は，以下のリンクフォルダPD実践の中にMkimage.tar.gzがあるのでダウンロードする．学内ネットワークに接続しないとダウンロードできないので注意．
     • PD実践
2. 開発プラットフオームEV3RTのインストール
   • パッケージバージョン1.1をダウンロードする。
     • ev3rt-1.1-release.zip
   • MSYSターミナル(スタート→MSYS2 64bit→MSYS2 MSYS)を起動する．以下のコマンドを入力してEnterキーを押して，作業用のフォルダーev3rtを作る．mkdirコマンドはフォルダ(ディレクトリ)を作るコマンド．なお、$はプロンプト(入力の目印)なので入力しない
     • $ mkdir /ev3rt
   • ダウンロードしたEV3RTのパッケージ(ev3rt-1.1-release.zip) を解凍し、hrp3.tar.xzだけを“作業フォルダーC:¥msys64¥ev3rt (msysでは/ev3rt)の中にマウスを使ってコピペする。なお、hrp3.tar.xzはレゴを動かすOSのカーネル・ソースコードが格納されている。
   • MSYSターミナルで，以下のコマンドで作業フォルダー(/ev3rt)に移動する．cdコマンドはフォルダを移動するコマンド．
     • $ cd /ev3rt
   • 起動したMSYSターミナルで以下のコマンドを実行してhrp3.tar.xzを展開する．少し，長いので打ち間違えないためにコピペすると良い。なお、$はプロンプトなのでコピペしない。
     • $ tar xvf hrp3.tar.xz
   • MSYSターミナルで次のコマンドで，ビルドに必要なファイルをインストールする．gemコマンドはRuby言語のパッケージ管理用コマンド．
     • $ gem install shell
   • 以上で開発環境のインストールは終わりです。お疲れ様！
3. サンプルアプリケーションのビルドと実行（以下の補足説明は重要）
   • MSYSターミナルで次のコマンドを入力し”workspace” フォルダーに移動する. なお、$はプロンプトなのでコピペしない。
     • $  cd  /ev3rt/hrp3/sdk/workspace
   • 手順③：サンプルプログラム”helloev3”をビルドする。
     • helloev3フォルダーがあることを確認するために、以下のコマンドを実行する。lsコマンドはフォルダにあるファイルを表示する．
       • $  ls  helloev3
     • helloev3フォルダがあると以下のファイルが表示される。表示されない場合は、作業２の[開発プラットフオームEV3RTのインストール]がうまくいっていないのでやり直す。
       • app.c  app.cfg  app.h  cli_main.c  cli_motor.c  cli_sensor.c  Makefile.inc
     • 次のmakeコマンドでビルドする。
       • $ make app=helloev3
     • 成功するとappというバイナリファイル（実行形式ファイル）が生成される。次のlsコマンドを実行するとディレクトリの内のファイルが表示される。appというファイルがあれば成功。
       • $ ls

参考サイト

• Toppers/EV3 RT 開発環境構築のマニュアル
• 開発プラットフォームEV3RTのダウンロード＆インストール

この記事はKITロボティクス学科プログラミング言語の授業用です。

ロボティクス学科対象のプログラミング言語（出村担当）ではC言語のエディタとして今年度からVSCodiumを使います。VS CodiumはマイクロソフトのVS Code (Visual Studio Code)を完全オープンソース化したものです。Microsoftのロゴ商標などがありません。それ以外はほぼ同じです。

VS CodeはいろいろなOSで使われとても人気のあるエディタですが、初心者には設定などが結構あり敷居が高いです。そこでVS Codeのポータブル機能を使い予め私が授業用に設定したものをダウンロードしてもらいあとは起動するだけですぐ使えます。なお、この方法は加藤丈和さんのサイトを参考にしました。ありがとうございます。

参考サイト

• VSCode (VSCodium) によるポータブルなC言語開発環境の構築

ダウンロード

• ここをクリックしてデスクトップにダウンロードする（サイズ358MB、学内ネットワークのみダウンロード可）

インストール

• デスクトップに保存したcprog20.zipを解凍して、できたcprog20ディレクトリ(フォルダ)をCドライブ直下に移動する。これだけでインストールは終わり。

実　行

• 移動したフォルダの中にあるStart.cmdをダブルクリックする。デスクトップにショートカットを作っておくと少し幸せになれるでしょう。
  
• 次のようなウインドウが開けば成功です。
  
• 終わり。お疲れ様。今回はとても簡単でしたね．