Donkey Carに機能を組み込んでみる

#やりたいこと RealSense T256をDonkey Carから使う

Tawn Kramer氏がすでにやっていましたので真似します。
ただ、DonekyCarの作りがよくわからないので、ソースを読みながら追っていきます。

#Donkey Carの構造をざっと調べる
ソースコードをざっとみて、どこを改造するか調べてみる。
RealSense T256の実行結果を何らか表示するには、以下の改造が必要。

1. センサのインスタンスを追加する場所
2. センサからのデータをDonkey serverに保持させる
3. Donkey serverのデータをhttpで出力する

以上の項目を実装するために必要な改造をどこに、どのように行うかを調べてみる。

#donkeycarクラスはどこ？

```
import donkeycar as dk
```

Donkey Carクラスの大元だと思いますが、検索かけても、class donkeycarが出てきません。
全てのパーツはこれを起点に読み込んでいるように見えますが、どうなんでしょうか？

```__init__.py
import os

import pkg_resources  # part of setuptools
import sys

__version__ = pkg_resources.require("donkeycar")[0].version
print('using donkey version: {} ...'.format(__version__))

current_module = sys.modules[__name__]

if sys.version_info.major < 3:
    msg = 'Donkey Requires Python 3.4 or greater. You are using {}'.format(sys.version)
    raise ValueError(msg)

from . import parts
from .vehicle import Vehicle
from .memory import Memory
from . import util
from . import config
from .config import load_config
```

よく見るdk.vihicle.の片鱗らしいものが、ここに見えます。
donkey carはmanage.pyで動的に生成しているので、多分、ここがdonkeycarを構成しているところと推測しています。
ここで、
donkeycar.vehicle
donkeycar.memory
donkeycar.config
その他のパーツをdonkeycarのメンバーとして組み入れているのでしょう。
このファイルだけで理解できない文章がいっぱい出てきます。
細部にこだわると、先に進めないので、一旦保留にして、Vehicleの中を見ることにします。

##Pythonの文法メモ１

>from . import parts

これは、同じディレクトリにある複数のファイルをインポートするための記述。

いちいち全部インポートするのが面倒なら、パッケージを作って全部読み込む。
http://flame-blaze.net/archives/4963

このページが参考になりそう。

#VehicleにAdd()する

Vehicle.part[]にバーツを追加して、周期を回すメインクラスです。
周期で回したいものがあれば、Add()して回せばOKというところです。

多分、衝突回避用の距離センサとかを追加するときも、Add()して回すのが作法なのでしょうね。

あと、10Hz以内で回る分しか追加できない。

#memoryクラス
大事そうなクラスでメンバに.memoryまで作って抱えているのに、.memoryは使われていないらしい。
（現在は）

```vehicle.py
class Vehicle():
    def __init__(self, mem=None):

        if not mem:
            mem = Memory()
        self.mem = mem
        self.parts = []
        self.on = True
        self.threads = []
        self.profiler = PartProfiler()
```

初期化で.memを作って、Memory()を動的に確保しているようだ。
.memoryではなく、.memを追う。
しかし、やっていることは、update_parts()を毎周期呼び出し。
エントリーしたパーツからinputして.memに保存、パーツを動かして処理を行い、outputを.memに保存する。これをパーツの分だけ繰り返す。
vehicleのrun()の中で呼び出して実行することで、CPUの負荷の管理を容易にしている。

```vehicle.py
    def update_parts(self):
        '''
        loop over all parts
        '''
        for entry in self.parts:

            run = True

            #check run condition, if it exists
            if entry.get('run_condition'):
                run_condition = entry.get('run_condition')
                run = self.mem.get([run_condition])[0]
            
            if run:
                #get part
                p = entry['part']

                #start timing part run
                self.profiler.on_part_start(p)

                #get inputs from memory
                inputs = self.mem.get(entry['inputs'])

                #run the part
                if entry.get('thread'):
                    outputs = p.run_threaded(*inputs)
                else:
                    outputs = p.run(*inputs)

                #save the output to memory
                if outputs is not None:
                    self.mem.put(entry['outputs'], outputs)

                #finish timing part run
                self.profiler.on_part_finished(p)
```


#RS_T265クラス

Tawn Kramer氏のソースコード。
VehicleのPartの形式に沿ったメンバ関数を作る。
基本はRealSenseのサンプル通りの関数呼び出し。
pipelineを作って、周期的にposeを読み出し。

```realsense.py
'''
Author: Tawn Kramer
File: realsense.py
Date: April 14 2019
Notes: Parts to input data from Intel Realsense cameras
'''
import time
import logging

import numpy as np
import pyrealsense2 as rs

class RS_T265(object):
    '''
    The Intel Realsense T265 camera is a device which uses an imu, twin fisheye cameras,
    and an Movidius chip to do sensor fusion and emit a world space coordinate frame that 
    is remarkably consistent.
    '''

    def __init__(self, image_output=False):
        #Using the image_output will grab two image streams from the fisheye cameras but return only one.
        #This can be a bit much for USB2, but you can try it. Docs recommend USB3 connection for this.
        self.image_output = image_output

        # Declare RealSense pipeline, encapsulating the actual device and sensors
        self.pipe = rs.pipeline()
        cfg = rs.config()
        cfg.enable_stream(rs.stream.pose)

        if self.image_output:
            #right now it's required for both streams to be enabled
            cfg.enable_stream(rs.stream.fisheye, 1) # Left camera
            cfg.enable_stream(rs.stream.fisheye, 2) # Right camera

        # Start streaming with requested config
        self.pipe.start(cfg)
        self.running = True
        
        zero_vec = (0.0, 0.0, 0.0)
        self.pos = zero_vec
        self.vel = zero_vec
        self.acc = zero_vec
        self.img = None

    def poll(self):
        try:
            frames = self.pipe.wait_for_frames()
        except Exception as e:
            logging.error(e)
            return

        if self.image_output:
            #We will just get one image for now.
            # Left fisheye camera frame
            left = frames.get_fisheye_frame(1)
            self.img = np.asanyarray(left.get_data())


        # Fetch pose frame
        pose = frames.get_pose_frame()

        if pose:
            data = pose.get_pose_data()
            self.pos = data.translation
            self.vel = data.velocity
            self.acc = data.acceleration
            logging.debug('realsense pos(%f, %f, %f)' % (self.pos.x, self.pos.y, self.pos.z))

    def update(self):
        while self.running:
            self.poll()

    def run_threaded(self):
        return self.pos, self.vel, self.acc, self.img

    def run(self):
        self.poll()
        return self.run_threaded()

    def shutdown(self):
        self.running = False
        time.sleep(0.1)
        self.pipe.stop()



if __name__ == "__main__":
    c = RS_T265()
    while True:
        pos, vel, acc = c.run()
        print(pos)
        time.sleep(0.1)
    c.shutdown()
```

#RS_T265クラスの呼び出し

path_follower.py (drive) [--log=INFO]

のように、manage.pyの代わりにこれを直接呼び出して実行していると思われる。
自分が使うときは、ここまで思い切った実行はできないと思うので、donkeycarの方に組み込む。

```path_follower.py

        from donkeycar.parts.realsense import RS_T265
        rs = RS_T265(image_output=False)
        V.add(rs, outputs=['rs/pos', 'rs/vel', 'rs/acc' , 'rs/camera/left/img_array'], threaded=True)

        class PosStream:
            def run(self, pos):
                #y is up, x is right, z is backwards/forwards
                return pos.x, pos.z

    V.add(PosStream(), inputs=['rs/pos'], outputs=['pos/x', 'pos/y'])

    origin_reset = OriginOffset()
    V.add(origin_reset, inputs=['pos/x', 'pos/y'], outputs=['pos/x', 'pos/y'] )
```

donkeycarからRS_T265を読みだして、Vehicleに登録している。
こんな感じで、色々登録した後、データをどう扱うのだろうか？

#rs/pos,rs/vel,rs/acc