亀場でプログラミング：掃除ロボット

このページでは、「亀場」を使って、掃除ロボットのアルゴリズムについて考察してみたい。

亀場関係のソフトウェアについてのさらに詳しい説明は 亀場サーバー操作マニュアル を参照のこと。

1. 掃除ロボット

少し大きな電気屋に行くと、割合と目立つ場所で、掃除ロボットのデモをやっている。 足を止めて、その動きを眺めていると（私はかなりの暇人かもしれない）、比較的単純な動作の繰り返しをしているだけのようにも見える一方で、 家具などの障害物や「行き止まり」箇所がたくさんある中で、どうやったら部屋全体をうまく掃いて（吸って？）回る ことができるものなのか、不思議な気もする。 そこで、このページでは、亀を掃除機に見立てて、亀場の掃除をするようにプログラムしてみたい。

準備

基本的な作業手順は、タートルグラフィックスと全く同様だ。まずは、亀場サーバー（tfield-linux）を起動しておく。

また、これも同様に、turtle.hを作業用ディレクトリにコピーしておく。

次に、掃除ロボット用のプログラムをエディタで作成する。以下にその出発点となるひな形を示す。

#include <stdio.h>
#include <math.h>
#include "turtle.h"

main()
{
  int res,cnt ;
  CON("localhost") ;
  BMODE() ;
  CLR() ;
  NM("KAME") ;
  PD() ;
  while (1) {
    RF(20.0) ;
    Q_FINDER(&res) ;
    if (res==0) {
      FD(2.0) ;
    }
    else {
      for (cnt=0; cnt<90; cnt=cnt+1) RT(2) ;
    }
  }
}

このプログラムを保存して（例えば sweeper.c) 

cc sweeper.c -lm
a.out

で実行する。亀が壁の間を往復運動すれば成功だ。

ひな形プログラムの仕組み

では、上のプログラムで、どうして亀が往復運動するのか、その仕掛けを説明しよう。

その際に必要となるいくつかの命令について、簡単にまとめておく。

対戦（お掃除）モード中の 亀の動作の制限事項

亀場の中に登場可能な物体は、亀、（まだ説明していなが）ドーナッツ、障害物、そして壁、の４種類がある。

BMODE();
亀場を「対戦モード（お掃除モード）」に切り替える。 対戦モードでは亀場の四方に壁が設けられ、亀はそこ（部屋）から出ることができなくなる。
具体的には、前方に壁がある状態で前進（FD(距離)）命令を出しても、（サーバー側が）その距離動くと壁と衝突すると判定した場合は、亀は前進しない（できない）。
また、対戦モードの間は、１回あたりの亀の移動量は最大で2ピクセル, 回転角度は最大で3°、に制限される （本物の掃除ロボットは急には動けないことを模擬するため）。
NM("KAME");
亀にニックネームを付ける。対戦モード中、このニックネームは亀と一緒に画面に表示される。あなたのイニシャルなどを設定すると良いだろう。
RF(20.0);
障害物センサーの到達距離を設定する。20はちょうど亀の大きさ（差し渡し）に相当する。
Q_FINDER(&res);
前方に障害物があるかどうか、前方センサー(FINDER)を使って調べ、その結果をint型の変数resにセットする。res==0は障害物無し、res==1は他の亀、 res==2はドーナッツ、res==3は壁以外の障害物、res==4は壁を、それぞれ検出したことを表す。

以上を念頭に、ひな形プログラムにコメントを付してみた：

右のプログラムで、無限ループ
while(1) { 何々; }
のところは、while文の代わりにfor文を使って
for(;;) { 何々; }
と記述することもできる。 

#include <stdio.h>
#include <math.h>
#include "turtle.h"     亀場を使う際に必ず必要な呪文

main()
{
  int res,cnt ;         以下で用いる変数を宣言しておく
  CON("localhost") ;    自分のパソコン（localhost）の亀場サーバーと通信を開始する
  BMODE() ;             対戦モード（お掃除モード）に亀場を切り替える
  CLR() ;               亀場を綺麗にする
  NM("KAME") ;          自分の亀に"KAME"というニックネームをつける
  PD() ;                歩いた軌跡が分かるように、ペンを下ろす
  while (1) {           以下の{から}までの間をずーーっと反復する（無限ループ）
    RF(20.0) ;                 障害物センサーの到達距離を20（亀のサイズ）に設定する
    Q_FINDER(&res) ;           センサーの出力結果を変数resにセットする
    if (res==0) {              もし障害物が無ければ
      FD(2.0) ;　　　　　　　　　　　　　　2ステップ前進する
    }
    else {                     それ以外なら（障害物があれば）
      for (cnt=0; cnt<90; cnt=cnt+1) RT(2) ;  「2°右回転」を90回繰り返す=合計180°の回転
    }
  }
}

この亀型掃除ロボットは、壁の間を往復するだけなので、バキューム機構が付いていたとしても、部屋のほんの一部しか綺麗にならない。 できるだけ一様に部屋がスイープされる、つまり、亀場全体ができるだけ一様に赤い色に塗りつぶされるようにできれば、 より良い掃除アルゴリズムということになる。 さらに欲を言えば、ゴミのたまりやすい部屋の隅を重点的に、でも中央付近も手を抜かないで、というのが理想だろう。 以下では、そんなアルゴリズムを考案して、それがうまく働くかどうか、シミュレーションで検証してみよう。

２．掃除ロボットの運動性能とセンサー情報の活用

アルゴリズムを考える前に、亀型掃除ロボットの機能・性能を確認しておく必要がある。まず、対戦モード（掃除モード）中は、 運動性能に制限があって、1ステップあたりの移動量は最大2(ビクセル）まで、回転角は3（°）までであることを頭に入れておこう。 亀は「ワープ」できないわけだ。 また、壁や他の亀などの物体が進路上にあると、前進（後退）命令を出しても、亀は現在位置から動くことができない。

一般に掃除ロボットは、部屋の形や家具が置かれている状態、ゴミの分布などを一切知らず、ただそこにポンと置かれるだけだ。 そして、限られたセンサーからの情報を元に、自分の行動を決めなければならない。 我々の亀ロボットでは、以下のセンサーを活用して、行動をプログラムすることができる：

前方センサー(FINDER)

前方センサは、前方の検出範囲内に物体が入ると反応する

c-turtle-graphics-finder

亀の前方に他の亀や障害物、壁などがないかチェックするセンサー。物体の種類も識別できる。 物体までの距離を直接測ることはできないが、RF()関数を使って、 検出する距離の範囲を調整することは可能。

基本的な使い方（左はif文を使用する例、右はswitch文を使用する例）

int rc ;
...

RF(検出範囲) ;
Q_FINDER(&rc) ;
if (rc==0) {
   障害物無しのときの動作 ;
}
else if (rc==1) {
   前方に亀がいるときの動作 ;
}
else if (rc==2) {
   前方にドーナッツがあるときの動作 ;
}
else if (rc==3) {
   前方に亀・ドーナッツ以外の物体があるときの動作 ;
}
else if (rc==4) {
   前方に壁があるときの動作 ;
}
 
int rc ;
...

RF(検出範囲) ;
Q_FINDER(&rc) ;
switch (rc) {
case 0:
   障害物無しのときの動作 ;
   break ;
case 1:
   前方に亀がいるときの動作 ;
   break ;
case 2:
   前方にドーナッツがあるときの動作 ;
   break ;
case 3:
   前方に亀・ドーナッツ以外の物体があるときの動作 ;
   break ;
case 4:
   前方に壁があるときの動作 ;
   break ;
}

方位センサー(RADAR)

方位センサーは、一番近い物体までの相対角度を検出する

c-turtle-graphics-rader

亀の進行方向に対して相対的にどの角度に物体があるかを検出するセンサー。 物体が複数個ある場合は、一番近いものが計測される。ただし、物体の種類は区別できない。また、壁には反応しない。 返される角度は亀の前方が0°で、右側が-マイナス、左側がプラスとなる。センサーの検出値には±20°の誤差が含まれる。

基本的な使い方

float a ;
...

Q_RADAR(&a) ;

最も近い物体のまでの角度（亀の前方向を0とした相対角度）がaにセットされている
ので、それを使って、行動プログラムを記述する ;

物体数センサー

亀場の中に何個の物体（亀とドーナッツを合わせた総数）があるのかを調べるセンサー。自分自身も１とカウントされる。

基本的な使い方

int nt ;
...

Q_NT(&nt) ;

亀場の中の物体の総数がntにセットされているので、それを使って行動プログラムを記述する ;

３．掃除アルゴリズムの実装

センサーの使い方が解ったところで、最初に示したひな形（往復運動ロボット）を、ここではさらに進化させてみたい。 ひな形ロボットが往復運動しか出来なかったのは、単純に、壁（障害物）を見つけたら180°回転していたからだ。 ということは、下図のように、障害物回避の際に、位置をずらしたり(a)、角度を変えたり(b)、あるいは、180°転回 するにしても軌道を曲線にしてやれば(c)、部屋のあちこちを掃除してくれそうだ。

c-turtle-graphics-cleaner-reflection

icon-pc　練習：掃除アルゴリズムの実装

上記のアイデアのひとつ、あるいはあなたの独自のアイデアに基づいて、掃除ロボットが床全体を掃引できるよう、 ひな形プログラムを改良しなさい。

icon-hint　ヒント

最初の(a)は部屋の隅を綺麗にしてくれそうなので、良さそうに感じるが、反対側の壁でも同じように「反射」すると、結局、同じ（四角形の）経路をぐるぐる回ることになるし、部屋のコーナーで行き止まりにならないような工夫も必要になるだろう。

二番目のアイデアは単純で良さそうだけれども、反射角度によっては、同じ辺りをぐるぐる回ってしまったり、部屋の隅が手薄になりやすいかもしれない。

三番目は、何となく市販の掃除ロボットの動きに近いようにも感じる。曲線を描かせるには、タートルグラフィックスのページに述べたように、前進の都度、すこしずつ一定の向きに回転を加えればよい。

ゴミの片付けの「可視化」

コインはFINDERやRADARには反応せず、また、亀の歩行にも影響しない。 コインを持った亀は、それを下に抱いた格好で表示される。

詳しくは、別ページに記載してあるが、亀場には「コイン」という アイテムをばらまいたり、亀がそれを拾ったりする機能がある。 そこで、コインをゴミに見立てて、掃除ロボットがゴミ集めをする様子が目に見えるようにしてみよう。 そのためには、以下のようなコードをプログラムに追加する。 

#include "turtle.h"

void collect_coin(unsigned int time) { PICKCOIN(); }  /* C:この行を追加 */

main()
{
   int cnt ; /* 追加箇所(A)で使う変数 */
   ...
   CON("localhost") ;
   for (cnt=0; cnt<30; cnt=cnt+1) COIN();   /* A:この行を追加 */
   SET_FOUND_COIN_HANDLER(collect_coin) ;   /* B:この行を追加 */
   ...
   while(1) {

      ゴミ集めのコード   
   
   }
}

亀場にコインを100枚ばらまいてから掃除をさせている様子の動画
右側のウィンドウ（ウェブブラウザ）では、「ライブ機能」を使って、カメの視点で亀場の様子を表示している。

追加部分の働きは、(A) ゴミに見立てたコインを30枚、亀場にばらまく (B)ゴミが近くにあったときに呼び出す関数をセットする (C)ゴミが見つかったときに行う処理を記述する、といった具合だ。 この流れについては、イベント処理のページに説明したので、 詳しい動作が気になる人は参照すること。

４．粗大ゴミの片付け

上述の「コイン」をゴミ屑とすれば、ドーナッツは粗大ゴミといったところだろうか。 ドーナッツは亀の歩行の障害物となる。
「ドーナッツには見えない」という声も聞こえてきそうだが、絵心の無い教員がgimpを使って苦労して作った画像なので、 そこのところは察してほしい。

亀場の中には大きなゴミに見立てたドーナッツを置くことができる。 DONUT()関数を１回呼ぶ毎に、ランダムな場所にドーナッツがひとつ配置される（ただし、出せるドーナッツは10個まで）。 以下は、最初に示したひな形に少しだけコードを追加して、粗大ゴミ（ドーナッツ）を10個置いた後、掃除ロボットが部屋を動き回り、それを見つけたら片付ける（砲撃して消去する）ようにした例だ。 

#include <stdio.h>
#include <math.h>
#include "turtle.h"

main()
{
  int res,cnt ;
  float a ;
  CON("localhost") ;
  BMODE() ;
  CLR() ;
  NM("KAME") ;
  for (cnt=0; cnt<10; cnt=cnt+1) DONUT() ; /* 「ゴミ」を10個ランダムに配置する */
  PD() ;
  while (1) {
    RF(20.0) ;
    Q_FINDER(&res) ;
    if (res==0) {
      FD(2.0) ;
    }
    else if (res==2) { /* もし前方のそれが「ドーナッツ」だったら */
      Q_RADAR(&a);      /* 角度を調べて */
      LT(a) ;           /* そちらの向きに自分の向きを調整して */
      FIRE() ;          /* 砲撃する */
    }
    else {
      for (cnt=0; cnt<70; cnt=cnt+1) RT(2) ;
    }
  }
}

icon-pc　発展：粗大ゴミの片づけプログラム

上のサンプロコードを手がかりに、10個の「粗大ゴミ」をできるだけ素早く・確実に始末してくれる掃除ロボットに発展させなさい。

icon-hint　ヒント

亀と一緒に表示される数値は、「粗大ゴミ」（ドーナッツ）を片付ける度に10点加算され、砲撃1回あたりマイナス1点となる （最初に持ち点100がある）。 ただし、「粗大ゴミ」には自分の至近距離で命中させないとポイントにはならない（遠くのゴミに命中してもポイントにはならない）。 従って、このポイント数は仕事の「確実さ」の指標として使うことができる。

亀場サーバーの隠し機能

ウィンドウがアクティブ（前面に出た）状態で、o（オー）キーを押すと、四角い石のような障害物をランダムに配置することができる。OはObstacleのO。

所用時間（亀場サーバーの画面更新回数）を調べたり、自分のスコアを得るには、コードに次のような追加をすれば良い： 

...

main() {
  int nt ;
  int t0,t1 ;
  int score ;
  .....
  Q_TIME(&t0) ;    /* ただいまの時間をt0にセット */
  while (1) {
  
     掃除のプログラム 

     Q_NT(&nt) ;          /* 亀場の物体の総数をntにセット */
     if (nt==1) break ;   /* もしも自分も含めて１つ（匹）だけだったら、片付け完了なので、ループを抜ける */
  }
  Q_TIME(&t1) ;                        /* ただいまの時間をt1にセット */
  printf("ELAPSED TIME= %d\n",t1-t0) ; /* t1-t0 が所用時間 */
  Q_SCORE(&score) ;                    /* 点数をscoreにセット */
  printf("SCORE= %d\n",score) ;
}

ランダムな行動をさせたい場合には乱数を用いることもできる。#include <stdlib.h>をプログラムの先頭部分に加えておいてから、drand48()という関数をプログラム中で呼び出すと、0から1.0までの（擬似的にランダムな）実数値を得ることができる。 例えば、回転角度を±3°の範囲でランダムに選びたければ、 

#include <stdlib.h>
...

main() {
  float a; 
  ...

  a = (drand48()-0.5)*6.0 ;
  LT(a) ;
  ...
}

といった具合に記述する。