人力操作ロボット車両をつくる

システム構成要素
Raspberry Pi 4準備
メインコントローラ ⇔ サブコントローラ間通信
サブコントローラ ⇔ Micro:bit間通信
メインコントローラ ⇔ ジョイスティック間通信
IMU（9軸センサー）
カメラ画像補正（広角や魚眼レンズの場合）
micro:bit code（tiny:bitに実装）
ロボット車両動作ビデオ
Pythonスクリプトに関して

システム構成要素

車両ハードウェア　　　tiny:bit
車両コントローラ　　　micro:bit
サブコントローラ　　　Raspberry Pi 4
サブコントローラ電源　モバイルバッテリー
走行画像撮影　　　　　Raspi Camera
IMU　　　　　　　　　９軸センサーモジュール（BNO055）
メインコントローラ　　Linux PC（ubuntu220.04LTS@note PC）
操作用ジョイステック　Dual Shock 4

Raspberry Pi 4準備

インストール

最新64bit OS Trixie版をインストール
OpenCV（画像操作用）：$sudo apt-get install python3-opencv
Picamera2（Camera用）：$sudo apt install python3-picamera2
Raspi Camera動作確認
- OSがBookworm版以降はインストール時点でカメラは有効
- 以下のpython3スクリプトで確認

import cv2
from picamera2 import Picamera2

picam = Picamera2()
picam.configure(picam.create_preview_configuration(main={"format": 'XRGB8888', "size": (640, 480)}))
picam.start()
while True:
  img = picam.capture_array()
  cv2.imshow("Camera", img)
 
  key = cv2.waitKey(1)
  # Escキーを入力されたら画面を閉じる
  if key == 27:
    break
picam.stop()
cv2.destroyAllWindows()

メインコントローラ ⇔ サブコントローラ間通信

通信方式

無線通信：WiFi
ローカルIPアドレス：192.168.xx.xx
TCP Socket Port：5569 / 5570 / 5571
TCP Socket Server & Client

通信データ

走行前方画像：format: XRGB8888, size: 640×480 JPEG（品質70%）
走行時左右車輪駆動データ：±0.000～1.000（浮動小数点以下3桁）
IMUデータ：Gyro / Euler / Gravity

TCP Socket Server Script (Python3)@Sub Controller

import socketserver

class MyTCPHandler(socketserver.BaseRequestHandler):
    """
    The request handler class for our server.

    It is instantiated once per connection to the server, and must
    override the handle() method to implement communication to the
    client.
    """

    def handle(self):
        # self.request is the TCP socket connected to the client
        pieces = [b'']
        total = 0
        while b'\n' not in pieces[-1] and total < 10_000:
            pieces.append(self.request.recv(2000))
            total += len(pieces[-1])
        self.data = b''.join(pieces)
        print(f"Received from {self.client_address[0]}:")
        print(self.data.decode("utf-8"))
        # just send back the same data, but upper-cased
        self.request.sendall(self.data.upper())
        # after we return, the socket will be closed.

if __name__ == "__main__":
    HOST, PORT = "localhost", 9999

    # Create the server, binding to localhost on port 9999
    with socketserver.TCPServer((HOST, PORT), MyTCPHandler) as server:
        # Activate the server; this will keep running until you
        # interrupt the program with Ctrl-C
        server.serve_forever()

TCP Socket Client Script (Python3)@Main Controller

import socket
import sys

HOST, PORT = "localhost", 9999
data = " ".join(sys.argv[1:])

# Create a socket (SOCK_STREAM means a TCP socket)
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock:
    # Connect to server and send data
    sock.connect((HOST, PORT))
    sock.sendall(bytes(data, "utf-8"))
    sock.sendall(b"\n")

    # Receive data from the server and shut down
    received = str(sock.recv(1024), "utf-8")

print("Sent:    ", data)
print("Received:", received)

サブコントローラ ⇔ Micro:bit間通信

通信方式

有線通信：USB Serial
Baud rate：115200
Raspberry Pi デバイス：/dev/ttyACM0

通信データ

走行時左右車輪駆動データ：±0～255 整数
データフォーマット：lxxx,rxxx, （+は送信せず）

参考スクリプト

pyserialモジュールを使用したスレッド版は、以下のページ等を参考にして作成。

Raspberry Pi + PySerialでシリアル通信 - Qiita

はじめに Raspberry Piでシリアル通信したいと思い、USBシリアル変換を使った通信を試しました。「Raspberry Piでシリアル通信試してみた」を参考にさせてもらいました。コマンド送信→コマンド受信の順で処理することを前提...

メインコントローラ ⇔ ジョイスティック間通信

通信方式

2.4GHz 周波数帯
Bluetooth Ver2.1+EDR

通信データ

HID (Human Interface Device) プロファイル
入力データ: アナログスティックのX/Y座標

Dual Shock4関連モジュール

ds4drv（sudo pip install ds4drv でインストール）
ds4drv起動後　[error][controller 1] Failed to create input device: …. 表示
- errorの理由は不明だが動作する（ubuntu22.04非サポートの為か？）

参考ライブラリ（ROS2 Documentationページ参照）

Writing a simple publisher and subscriber (Python) — ROS 2 Documentation: Foxy documentation

ROS2インストール参考ページ

実習ROS 2 インストール - Qiita

概要この記事では、ROS 2のインストール方法について説明する。対応関係 ROSにはバージョンがあり、そのバージョンに応じてサポートされているOSのバージョンが異なる。また、1つのバージョンのROSは複数のOSをサポートすることがあり...

ペアリング

Linux PCのBluetoothを有効に
DS4の中央 [PSボタン] を押してペアリング開始
Linux PC側でWireless Controller(DS4)を検出したら接続する

IMU（9軸センサー）

BNO055搭載モジュール　(Bosch Sensortec製：I2C接続）
取得情報
- Gyro : 角速度センサーとして1秒間に物体がどれだけ3軸(XYZ)方向の角度変化量（rad/s）
- Euler : 3軸(XYZ)に対するオイラー角（roll: ±90°, pitch: ±180, yaw: 0～360）
- Acceleration : 3軸(XYZ)に対する重力+直線ベクトル+（m/s2）
精度：小数点以下4桁
将来の自立運転（SLAM）に使用する予定
Raspberry Pi設定
- [設定]->[Control Center]->[インターフェース]->[I2C] 有効
- /boot/firmware/config.txt に dtparam=i2c_arm_baudrate=400000 を追加
- I2Cで接続されているデバイスの確認
  - $ i2cdetect -y 1
  - 表示されるアドレスを確認（0x29 等）
実行スクリプト（Python3）
- 以下のページに記載してあるPythonスクリプトのIC2アドレスを、上記で確認したアドレスに修正
- def __init__(self, sensorId=-1, address=0x29):

カメラ画像補正（広角や魚眼レンズの場合）

チェッカー柄による補正係数取得

様々な角度からチェッカー柄を撮影する（20枚程度）
OSバージョンによりカメラ取得の方法を変更必要
画角、半径方向の歪み係数、円周方向の歪み係数を求める
以下ページ参照

魚眼レンズの補正 - Qiita

魚眼レンズの補正はじめに研究で使用しているドローンが魚眼レンズなため、取得した画像を補正する必要が出てきた。参照から一般的な補正は、rectilinear補正らしく、opencvでできるみたいなので触ってみた。環境構築 python...

画像補正

map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), K, DIM, cv2.CV_16SC2)
undistorted_img = cv2.remap(img, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT)

補正後の画角を拡大

nK = K.copy()
nK[0,0]=K[0,0]/1.2
nK[1,1]=K[1,1]/1.2
map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), nK, DIM, cv2.CV_16SC2)

以下ページ参照

Python で魚眼レンズの歪み補正

【経緯】使えれば幸い程度に．Python のバージョンは 3.8-32．使用カメラは ELP USB8MP02G-L180 (Amazon)．【魚眼レンズの補正】・参考記事：魚眼レンズの補正 - Qiitaキャリブレーション用にチェッカー柄...

micro:bit code（tiny:bitに実装）

初期化部

シリアル通信：通信先をUSBに変更
変数を初期化：0を設定

受信部（無限ループ）

受信区切り：「 , 」カンマまで受信
左右区別：「 l 」⇒ 左ジョイスティック、「r」⇒ 左ジョイスティック値
ジョイスティック値：操作量（文字列）を切り出して数値化(±0～255)
絶対値：逆方向用に準備

モーター駆動部（無限ループ）

左右操作量の符号が同一の場合
- 正値：CarCtrSpeed2 Run speed1=左量 speed2=右量 ⇒ 前方操作
- 負値：CarCtrSpeed2 Back speed1=左量 speed2=右量 ⇒ 後方操作
左右操作量の符号が逆の場合
- 左正/右負：CarCtrSpeed SpinRight speed=左量 ⇒ 右回りスピン操作
- 左負/右正：CarCtrSpeed SpinLeft speed=右量 ⇒ 左回りスピン操作

micro:bit表示部（ハート点滅ループ）