ラズベリーパイ(Raspberry Pi Zero WH)でロボットをラジコン操作しよう

2023-09-24

『ラズベリーパイ（Raspberry Pi Zero WH）でScratch ロボットと遊ぼう！』で紹介したロボットを、今度はPythonとJavaScriptで動かしてみたいと思います。

さらにロボットの前につけたカメラで、ロボットがみている風景を見ながら操作できるようにします。

WebIOPi、MJPG-Streamer、SVGを組み合わせて、スマホでラジコン操作に挑戦です。

準備するもの・配線方法

準備するものや配線方法はこちらの記事と同じなので、確認してください。

check

ラズベリーパイ（Raspberry Pi Zero WH）でScratch ロボットと遊ぼう！ラズベリーパイ（Raspberry Pi Zero WH）で、Scratch（スクラッチ）のプログラミングをして、ロボットを動かしてみたいと思います。 \ 学会発行誌の謝辞に掲載いただき…

一つ追加になるのがこちらのカメラです。

RasTech Raspberry Pi カメラモジュール Raspberry Pi カメラ 500万画素 Raspberry Pi4B/3B+/3B/2B+/ZERO1.3/ZERO 2W/ZERO W for Raspberry Pi クリアスタンド*1、リボンケーブル*3

created by Rinker

Ｒ∞ＲａｓＴｅｃｈ

透明のアクリルのケースホルダーがついていて、簡単にカメラを固定することができます。

このホルダーの組み立ては、こちらのYoutubeの動画を参考にしたら簡単でした。

しかも、組み立て用のミニドライバーまでついているので驚きです。

画像・画質も十分ですし、ラズベリーパイ（Raspberry Pi Zero WH）に接続するコードも付属されています。

コスパ◎で、とってもおすすめです。

カメラを有効にする

まずは、ラズベリーパイのカメラの設定を有効にします。

raspi-configを起動します。

$ sudo raspi-config

[Interfaces Options] を選択します。

[Camera]を選択します。

enabled? で[はい] を選びます。

再起動します。

ちゃんとカメラが検出されているか、確認します。

vcgencmd get_camera
supported=1 detected=1

detectedが1であれば、カメラが検出されているのでOKです。

このあと出てくるストリーミングの取得が、なかなかうまくいかなくて、試行錯誤している途中、ためしにこのコマンドでチェックしてみたら、「detected=0」になっていてびっくり！

いつの間にかケーブルが抜けてしまっていたようです。

操作画面

こちらのような操作画面を作りたいと思います。

右側のストリーミング画像を見ながら操作します。

[スクロールON]ボタンをクリックすると、スクロールON/OFFが切り替わります。

スクロールONの状態で、スマホで操作する際、アドレスバーを非表示にしたり、ズームで表示サイズを調整したりします。

左下の円にタッチしてロボットを操作をする場合は、スクロールOFFにして画面が動かないようにします。

画面を表示した最初の3秒間は、「前進」「後退」といったガイドの文字列を表示させます。

円の上の方をクリックすると前進、下の方をクリックすると後退します。

外側の色が濃い部分をクリックするとより速く動きます。左右も同様です。

そして、パソコンの大きな画面でもスマホの小さな画面でも操作できるように、レスポンシブ対応にしたいと思います。

どう実装するのか本やネットでみてみると、クリックしたX・Yの座標と図形の座標を比較して、どこをクリックしたか算出する方法が多く見つかりました。

レスポンシブ対応にするには、さらに表示幅によって拡大・縮小させ、座標を自分で設定しなおさなくてはいけないようです。

処理が難しそうですし、もっと画像をオブジェクトとして扱える簡単な方法はないのかいろいろ調べてみて、いちばん簡単そうなSVGを使うことにしました。

SVG

SVGとは（Scalable Vector Graphics）の略で、JPEGやPNGのような点々で表現した画像(ビットマップデータ)ではなく、点の座標とそれを結ぶ線を計算式で表したベクターデータです。

拡大・縮小すると計算しなおして描画するので、ギザギザにもなりませんし、レスポンシブ対応も簡単です。

外部の画像ファイルを読み込むこともできますし、HTMLの中にSVGを直接記述することもでき、さらにJavaScriptで操作することもできます。

SVGのコマンド

例えば、円や四角を描画する場合、このようにSVGで記述します。

// 円を描画
<circle id="maru" cx = "100" cy = "100" r = "50" stroke = "black" fill = "＃fff" stroke-width = "2" />

// 四角を描画
<rect id="shikaku" x="0" y="0" width="100" height="100" stroke="black" fill="#fff" stroke-width="1" />

そして、描いた図形をidで指定して、JQueryで処理を書くことができます。

$("#shikaku").click("処理");

SVGでコントローラー作成

このSVGで円のコントローラーを描いていきます。

このコントローラーは扇形を組み合わせて作っています。

例えば「前進」の部分は、3つの大きさの扇形を重ねて、3段階の速さを選択できるようにしました。

SVGで扇形を描画

SVGで扇形を描く場合は、「パス描画」というコマンドで記述します。

M150 330 L44 224 A150 150 0 0 1 256 224 Z

コマンドの意味を解説します。
① M(Move to) x y ：円の中心点の座標(150,330)移動します
② L(Line to) x y ：座標(44,224)まで線を引きます
③ A(arc) rx ry x-axis-rotation large-arc-flag sweep-flag x y：指定したパラメータで円弧を描きます。
rx：x軸の半径 (150)
ry：y軸の半径 (150)
x-axis-rotation：円弧の回転度 (0)
large-arc-flag：中心角が180度以上なら1、未満なら0 (0)
sweep-flag：時計回りで円弧を描く場合1、反時計回りなら0 (1)
x y：円弧を描き終える座標(256,224)
④ Z(Close path)：最初の座標(150,330)まで線を引いて閉じます

円弧の座標を取得

ここで円弧を描き始める座標（L x yの部分）と描き終える座標（最後の x yの部分）を算出するために、三角関数を使います。

描きたい扇形の角度の x’ y’ の座標を、次の式で求めることができます。

x’ = cx + r * sin θ
y’ = cy + r * cos θ

JavaScriptで計算する場合、Math.sin(x)、Math.cos(x)を使用しますが、xには弧度法を指定します。

45°・60°といった度数法に、(π/180)を掛けて弧度法に変換します。

SVGコマンド取得JavaScript

扇形の座標の計算方法が分かったので、前・後・左・右各3サイズの扇形を描くコマンドをJavascriptで取得します。

// 扇形のPathを取得(円の中心x, y, 半径r, 開始中心角(度数法), 終了中心角) 
var path = getPath(150, 330, 150, 315,  45) // 前進
         + getPath(150, 330, 110, 315,  45)
         + getPath(150, 330,  70, 315,  45)
         + getPath(150, 330, 150,  45, 135) // 右旋回
         + getPath(150, 330, 110,  45, 135)
         + getPath(150, 330,  70,  45, 135)
         + getPath(150, 330, 150, 135, 225) // 後退
         + getPath(150, 330, 110, 135, 225)
         + getPath(150, 330,  70, 135, 225)
         + getPath(150, 330, 150, 225, 315) // 左旋回
         + getPath(150, 330, 110, 225, 315)
         + getPath(150, 330,  70, 225, 315);

function getPath(cx, cy, r, startDegree, finishDegree) {

  // 円弧始まりの座標を算出(小数点以下四捨五入)
  var startX = Math.round(cx + r * Math.sin(startDegree / 180 * Math.PI));
  var startY = Math.round(cy - r * Math.cos(startDegree / 180 * Math.PI));

  // 円弧終わりの座標を算出
  var finishX = Math.round(cx + r * Math.sin(finishDegree / 180 * Math.PI));
  var finishY = Math.round(cy - r * Math.cos(finishDegree / 180 * Math.PI));

  // 扇形の中心角は180度以上？
  var largeArcFlag = (finishDegree - startDegree >= 180) ? 1 : 0;


  // cx, cyに移動
  var path = 'M' + cx + ' ' + cy + ' '

  // startX, startYまで線を引く
  + 'L' + startX + ' ' + startY + ' ' 

  // 円弧を描く  
  + 'A' + r + ' ' + r + ' ' + 0 + ' ' + largeArcFlag 
  + ' ' + 1 + ' ' +  finishX + ' ' + finishY+ ' '

  // 最初のcx,cy座標まで線を引く
  + 'Z<br>';

  return path;

}

このプログラムで取得した扇形を描くSVGコマンドをindex.htmlに埋め込んでいきます。

扇形を描くのにこちらのサイトを参考にさせていただきました。ありがとうございます。
SVGで円グラフを描く
三角関数と弧度法についてはこちらのサイトで分かりやすく解説されています。
三角関数の基礎知識。sinθ cosθ tanθ の覚え方・弧度法・三角比の表まとめ

MJPG-Streamer

次にカメラで撮影したストリーミング画像を画面に表示させたいと思います。

今回は、Raspberry Pi でよく使われているライブカメラ用ソフト「MJPG-Streamer」を使うことにしました。

MJPG-Streamerのインストール

ネット上にも、いろいろな方法が紹介されていますが、うまく起動してくれなかったり、思うようにPathが通せなかったりして、はまってしまいました。

結局、こちらのラズベリーパイのバイブル『Raspberry Piで学ぶ電子工作』に載ってる方法でインストールしたら、すんなり起動しました。やっぱりすごいです。

こちらの本は、ほんとにすばらしいです。初心者の人にもやさしく基礎から解説されていて、ラズベリーパイをさわる方は最初に読んだ方がいいと思います。

MJPG-Streamerのインストール

$ sudo apt-get update
$ sudo apt-get install libjpeg8-dev cmake
$ git clone https://github.com/jacksonliam/mjpg-streamer.git
$ cd mjpg-streamer/mjpg-streamer-experimental
$ make
$ cd
$ sudo mv mjpg-streamer/mjpg-streamer-experimental /opt/mjpg-streamer

MJPG-Streamerの起動

本の中では自動起動させるシェルが紹介されていますが、忘れてしまいそうなので少し変えた次のコマンドを手動で起動させています。

$ LD_LIBRARY_PATH=/opt/mjpg-streamer/ /opt/mjpg-streamer/mjpg_streamer -i "input_raspicam.so -fps 15 -q 50" -o "output_http.so -p 9000 -w /opt/mjpg-streamer/www"

オプションは、デフォルトから変更したいものだけを指定します。

-r	解像度　(def.640×480)
-f	フレームレート(def.5)
-q	PEGのクオリティ(1-100)
-p	ポート(def.8080)
-w	wwwフォルダのパス

MJPG-Streamerの動作確認

http:/raspberrypi.local:9000/ にアクセスして、こちらの画面が表示されたら成功です！

「http:/raspberrypi.local:9000/?action=stream」で、モーションJPEG形式の動画を取り出せます。

Motion-JPEGとは、画像の圧縮方式であるJPEGを応用して動画データを生成する圧縮・記録方式のことである。圧縮されたJPEG画像を各コマに配置して、アニメーションのように再生する。
引用元：IT用語辞典バイナリ

プログラム

「ラズベリーパイ（Raspberry Pi Zero WH）をスマホから操作する方法（WebIOPi）」と同じ要領でプログラムを作っていきます。

WebIOPiをベースに、SVGを使ってMotion-JPEGのストリーミングを取り込みます。

/home/pi/webiopi/test/に、以下のindex.htmlとscript.pyを保存します。

index.html

パソコンからはクリックで、スマホからはタッチ操作で動かすようにしました。

処理内容の詳細は、コメントを見てください。

<!DOCTYPE html>
<html>
<head>
<meta charset="utf-8">
<title>Raspberry Pi | Robot</title>
<script src="https://ajax.googleapis.com/ajax/libs/jquery/3.4.1/jquery.min.js"></script>
<script type="text/javascript" src="/webiopi.js"></script>
<script type="text/javascript">
 
// WebIOPiの準備が整ったときに呼び出す関数
webiopi().ready(function() {
 
  // モータードライバーに接続したGPIO端子番号
  var rFwdGpio  = 17;
  var rBackGpio = 27;
  var lFwdGpio  = 2;
  var lBackGpio = 3;
 
  // 操作を表示するエリア
  var txtOpe = $('#txt_ope').get(0);
  
  // タッチ可の端末ではtrue
  var isTouch = ('ontouchstart' in window);
 
  // 前回の操作を保存
  var prevOpe = '';
 
  // ロボットを操作する関数 (右前,右後,左前,左後,操作)
  function robot_move( rf, rb, lf, lb, ope ){
 
    // モータードライバーにパルスレートを設定
    webiopi().pulseRatio(rFwdGpio,  rf);
    webiopi().pulseRatio(rBackGpio, rb);
    webiopi().pulseRatio(lFwdGpio,  lf);
    webiopi().pulseRatio(lBackGpio, lb);
 
    // 操作を表示
    txtOpe.textContent = ope;
  }
 
  // タッチorクリックイベントを受け取って、ロボット操作関数を呼ぶ
  $(document).bind('click touchstart touchmove', function(e){
 
    // タッチorクリックされたXY座標を取得
    var x = (isTouch ? e.originalEvent.changedTouches[0].pageX : event.pageX);
    var y = (isTouch ? e.originalEvent.changedTouches[0].pageY : event.pageY);
 
    // XY座標から操作のIDを取得
    var elemId = (document.elementFromPoint(x, y)).id;
 
    // 前回の操作と違う操作なら
    if(prevOpe != elemId) {
 
      // 今回の操作を保存
      prevOpe = elemId;
 
      // 操作に応じたパラメータで、ロボット操作関数をよぶ
      switch(elemId){
        case 'fwd1'  : robot_move( 0.5,   0, 0.5,   0, "前進1"   ); break;
        case 'fwd2'  : robot_move( 0.7,   0, 0.7,   0, "前進2"   ); break;
        case 'fwd3'  : robot_move( 0.9,   0, 0.9,   0, "前進3"   ); break;
        case 'back1' : robot_move(   0, 0.5,   0, 0.5, "後退1"   ); break;
        case 'back2' : robot_move(   0, 0.7,   0, 0.7, "後退2"   ); break;
        case 'back3' : robot_move(   0, 0.9,   0, 0.9, "後退3"   ); break;
        case 'right1': robot_move(   0,   0, 0.5,   0, "右旋回1" ); break;
        case 'right2': robot_move(   0,   0, 0.7,   0, "右旋回2" ); break;
        case 'right3': robot_move(   0,   0, 0.9,   0, "右旋回3" ); break;
        case 'left1' : robot_move( 0.5,   0,   0,   0, "左旋回1" ); break;
        case 'left2' : robot_move( 0.7,   0,   0,   0, "左旋回2" ); break;
        case 'left3' : robot_move( 0.9,   0,   0,   0, "左旋回3" ); break;
        default      : robot_move(   0,   0,   0,   0, "ストップ"); break;
      }
    }  
  });
 
  // タッチ後、画面から指を離したらストップ
  $(document).bind('touchend', function(e){
    robot_move( 0, 0, 0, 0, "ストップ" );
  }); 
 
});
</script>
</head>
<body>
<!-- SVGをタグで埋め込む -->
<svg xmlns="http://www.w3.org/2000/svg"
     xmlns:xlink="http://www.w3.org/1999/xlink"
     viewBox="0, 0, 1040, 480" >
 
  <!-- スクロールON・OFFボタン -->
  <rect x="2" y="60" width="130" height="50" stroke="grey" stroke-width="2" fill="white" />
  <text id="txt_scroll" x="10px" y="90px">スクロールON</text>
 
  <!-- 操作内容 -->
  <text id="txt_ope"  x="10px" y="150px" ></text>
 
  <!-- コントローラー -->
  <!-- 前進 -->
  <path id="fwd3" d="M150 330 L44 224 A150 150 0 0 1 256 224 Z" fill="#ffdbff" />
  <path id="fwd2" d="M150 330 L72 252 A110 110 0 0 1 228 252 Z" fill="#ffe5ff" />
  <path id="fwd1" d="M150 330 L101 281 A70 70 0 0 1 199 281 Z"  fill="#ffefff" />
 
  <!-- 右旋回 -->
  <path id="right3" d="M150 330 L256 224 A150 150 0 0 1 256 436 Z" fill="#eddbff" />
  <path id="right2" d="M150 330 L228 252 A110 110 0 0 1 228 408 Z" fill="#f2e5ff" />
  <path id="right1" d="M150 330 L199 281 A70 70 0 0 1 199 379 Z"   fill="#f7efff" />
 
  <!-- 後退 -->
  <path id="back3" d="M150 330 L256 436 A150 150 0 0 1 44 436 Z" fill="#ffdbff" />
  <path id="back2" d="M150 330 L228 408 A110 110 0 0 1 72 408 Z" fill="#ffe5ff" />
  <path id="back1" d="M150 330 L199 379 A70 70 0 0 1 101 379 Z"  fill="#ffefff" />
 
  <!-- 左旋回 -->
  <path id="left3" d="M150 330 L44 436 A150 150 0 0 1 44 224 Z" fill="#eddbff" />
  <path id="left2" d="M150 330 L72 408 A110 110 0 0 1 72 252 Z" fill="#f2e5ff" />
  <path id="left1" d="M150 330 L101 379 A70 70 0 0 1 101 281 Z" fill="#f7efff" />
 
  <!-- 中心の円 -->
  <circle id="stop" cx="150" cy="330" r="30" fill="white" />
 
  <!-- コントローラーのガイド -->
  <text id="txt_fwd" x="135px" y="200px" >前進</text>
  <text id="txt_rgt" x="250px" y="350px" >右旋回</text>
  <text id="txt_bck" x="135px" y="470px" >後退</text>
  <text id="txt_lft" x="3px"   y="350px" >左旋回</text>
 
  <!-- ストリーミング画像を表示するエリア -->
  <image id="svg_img" x="320px" xlink:href="" />
</svg>
 
<script>
 
// スクロールのON・OFFを切り替え
var txt_scl = $('#txt_scroll').get(0);
 
// スクロールON・OFFフラグ
var flag = false;
 
// スクロールをキャンセル
var chgScl = function( e ){
  // デフォルトの処理をキャンセル
  e.preventDefault();
}
 
// スクロールのON・OFFを切り替える関数
function changeScroll(){
  if(flag) {
    //スクロール復帰(スクロールキャンセル関数をイベントリスナーから削除)
    document.removeEventListener('touchmove', chgScl, { passive: false });
    txt_scl.textContent = 'スクロールON';
  } else {
    //スクロール禁止(スクロールキャンセル関数をイベントリスナーに登録)
    document.addEventListener('touchmove', chgScl, { passive: false });
    txt_scl.textContent = 'スクロールOFF';
  }
  // スクロールフラグを切り替え
  flag = !flag;　　
}
 
// スクロールON・OFFの文字列をクリックして切り替え
$("#txt_scroll").click(changeScroll);
 
 
// MJPG-streamerのStreamを設定(スマホ用にホスト名を動的に設定) 
var element = document.getElementById('svg_img');
element.setAttribute('xlink:href',
　　'http://'+location.hostname+':9000/?action=stream');
 
// 表示した3秒後にコントローラーのガイドを消去
$(function(){
  var fTxt = $('#txt_fwd').get(0);
  var rTxt = $('#txt_rgt').get(0);
  var bTxt = $('#txt_bck').get(0);
  var lTxt = $('#txt_lft').get(0);
 
  setTimeout(function(){
    fTxt.textContent = "";
    rTxt.textContent = "";
    bTxt.textContent = "";
    lTxt.textContent = "";
  },3000);
});
 
</script> 
</body>
</html>

JavascriptやCSS(スタイルシート)などは、別のファイルにした方がいいのですが、ここでは内容を追いやすいように1つのファイルにまとめています。

script.py

前回とだいたい同じですが、Pythonのプログラムファイルです。

import webiopi
 
GPIO = webiopi.GPIO
 
R_FWD  = 2 
R_BACK = 3
L_FWD  = 27
L_BACK = 17

 
def setup():
    # GPIOをPWMに設定
    GPIO.setFunction(R_FWD,  GPIO.PWM)
    GPIO.setFunction(R_BACK, GPIO.PWM)
    GPIO.setFunction(L_FWD,  GPIO.PWM)
    GPIO.setFunction(L_BACK, GPIO.PWM)

    # 初期化(デューティー比0)
    GPIO.pwmWrite(R_FWD,  0)
    GPIO.pwmWrite(R_BACK, 0)
    GPIO.pwmWrite(L_FWD,  0)
    GPIO.pwmWrite(L_BACK, 0)

# WebIOPiにより繰り返される関数
def loop():
    webiopi.sleep(5)

# WebIOPi終了時に呼ばれる関数
def destroy():
   # GPIO関数のリセット
    GPIO.setFunction(R_FWD,  GPIO.IN)
    GPIO.setFunction(R_BACK, GPIO.IN)
    GPIO.setFunction(L_FWD,  GPIO.IN)
    GPIO.setFunction(L_BACK, GPIO.IN)

動作確認

WebIOPiとMJPG-Streamerを起動し、パソコンやスマホのブラウザからアクセスして動作を確認します。

$ sudo systemctl start webiopi

$ LD_LIBRARY_PATH=/opt/mjpg-streamer/ /opt/mjpg-streamer/mjpg_streamer -i "input_raspicam.so -fps 15 -q 50" -o "output_http.so -p 9000 -w /opt/mjpg-streamer/www"