Webots講座8-2022:キーボード入力によるマニュアル操作(Python)


Webots講座の8回目です.引き続きWebotsの自動運転シミュレータを使っていきます.今回はキーボードによるマニュアル操作を導入します.これでゲームのように車を操作できますね.

レファレンス

キーボードの使い方

  • キーボードの使い方は次のとおりです.
    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()

ハンズオン

  1. Webots入門講座3:自動運転シミュレータ (Python)を実施した要領で,新規プロジェクトrobot_car,コントローラrobot_car_lidar.pyを作成し,サンプルコードを実行しましょう.
  2. サンプルコードを実行すると初めは自動運転モードでロボットカーが進みます.RIGHT(→),LEFT(←),’S’,’s’キーを押すとマニュアル運転モードになります.マニュアルモードでロボットをしばらく走らせてみましょう!’A’,’a’キーを押すと自動運手モードに切り替わります.
  3. このサンプルでは障害物回避は組み込まれていないので,自動運転モードにそれを組み込んでコースを1周するプログラムを作成して,マニュアル運転モードと比較してください.

終わり

コメント

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