#Drive donkey with 3 ToF sensors：ドンキーカーに赤外線センサを載せて、回避行動させる。
こんばんはRomihiです。
実は本日初投稿、こちらのアドベントカレンダーでQiitaデビューです。
本日は赤外線センサを使った、Donkeycarの簡単な制御の方法を紹介します。
##まずは、赤外線センサを準備します。
ハロウィンレーサーに搭載した赤外線センサ：VL53L1X
| 諸元 | 値 |
|---|---|
| 解像度 | 1 mm |
| 最大範囲 | 400 cm |
| 最小範囲 | 4 cm |
| インタフェース | I²C |
| 最小動作電圧 | 2.6 V |
| 最大動作電圧 | 5.5 V |
| 電源電流 | 15 mA |
| *引用元SWITCHSCIENCE |
・Pololu(★オススメ) https://www.pololu.com/product/3415
＊左右に配置
・pimoroni https://shop.pimoroni.com/products/vl53l1x-breakout
＊中央に配置
★オススメの理由
(1) 小さい
Pololu：13 × 18 × 2 mm < pimoroni：19 × 19 × 3.2 mm
(2) XSHUTをLOWにすることで、各センサにアドレスを一つ一つ割り当てられる。
同じバス内に複数センサを接続する場合、デフォルトのアドレス(0x29)が同じのため、うまく通信ができません。
そのため、まずは全て(or一つを残し）のXSHUTをLOWにし,一つずつHIGHにする⇨アドレスを変える作業が必要です。
下記コード内、#change sensorの部分で実施しています。
##配線
通信にはI2Cを利用します。サーボドライバのI2Cバス線の足がついてない方が余っているので、足をはんだ付けして、使いました。同バス内の接続はGROVEシステムのI2Cハブを使って簡単に取り外しができるようにしています。
#次に、下記のコードを追加します。
DISTANCE.py
#場所~projects/donkeycar/donkeycar/parts/DISTANCE.py
#!/usr/bin/env python
import os
import time
import sys
import signal
import RPi.GPIO as GPIO
#Using vl53l1x library for python by pimoroni
#https://github.com/pimoroni/vl53l1x-python
#sudo pip install smbus2
#sudo pip install vl53l1x
#checked at ver 0.0.4
class DISTANCE:
''' 20190807 Using VL531X '''
def __init__(self, delay=0.001):
import VL53L1X
""" Open and start the VL53L1X ranging sensor"""
self.addr_default = 0x29
self.addr_R = 0x26
self.addr_Fr = 0x27
self.addr_L = 0x28
self.XSHUT1 = 37 #Right
self.XSHUT2 = 38 #Left
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BOARD)
#OFF
GPIO.setup(self.XSHUT1,GPIO.OUT)
GPIO.output(self.XSHUT1,GPIO.LOW)
GPIO.setup(self.XSHUT2,GPIO.OUT)
GPIO.output(self.XSHUT2,GPIO.LOW)
#ON
GPIO.setup(self.XSHUT1,GPIO.IN)
GPIO.setup(self.XSHUT2,GPIO.IN)
#OFF
GPIO.setup(self.XSHUT1,GPIO.OUT)
GPIO.output(self.XSHUT1,GPIO.LOW)
GPIO.setup(self.XSHUT2,GPIO.OUT)
GPIO.output(self.XSHUT2,GPIO.LOW)
#change sensor@default address to addr_Fr
try: VL53L1X.VL53L1X(i2c_bus=1, i2c_address=self.addr_Fr)
except:
print("change address for addr_Fr:{0}".format(self.addr_Fr))
self.tof_Fr = VL53L1X.VL53L1X(i2c_bus=1, i2c_address=self.addr_default)
self.tof_Fr.open() # Initialise the i2c bus and configure the sensor
self.tof_Fr.change_address(self.addr_Fr)
self.tof_Fr.close()
#change sensor@default address to addr_R
GPIO.setup(self.XSHUT1,GPIO.IN)
try: VL53L1X.VL53L1X(i2c_bus=1, i2c_address=self.addr_R)
except:
print("change address for addr_R:{0}".format(self.addr_R))
self.tof_R = VL53L1X.VL53L1X(i2c_bus=1, i2c_address=self.addr_default)
self.tof_R.open() # Initialise the i2c bus and configure the sensor
self.tof_R.change_address(self.addr_R)
self.tof_R.close()
#change sensor@default address to addr_L
GPIO.setup(self.XSHUT2,GPIO.IN)
try: VL53L1X.VL53L1X(i2c_bus=1, i2c_address=self.L)
except:
print("change address for addr_L:{0}".format(self.addr_L))
self.tof_L = VL53L1X.VL53L1X(i2c_bus=1, i2c_address=self.addr_default)
self.tof_L.open() # Initialise the i2c bus and configure the sensor
self.tof_L.change_address(self.addr_L)
self.tof_L.close()
self.tof_Fr = VL53L1X.VL53L1X(i2c_bus=1, i2c_address=self.addr_Fr)
self.tof_Fr.open() # Initialise the i2c bus and configure the sensor
self.tof_R = VL53L1X.VL53L1X(i2c_bus=1, i2c_address=self.addr_R)
self.tof_R.open() # Initialise the i2c bus and configure the sensor
self.tof_L = VL53L1X.VL53L1X(i2c_bus=1, i2c_address=self.addr_L)
self.tof_L.open() # Initialise the i2c bus and configure the sensor
self.tof_Fr.start_ranging(1) # Start ranging, 1 = Short Range, 2 = Medium Range, 3 = Long Range
self.tof_R.start_ranging(1) # Start ranging, 1 = Short Range, 2 = Medium Range, 3 = Long Range
self.tof_L.start_ranging(1) # Start ranging, 1 = Short Range, 2 = Medium Range, 3 = Long Range
self.distance_Fr =0.
self.distance_R =0.
self.distance_L =0.
self.delay = delay
self.sleeptime= 0.00000001 #0.00001
self.sleeptime2=0.00000001
self.on = True
self.count=0
def update(self):
while self.on:
self.GetDistance()
time.sleep(self.delay)
def measure_R(self):
distance_in_mm = self.tof_R.get_distance() # Grab the range in mm
return distance_in_mm
def measure_Fr(self):
distance_in_mm = self.tof_Fr.get_distance() # Grab the range in mm
return distance_in_mm
def measure_L(self):
distance_in_mm = self.tof_L.get_distance() # Grab the range in mm
return distance_in_mm
def GetDistance(self):
try:
self.distance_R=self.measure_R()
self.distance_Fr=self.measure_Fr()
self.distance_L=self.measure_L()
except:
print('failed to read DISTANCE!!')
def React(self,throttle,stearing):
#returning throttle and stearing values
stop_Fr=100
stop_RL=150
turn_Fr=300
turn_RL=300
if self.distance_R < stop_RL and self.distance_Fr < stop_Fr and self.distance_L < stop_RL:
print('wating the obstacle to move...')
return 0,0
elif self.distance_R > turn_RL and self.distance_Fr < turn_Fr and self.distance_L < turn_RL:
print('avoiding to Right!!')
return 0.5,-1
elif self.distance_R < turn_RL and self.distance_Fr < turn_Fr and self.distance_L > turn_RL:
print('avoiding to Left!!')
return 0.5,1
else: return throttle, stearing
def run_threaded(self):
self.count+=1
if self.count>14:
print("Distance_R_Fr_L:",self.distance_R,self.distance_Fr,self.distance_L)
self.count=0
return self.distance_R,self.distance_Fr,self.distance_L
def run(self):
self.GetDistance()
print("Distance_R_Fr_L:",self.distance_R,self.distance_Fr,self.distance_L)
return self.distance_R,self.distance_Fr,self.distance_L
def shutdown(self):
self.tof_R.stop_ranging() # Stop ranging
self.tof_R.close()
self.tof_Fr.stop_ranging() # Stop ranging
self.tof_Fr.close()
self.tof_L.stop_ranging() # Stop ranging
self.tof_L.close()
self.on = False
print()
sys.exit(0)
def exit_handler(self,signal, frame):
self.tof_R.stop_ranging() # Stop ranging
self.tof_R.close()
self.tof_Fr.stop_ranging() # Stop ranging
self.tof_Fr.close()
self.tof_L.stop_ranging() # Stop ranging
self.tof_L.close()
self.on = False
print()
sys.exit(0)
signal.signal(signal.SIGINT, exit_handler)
if __name__ == "__main__":
iter = 0
p = DISTANCE()
while iter < 100:
data = p.run()
print("Distance_R_Fr_L:",data)
time.sleep(0.2)
iter += 1
#次に、下記のコードをそれぞれのファイルに追加します。
はじめに、赤外線センサのフラッグを立てます。
myconfig.py
#場所:~mycar*/myconfig.py
'HAVE_DISTANCE = True '#when true, this add a ranging sensor'
まだまだ改善が必要ですが、今回は簡単な制御のために、
下記のしきい値を決め打ちで与え、推論値(pilot_angle, pilot_throttle)をセンサで測った距離によって、上書きします。
stop_Fr=200 ＊これ以下で停止。
turn_Fr=500 ＊~ stop_Fr=200 のとき、以下のしきい値を下回る場合、回避行動をすることとしてます。
turn_R=150
turn_L=180
manage.py
#場所:~mycar*/manage.py
class DriveMode:
stop_Fr=200
turn_Fr=500
turn_R=150
turn_L=180
def run(self, mode,
user_angle, user_throttle,
pilot_angle, pilot_throttle):
if mode == 'user':
return user_angle, user_throttle
elif mode == 'local_angle':
if vars(distances)["distance_Fr"]< self.stop_Fr:
print('wating the obstacle to move...')
return 0,0
elif vars(distances)["distance_Fr"] < self.turn_Fr and vars(distances)["distance_L"] > self.turn_L:
print('avoiding to Left!!')
return -1.0, pilot_throttle*1.0
elif vars(distances)["distance_R"] > self.turn_R and vars(distances)["distance_Fr"] < self.turn_L:
print('avoiding to Right!!')
return 1.0, pilot_throttle*1.0
else:
return pilot_angle, pilot_throttle * cfg.AI_THROTTLE_MULT
else:
return pilot_angle, pilot_throttle * cfg.AI_THROTTLE_MULT
ここでは、local_angleモードを利用してますが、モードを追加する場合は、controller.pyの
def toggle_mode(self):
以下の様に追加のモード(dist_user/local)を記述します。
controller.py
#場所:~~projects/donkeycar/donkeycar/parts/controller.py
def toggle_mode(self):
'''
switch modes from:
user: human controlled steer and throttle
local_angle: ai steering, human throttle
local: ai steering, ai throttle
'''
if self.mode == 'user':
self.mode = 'local_angle'
# elif self.mode == 'local_angle':
# self.mode = 'local'
elif self.mode == 'local_angle':
self.mode = 'dist_user'
elif self.mode == 'dist_user':
self.mode = 'dist_local'
elif self.mode == 'dist_local':
self.mode = 'local'
###
else:
self.mode = 'user'
print('new mode:', self.mode)
更に、DISTANCEの起動、V.addによりセンサから得た距離の値をjsonファイルに保存します。
manage.py
###DISTANCE
if cfg.HAVE_DISTANCE:
from donkeycar.parts.DISTANCE import DISTANCE
distances = DISTANCE()
V.add(distances, outputs=['distance_R','distance_Fr','distance_L'], threaded=True)
##次回の宿題
・今回のように、決め打ちで回避行動をするしきい値を決めておけば、複数台でレースを行う上で動作がわかりやすく制御もしやすそうです。しかし、今後は回避行動を起こす、ちょうどよい距離等も学習値としてモデルに取り入れられないか、と考えており、次回の宿題とします。
##おわり
次回はハロウィンレーサーの作り方！
ハロウィンとっくに終わってますが。。。