Drive donkeycar with 3 IR ToF sensors

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)

次回の宿題

・今回のように、決め打ちで回避行動をするしきい値を決めておけば、複数台でレースを行う上で動作がわかりやすく制御もしやすそうです。しかし、今後は回避行動を起こす、ちょうどよい距離等も学習値としてモデルに取り入れられないか、と考えており、次回の宿題とします。

おわり

次回はハロウィンレーサーの作り方！
ハロウィンとっくに終わってますが。。。