読者です 読者をやめる 読者になる 読者になる

Reactive Extensionsでロボット制御?

C# Rx

最近、Reactive Extensions(Rx)を使ってプログラミングを書くのがすごく楽しい。

今のところRxの使い道としては、GUIアプリケーションや、Webサービスの非同期呼び出しなどが多いでしょうか。

やさしいFunctional reactive programming(概要編) - maoeのブログによると、Reactive Programmingの応用先としては、以下のようなものがあるそうです。

 * アニメーションやコンピュータミュージックのシグナル処理を自然に書ける
 * イベントドリブンなコードで処理がぶつ切りになってしまうのを避ける
   * GUIプログラミングとか
   * ロボット制御とか

ロボット制御!

そう。いろいろなセンサからの情報を非同期で受け取って、時間のかかる複雑な計算処理を周期的に実行して、アクチュエータの制御をするなんて、まさにReactive Programming向き!?

じゃあ何か制御プログラムを書いてみるか!とは言っても、ハードウェアを用意したりとかは大変・・・。
というわけで、まずは、論文読み: Arrows, Robots, and Functional Reactive Programming http://www.haskell.org/yampa/AFPLectureNotes.pdf - 言語ゲームを参考にして、左右の車輪の速度からロボットの位置を算出するプログラムをRxで書いてみました。

using System;
using System.Reactive.Linq;
using System.Reactive.Subjects;

class RxSimBot
{
    static void Main()
    {
        // 車体の幅
        const double length = 1.0;

        // 右車輪の速度を発生させるオブジェクト
        var vrSubject = new Subject<double>();
        // 左車輪の速度を発生させるオブジェクト
        var vlSubject = new Subject<double>();

        // 左車輪の速度と右車輪の速度を結合、ついでに前回との時間差も付与。
        var source = Observable.Zip(vrSubject, vlSubject, (vr, vl) => new { vr, vl })
            .TimeInterval();

        // 車体の向き theta = (1/l) * integral (vr - vl)
        var thetaObserver = source
            .Scan(0.0, (s, v) => s + (v.Value.vr - v.Value.vl) * v.Interval.TotalSeconds)
            .Select(integral => (1.0 / length) * integral);

        // 左右の車輪速度に車体の向きを結合
        var sourceWithTheta = source
            .Zip(thetaObserver, (v, theta) => new { v.Value.vr, v.Value.vl, theta, v.Interval });

        // 車体のX座標 x = (1/2) * integral ((vr + vl) * cos theta)
        var xObserver = sourceWithTheta
            .Scan(0.0, (s, v) => s + (v.vr + v.vl) * Math.Cos(v.theta) * v.Interval.TotalSeconds)
            .Select(integral => (1.0 / 2.0) * integral);

        // 車体のY座標 y = (1/2) * integral ((vr + vl) * sin theta)
        var yObserver = sourceWithTheta
            .Scan(0.0, (s, v) => s + (v.vr + v.vl) * Math.Sin(v.theta) * v.Interval.TotalSeconds)
            .Select(integral => (1.0 / 2.0) * integral);

        // x,y,thetaを結合して結果を表示する。
        xObserver.Zip(yObserver, (x, y) => new { x, y })
            .Zip(thetaObserver, (pos, theta) => new { pos.x, pos.y, theta })
            .Subscribe(Console.WriteLine);

        // 1秒周期でに左右の車輪の速度を与える
        Observable.Interval(TimeSpan.FromSeconds(1))
            .Subscribe(_ =>
            {
                // 左右車輪に同じ値を与えているためyとthetaは変化しない
                vrSubject.OnNext(1);
                vlSubject.OnNext(1);
            });

        Console.ReadKey();
    }
}

Rxを使わない場合は、センサ入力と計算処理を別スレッドに分けて、データの受け渡しを排他制御して、計算処理はfor文になったりするので、それと比べるとだんぜんシンプルですね。
なにより、積分計算がTimeIntervalとScanでサクっと書けるのに感動しました!

なおこのプログラムでは、左右の車輪の速度をSubject.OnNextで与えていますが、実際のロボットの場合はここがセンサ入力になります。