Jetson Nanoで自律走行ラジコンを作る(6)

今回は、前回作ったDNNにより推論を実行するモジュールをラジコンの制御に組み込む。

「Jetson Nanoで自律走行ラジコンを作る」シリーズのまとめ記事は以下。

kimitok.hateblo.jp

ラジコンの制御に推論モジュールを組み込むにあたり、以下のようなクラス*1を作成した。

struct DriveParameter {
    float steering;
    float throttle;
};

class DriveDetector {
    public:

    DriveDetector(const std::string &model_file_path);
    ~DriveDetector();

    DriveParameter detect(const cv::Mat &image);
    //...
};

クラスのインターフェースとしては、コンストラクタでエンジンファイルのパスを受け取り、推論を実行する際はOpenCVのMatを受け取ってDriveParameterを返却するごくシンプルな作りにしている。

実装側は、コンストラクタでエンジンファイルの読み込みやGPUのメモリの確保・cudaStream_tの初期化まで行い、detect()で行う処理を最低限にするようにしている。

DriveDetector::DriveDetector(const std::string &model_file_path)
{
    //...
    
    cudaStream_t stream;
    if (cudaStreamCreate(&stream) != cudaSuccess)
    {
        debug_debug("ERROR: cuda stream creation failed.");
    }
}

DriveParameter DriveDetector::detect(const cv::Mat &image)
{
    cv::Mat image_rgb, image_rgb_float;
    cv::cvtColor(image, image_rgb, cv::COLOR_BGR2RGB);
    image_rgb.convertTo(image_rgb_float, CV_32F);

    // Copy image data to input binding memory
    if (cudaMemcpyAsync(input_mem, image_rgb_float.data, input_mem_size, cudaMemcpyHostToDevice, stream) != cudaSuccess)
    {
        debug_debug("ERROR: CUDA memory copy of input failed, size = %zu bytes", input_mem_size);
        return value_on_error;
    }

    // Run TensorRT inference
    void* bindings[] = {input_mem, output_mem};
    if (context->enqueueV2(bindings, stream, nullptr) == false)
    {
        debug_debug("ERROR: TensorRT inference failed");
        return value_on_error;
    }

    // Copy predictions from output binding memory
    auto output_buffer = std::unique_ptr<float[]>{new float[output_mem_size]};
    if (cudaMemcpyAsync(output_buffer.get(), output_mem, output_mem_size, cudaMemcpyDeviceToHost, stream) != cudaSuccess)
    {
        debug_debug("ERROR: CUDA memory copy of output failed, size = %zu bytes", output_mem_size);
        return value_on_error;
    }
    cudaStreamSynchronize(stream);

    return {output_buffer[0], output_buffer[1]};
}

ここまで作れば、あとは全体の制御をしている部分に上記を組み込むだけである。

class RCCarServer : public RCCarControllerListener {
    //...

    DriveDetector detector;

    //...
};


const std::string MODEL_PATH = "/opt/lunchjet/model.trt";

RCCarServer::RCCarServer(std::atomic<bool> &stop_controller_thread)
 : controller("/dev/input/event2", *this, stop_controller_thread),
   //...
   detector(MODEL_PATH)
{
    //...
}

void RCCarServer::handle_video(cv::Mat &image)
{
    if(is_manual_drived) {
        record_control_data(image);
    }
    else {
        auto params = detector.detect(image);

        driver.steer(params.steering);
        driver.go(params.throttle * throttle_magnification);
    }
}

ここで、実際に自律走行をするにあたり安全機構を追加している。
ゲームパッドの右奥のトリガーの値を使って、AIモデルから出力されたスロットルの値を調整できるようにした。

具体的には、上記コードの中の

        driver.go(params.throtle * throttle_magnification);

という部分のthrotle_magnificationである。トリガー入力なし時はthrotle_magnificationを1、フルトリガーで0としている。トリガーはアナログ入力ができるので、デプロイしたAIモデルの様子を見る際にごく低速で動かしたり、何かに衝突しそうになった時に停止させることができる。
つまり自動車免許の教習車で教官がいつでもブレーキを踏める仕組みのようなものである。

上記を含め、今のところゲームパッドのボタンアサインは以下のようにしている。
訓練データの収集開始・停止は今はモードスイッチに連動させているが、独立したボタンに割り当てたほうがよい気もしている。

f:id:kimito_k:20210630043930p:plain