Webots講座6-2023：カメラを使った簡単な自動運転 (Python)


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

 

レファレンス

 

カメラの使い方

  • カメラセンサの使い方は次のとおりです．
    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，コントローラrobot_car_auto controller.pyを作成し，サンプルコードを実行しましょう．
    • import cv2でエラーが出る場合はopencvがインストールされていません．Anacondaの場合は，Anaconda Promptで次のコマンドでインストールしてください．
      • pip install opencv-python
  2. サンプルコードの巡行速度は20[km/h]です．これを60[km/h]，100[km/h]に変更してシミュレーションを実行してロボットカーの挙動を観察しましょう．どうなるかな？
  3. 巡航速度を上げてもロボットカーが転倒しないように，カーブで減速するようにしよう．
  4. サンプルコードは黄色ラインの中央を走行します．ロボットカーに交通法規を守らせましょう．テストコースが米国だと思い，黄色ラインの右側を走行するようにサンプルを変更しましょう．
    • ヒント：簡単な方法は，78～95行目のcalcSteerinigASngle()メソッドのあるパラメータを変更すれば実現できます．
  5. 51～55行目のmaFilter()移動平均フィルタを実装しましょう．
  6. このサンプルでのステアリング角の制御は比例制御です．よりスムーズに黄色ラインに追従できるようにPID制御に変更してみよう．

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

終わり

コメント

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