LoginSignup
4
1

More than 3 years have passed since last update.

@darkimpact0626(小西 孝海)

RasberryPiMouseで自動運転実験記(1)　目指せ！自己位置推定(SLAM)

Last updated at Posted at 2020-04-25

やりたいこと

RasberryPiMouseというラズパイ搭載のラジコン(下図)で、自動操縦がやりたい。
自動操縦と言っても、

  • 人に追従するカルガモの子供みたいな自動運転
  • サーキットをライントレーサーを使って周回する自動運転
  • 人にぶつかりそうになったらブレーキを掛ける自動運転？というより自動制御？

みたいなのがありますが、ここで言う自動運転とは、PCからRasberryPiMouseに座標を送信すると、指定された座標に向かって自動的に進むこと、って定義してみました。
大きな倉庫とかで、車から降ろされた荷物を棚の前に運んでくれるロボットが作れないかなって思ったのがきっかけ。
まぁ、金を出せばそんなロボットは売ってるんだけど、社内のエンジニアがDIYで作りましたってなったら、なんか面白いかな、みたいな感じ。

raspymouse.jpg
https://products.rt-net.jp/micromouse/raspberry-pi-mouse

遊びでやるので、まずは小さくやってみます。
座標は、床に張ったカラーシールで識別します（安い）(下図)。
image.png

床に張ったシールは、RasberryPiMouseに別途カメラをつけて、物体検出で認識させる予定。
RasberryPiMouseが向いている方向は9軸センサで識別する予定。シールの位置から向きも推定できそうだけど、実際は画像からの計算結果と、センサーの結果を比べてデバッグとか補正とかに使えるんじゃないかなとか、まだぼんやりしているけど、とりあえず使えるようにしたい。
座標の中でさらに細かい位置は、カメラに映るシールの位置から詰める(下図)。
image.png

そんなにうまくいくのか、実験記を書いてみたいと思います。

とりあえず最初にやってみたこと

何はともあれ、RaspberryPiMouseを動かすことに慣れないと、上のやりたいことができるのかも判断できないので、ひとまずPCからRaspberryPiMouseをコントロールするところまでやってみました。

RasberryPiMouseってどうやって動かす？

すごく簡単。RaspberryPiMouseに搭載されているラズパイからecho文で動かせます。
モーターを回したいときは、以下のようにデバイスファイルに書き込めばOK。

echo 400 > /dev/rtmotor_raw_l0

これだけで左のモーターは1秒で1回転させられます。
デバイスファイルっ便利だなぁ…。

PCからRaspberryPiMouseのLEDをチカチカさせる

とりあえず、RaspBerryPiMouseについているLEDを自動で付いたり消したりするのを作ってみました。

ネットワークの構成

構成は単純です。ノートPCからRaspberryPiMouseのラズパイとWIFIでつなぎます。
image.png
メンドクサイのは、つなぐときにraspberryPiMouseのIPアドレスを調べないといけない点で、将来はLCDに表示とかやりたい。
とはいっても、DHCPでIPアドレスが一度割り振られると、そう簡単に変わるわけではないのですが、変わるとき、変わらないときについては条件があって、いつか別記事で書いてみたいな、と思います。
ただ、この問題はラズパイにhostnameを設定してしまえばOKなんですが、それもまたメンドクサイ…。

簡略式ラズパイの見つけ方

毎回IPアドレスを調べるのが面倒なときは、arpコマンドを使う。

arp -a

すると、ズラッとIPアドレスとMACアドレスの一覧が出ます。
このとき、macアドレスがb8・・・で始まるものがラズパイです。

raspberryPiMouseの中身

flaskベースで作ります。flaskで要求を受け付けて(インターフェース)、MouseController.pyでRaspberryPiMouseに命令を出します。
PCとRaspberryPiMouseとのやり取りはjson形式でやります。
こんなイメージです。
image.png

以下はflaskのコード。raspberryPiMouseの0番目のledをチカチカさせたかったら、ブラウザから以下のURLを開けば光る、という仕掛けです。

http://RaspberrypiMouseのIPアドレス/led_on/0

flaskのコード全体は以下。各要求に対してMouseクラスに命令を出します。

# coding:UTF-8
from flask import Flask, render_template, request, jsonify
from Mouse.MouseController import MouseController

app = Flask(__name__)
mouse = MouseController()


@app.route("/led_on/<index>", methods=["GET"])
def led_on(index=None):
    rtn = {"status":"OK"}
    if index != None and (index == "0" or index == "1" or index == "2" or index == "3"):
        mouse.led_on(index)
    else:
        rtn = {"status":"NG"}
    return jsonify(rtn)


@app.route("/led_off/<index>", methods=["GET"])
def led_off(index=None):
    rtn = {"status":"OK"}
    if index != None and (index == "0" or index == "1" or index == "2" or index == "3"):
        mouse.led_off(index)
    else:
        rtn = {"status":"NG"}
    return jsonify(rtn)


@app.route("/buzzer_on/<hz>", methods=["GET"])
def buzzer_on(hz=None):
    rtn = {"status":"OK"}
    if hz != None:
        mouse.buzzer_on(hz)
    else:
        rtn = {"status":"NG"}
    return jsonify(rtn)

@app.route("/buzzer_off", methods=["GET"])
def buzzer_off():
    rtn = {"status":"OK"}
    mouse.buzzer_off()

    return jsonify(rtn)


@app.route("/motor_left/<hz>", methods=["GET"])
def motor_left(hz=None):
    rtn = {"status":"OK"}
    if hz != None:
        mouse.motor_left(hz)
    else:
        rtn = {"status":"NG"}
    return jsonify(rtn)


@app.route("/motor_right/<hz>", methods=["GET"])
def motor_right(hz=None):
    rtn = {"status":"OK"}
    if hz != None:
        mouse.motor_right(hz)
    else:
        rtn = {"status":"NG"}
    return jsonify(rtn)

@app.route("/motor_both/<lhz>/<rhz>/<sec>", methods=["GET"])
def motor_both(lhz, rhz, sec):
    rtn = {"status":"OK"}
    mouse.motor_both(lhz, rhz, sec)
    return jsonify(rtn)

@app.route("/motor_start/<hz>", methods=["GET"])
def motor_start(hz):
    rtn = {"status":"OK"}
    mouse.motor_start(hz)
    return jsonify(rtn)

@app.route("/motor_stop", methods=["GET"])
def motor_stop():
    rtn = {"status":"OK"}
    mouse.motor_stop()
    return jsonify(rtn)

@app.route("/get_light_sensor", methods=["GET"])
def get_light_sensor():
    sensor = mouse.get_light_sensor()
    rtn = {"status":"OK"}
    rtn.update(sensor)
    return jsonify(rtn)


@app.route("/get_switch_status", methods=["GET"])
def get_switch_status():
    sensor = mouse.get_switch_status()
    rtn = {"status":"OK"}
    rtn.update(sensor)
    return jsonify(rtn)


if __name__ == '__main__':
    app.run(host="0.0.0.0", debug=False)

MouseController.pyは以下。要求された内容をデバイスファイルに書き込むだけですね。

# coding:UTF-8
import cv2
import json
import base64
import picamera
import picamera.array
from pyzbar.pyzbar import decode
from PIL import Image

class MouseController:
    def __init__(self):
        self.SOFTSWITCH = "/dev/rtmoteren0"

        self.LED = [r'/dev/rtled0', r'/dev/rtled1', r'/dev/rtled2', r'/dev/rtled3']
        self.SWITCH = [r'/dev/rtswitch0', r'/dev/rtswitch1', r'/dev/rtswitch2']

        self.BUZZER = "/dev/rtbuzzer0"
        self.MOTOR_RIGHT = "/dev/rtmotor_raw_r0"
        self.MOTOR_LEFT = "/dev/rtmotor_raw_l0"
        self.MOTOR_BOTH = "/dev/rtmotor0"
        self.LIGHT_SENSOR = "/dev/rtlightsensor0"
        #self.camera = picamera.PiCamera(resolution=(128, 128))
        #self.camera.resolution = (320, 240)
        #self.camera.vflip = True
        #self.camera.hflip = True

        #self.stream = picamera.array.PiRGBArray(self.camera)
        # ソフトウェアスイッチをONにする
        self.software_switch_on()

    def software_switch_on(self):
        with open(self.SOFTSWITCH, "w") as f:
            f.write("1")

    def software_switch_off(self):
        with open(self.SOFTSWITCH, "w") as f:
            f.write("0")

    """
    LED操作
    """
    def led_on(self, index):
        index = int(index)
        with open(self.LED[index], "w") as f:
            f.write("1")

    def led_off(self, index):
        index = int(index)
        with open(self.LED[index], "w") as f:
            f.write("0")

    """
    ブザー操作
    """
    def buzzer_on(self, hz):
        with open(self.BUZZER, "w") as f:
            f.write(hz)

    def buzzer_off(self):
        with open(self.BUZZER, "w") as f:
            f.write("0")

    """
    モーター操作
    """
    def motor_both(self, lhz, rhz, sec):
        with open(self.MOTOR_BOTH, "w") as f:
            cmd = "{} {} {}".format(lhz, rhz, sec)
            f.write(cmd)

    def motor_left(self, hz):
        with open(self.MOTOR_LEFT, "w") as f:
            f.write(str(hz))

    def motor_right(self, hz):
        with open(self.MOTOR_RIGHT, "w") as f:
            f.write(str(hz))

    def motor_start(self, hz):
        self.motor_left(hz)
        self.motor_right(hz)

    def motor_stop(self):
        self.motor_left(0)
        self.motor_right(0)

    """
    センサー取得
    """
    def get_light_sensor(self):
        with open(self.LIGHT_SENSOR, "r") as f:
            values = f.readline().strip()

        values = values.split(" ")
        return {"sensor0": values[0], "sensor1": values[1], "sensor2": values[2], "sensor3": values[3]}

    """
    スイッチ取得
    """
    def get_switch_status(self):
        status = {}
        for i, dev in enumerate(self.SWITCH):
            with open(dev, "r") as f:
                value = f.readline().strip()
                key = "switch{}".format(i)
                status[key] = value
        return status

PC側で毎回ブラウザで操作するのはしんどいので、pythonのrequestsを使って、コマンドで実行します。
以下のMouseクラスは、出せる命令をクラスにまとめたものです。self.URL2は動画のストリーミングに使いますが、これはまた別の記事で書きたいと思います。
ledをon、offしたければ、mouseクラスのled_on、led_offメソッドを交互に呼び出せばOK。

# coding:UTF-8
import requests
import json

class Mouse:
    def __init__(self):
        self.URL = "http://192.168.1.117:5000"
        self.URL2 = "http://192.168.1.15:5000"
        self.LED_STATUS = [False, False, False, False]
        self.BUZZER_STATUS = False

    def led(self, index):
        if self.LED_STATUS[index]:
            self.led_off(index)
            self.LED_STATUS[index] = False
        else:
            self.led_on(index)
            self.LED_STATUS[index] = True

    def led_on(self, index):
        req_url = "{}/led_on/{}".format(self.URL, index)
        r = requests.get(req_url)
        data = r.json()
        return data

    def led_off(self, index):
        req_url = "{}/led_off/{}".format(self.URL, index)
        r = requests.get(req_url)
        data = r.json()
        return data

    def buzzer(self, hz):
        if self.BUZZER_STATUS:
            self.buzzer_off()
            self.BUZZER_STATUS = False
        else:
            self.buzzer_on(hz)
            self.BUZZER_STATUS = True

    def buzzer_on(self, hz):
        req_url = "{}/buzzer_on/{}".format(self.URL, hz)
        r = requests.get(req_url)
        data = r.json()
        return data

    def buzzer_off(self):
        req_url = "{}/buzzer_off".format(self.URL)
        r = requests.get(req_url)
        data = r.json()
        return data

    def motor_both(self, lhz, rhz, sec):
        req_url = "{}/motor_both/{}/{}/{}".format(self.URL, lhz, rhz, sec)
        r = requests.get(req_url)
        data = r.json()
        return data

    def motor_left(self, hz):
        req_url = "{}/motor_left/{}".format(self.URL, hz)
        r = requests.get(req_url)
        data = r.json()
        return data

    def motor_right(self, hz):
        req_url = "{}/motor_right/{}".format(self.URL, hz)
        r = requests.get(req_url)
        data = r.json()
        return data

    def motor_start(self, hz):
        req_url = "{}/motor_start/{}".format(self.URL, hz)
        r = requests.get(req_url)
        data = r.json()
        return data

    def motor_stop(self):
        req_url = "{}/motor_stop".format(self.URL)
        r = requests.get(req_url)
        data = r.json()
        return data


    def get_light_sensor(self):
        get_url = "{}/get_light_sensor".format(self.URL)
        r = requests.get(get_url)
        data = r.json()
        return data

    def get_geomagnet(self):
        get_url = "{}/get_geomagnet".format(self.URL2)
        r = requests.get(get_url)
        data = r.json()
        return data

    def rotate(self, angle):
        data = self.get_geomagnet()
        rt_deg = data["angle"]
        default_speed = 400
        #2000秒で360度回転
        time = int(2000 * (abs(angle - rt_deg) / 360.))
        self.motor_both(-400, 400, time)
        print(rt_deg, angle, time)
        time.sleep(float(time/1000.))
        self.rorate_detail(angle)


    def rotate_detail(self, angle):
        if angle == 360:
            max_deg = 360.
            min_deg = 350.
        else:
            max_deg = angle + 5.
            min_deg = angle
        default_speed = 400
        while True:
            data = self.get_geomagnet()
            rt_deg = data["angle"]
            print(rt_deg)
            rt_deg = float(rt_deg)
            speed = int((abs(angle - rt_deg) / 360.) * default_speed) + 10

            if rt_deg < max_deg and rt_deg > min_deg:
                self.motor_stop()
                break
            elif rt_deg > max_deg:
                self.motor_left(speed)
                self.motor_right(-speed)
            elif rt_deg < min_deg:
                self.motor_left(-speed)
                self.motor_right(speed)

PCのクライアントはこんな感じで書けば、LEDがチカチカします。


# coding:UTF-8
import time
import cv2
from RtClient.RtClient import Mouse

if __name__ == '__main__':
    mouse = Mouse()
    while True:
        for i in range(4):
            mouse.led_on(i)
            time.sleep(0.1)
        for i in range(4):
            mouse.led_off(i)
            time.sleep(0.1)

カメラ映像をPCで見たい

LEDをチカチカさせた原理を使えば、モータの回転もPCから制御できるので、コントロールできます。
しかし、PCを操作し、RaspberryPiMouseの動きを見てまた操作、というのはツライ。
よって、次はRaspberryPiMouseにカメラをつけて、カメラ映像をPCで見ながら操作できるようにしたお話を書きたいと思います。
簡単なようで、画像の転送速度のスピードが出なくて、映像がカクカクしてしまい、まともに操作できなかったので、どうやって解消したか、について書きたいと思います（だれも望んでいない）。

4
1
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
4
1