Webots講座の８回目です．今までWebots入門講座という名称でしたが，今後発展的な内容も増えてくると思うのでWebots講座に変更しました．さて，引き続きWebotsの自動運転シミュレータを使っていきます．今回はキーボードによるマニュアル操作を導入します．これでゲームのように車を操作できますね．
レファレンス
- Keyboard (Cyberbotics Ltd.)
- Webots for Automobiles (Cyberbotics Ltd.)
- 参考にしたコード
- Webots/projects/vehicle/controllers/autonomous_vehile/autnomous_vehicle.c
キーボードの使い方
- キーボードの使い方は次のとおりです．
- 有効化
- keyboard.enable(TIME_STEP)メソッドでキーボードを有効化する．TIME_STEPはサンプリングの間隔[ms]．
- 入力キーの取得
- メインループの中で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()
ハンズオン
- Webots入門講座３：自動運転シミュレータ (Python)を実施した要領で，新規プロジェクトrobot_car，コントローラrobot_car_lidar.pyを作成し，サンプルコードを実行しましょう．
- サンプルコードを実行すると初めは自動運転モードでロボットカーが進みます．RIGHT(→)，LEFT(←)，’S’，’s’キーを押すとマニュアル運転モードになります．マニュアルモードでロボットをしばらく走らせてみましょう！’A’，’a’キーを押すと自動運手モードに切り替わります．
- このサンプルでは障害物回避は組み込まれていないので，自動運転モードにそれを組み込んでコースを１周するプログラムを作成して，マニュアル運転モードと比較してください．
終わり
コメント