MatrixLEDパネルをROSから制御する

はじめに

秋葉原のShigezoneさんからMatrix LED(64x64)を購入しましたが、ROSで動作させてみようと思い立ちました。この記事では、「ImageトピックをSubscribeしMatrix LEDに画像を表示するROSノード」を作ります。

環境準備

Raspberry Piの準備

今回はROSを取り扱うため、UbuntuをインストールしたRaspberry Pi3を利用します。
こちらのイメージをSDカードにフラッシュし、ROSをインストールします。arm64のUbuntu18.04.3をインストールしました。ROSは、ros-melodic-desktopをこちらのサイトを参考にインストールしました。

Matrix LEDパネル接続ボードの準備

MatrixLEDのドライバとしてこちらのソフトウェアを利用するため、対応するドライバボードを準備しました。
ボードはelectrodragonから入手しましたが、自分で準備してもよいと思います。購入したボードには3.3V→5Vレベルコンバータが内蔵されているため、安定した動作が期待できます。
使用するMatrixLEDは64x64のサイズでありアドレスラインがA～Eまであるため、ドライバボード側も「E」を結線するよう1か所はんだ付けする必要があります。今回購入したMatrixLEDパネルでは、Pin8をEに接続するようジャンパを設定しました。
LEDパネル本体には5V(少なくとも3Aだと思います)を別途供給する必要があります。

LEDパネルドライバの準備

ハードウェアが正常に動作するか確認するため、リポジトリからcloneした後サンプルプログラムをコンパイルします。

rpi-rgb-led-matrix$ make -C examples-api-use
rpi-rgb-led-matrix$ sudo examples-api-use/demo -D0 --led-rows=64 --led-cols=64 --led-panel-type=FM6126A

カラフルなキューブがくるくるするはずです。
Matrix LEDパネルの種類によってはFM6126Aをチップとして搭載されている場合もあり、その場合は初期化に必要な信号が異なるためオプションの指定が必要です。

問題なく動作するようであれば、同リポジトリの手順に従ってPython用のライブラリをインストールします。

$ sudo apt-get update && sudo apt-get install python2.7-dev python-pillow -y
rpi-rgb-led-matrix/bindings/python$ make build-python
rpi-rgb-led-matrix/bindings/python$ sudo make install-python

ここまでくれば、「from rgbmatrix import RGBMatrix, RGBMatrixOptions」ができるようになります。

ROSをsudoで使うために

今回用いたMatrixLEDドライバソフトウェアはsudoで実行する必要があります。
Ubuntu18.04+Raspberry pi3の組み合わせではどうにも一般ユーザで必要なハードウェアリソース(GPIOなど)にアクセスさせることができなかったため、ROS側をsudoで動作させてしまうことにしました。
(本来であれば一般ユーザで必要なハードウェアリソースにアクセスできるよう設定するべきかと思います。)

一般的に、sudo実行時には環境変数がクリアされてしまうため、ROSがうまく動きません。
そこで、sudoersファイルに設定を追加することにしました。

Defaults        env_keep += "PATH PKG_CONFIG_PATH PYTHONPATH ROS_ROOT ROS_ETC_DIR ROS_MASTER_URI ROS_VERSION ROS_PYTHON_VERSION ROS_PACKAGE_PATH ROS_DISTRO CMAKE_PREFIX_PATH LD_LIBRARY_PATH"

「sudo visudo」コマンドでこの1行を追加することで、sudo実行時にROSに必要な環境変数が引き継がれます。ただし、LD_LIBRARY_PATHについては仕様により引き継がれません。

そこで、必要なパスを「/etc/ld.so.conf.d/ros.conf」に記載してリンクさせることにしました。
ROS merodicの場合はこんな感じです。

/home/user/catkin_ws/devel/lib
/opt/ros/melodic/lib

設定が終わったら、「sudo ldconfig」を実行します。

さらに、/home/user/.ros/logに対して、daemonユーザがアクセスできるようにパーミッション設定をしておきます。ここがないと、ROSのノード実行時にPermission errorで止まってしまいます。
(sudoでROSのノードを実行する場合、Logファイルはdaemonユーザとして生成されるようです。)
ここまでくれば、sudoを付けてもROSのリソースにアクセスできると思います。

ROSノードの実装

まずは、ROSパッケージを作ります。

~/catkin_ws/src$ catkin_create_pkg matrix_led_ros roscpp rospy std_msgs
~/catkin_ws$ catkin_make

ここからは、作成したmatrix_led_rosパッケージ配下にスクリプトを実装していきます。

GIFからImageトピックをPublishするノードを作る

次に作るプログラムのテストをするために、GIFを読み込んでImageトピックとしてPublishするノードを実装しました。

#!/usr/bin/env python
from __future__ import print_function

import roslib
import sys
import rospy
import cv2
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
import argparse

FILE_NAME = "test.gif"
REFRESH_RATE = 1
parser = argparse.ArgumentParser(
            prog='gif-publisher.py',
            usage='python gif-publisher --filename inputfilename',
            description='publish Image message from gif file',
            epilog='end',
            add_help=True,
            )
parser.add_argument('-f', '--filename', help='input file name',
                    required=True)
parser.add_argument('-r', '--rate', help='refresh rate',type=int)
args = parser.parse_args()

if args.filename:
  FILE_NAME = args.filename
if args.rate:
  REFRESH_RATE = args.rate

class image_publisher:
  def __init__(self):
    self.image_pub = rospy.Publisher("/imagetopic",Image)
    self.bridge = CvBridge()

  def readGif(self, filename, hz=1):
    gif = cv2.VideoCapture(filename)
    r = rospy.Rate(hz)
    while not rospy.is_shutdown():
        try:
            stat, frame = gif.read()
            if not stat:
                gif = cv2.VideoCapture(filename)
            else:
              try:
                self.image_pub.publish(self.bridge.cv2_to_imgmsg(frame, "bgr8"))
                r.sleep()
              except CvBridgeError as e:
                  print(e)
        except KeyboardInterrupt:
            break

def main(args):
  ip = image_publisher()
  rospy.init_node('gif_image_publisher', anonymous=True)
  ip.readGif(FILE_NAME, REFRESH_RATE)
  try:
    rospy.spin()
  except KeyboardInterrupt:
    print("Shutting down")

if __name__ == '__main__':
    main(sys.argv)

「python gif-publisher.py -f test.gif -r 10」のように使います。この例では、test.gifを10fpsで/imagetopicという名前でPublishします。
何も指定しないと爆速でフレームがPublishされてしまうので、「r = rospy.Rate(hz)」と「r.sleep()」でフレームレートを設定しました。Raspberry piの場合、30fps弱まで動作しました。

Imageトピックを受け取るROSノードを作る

ようやく本題です。/imagetopicをSubscribeし、Matrix LEDパネルに表示させるスクリプトです。

#!/usr/bin/env python
from __future__ import print_function

import roslib
import sys, time
import rospy
import cv2
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
from rgbmatrix import RGBMatrix, RGBMatrixOptions
from PIL import Image as pilI

BRIGHTNESS = 0.5

class image_viewer:
  def __init__(self):
    self.bridge = CvBridge()
    self.image_sub = rospy.Subscriber("imagetopic",Image,self.callback)

    # Configuration for the matrix
    self.options = RGBMatrixOptions()
    self.options.rows = 64
    self.options.cols = 64
    self.options.chain_length = 1
    self.options.parallel = 1
    self.options.hardware_mapping = 'regular'
    self.matrix = RGBMatrix(options = self.options)
    self.max_brightness = self.matrix.brightness
    self.matrix.brightness = self.max_brightness * BRIGHTNESS
    self.double_buffer = self.matrix.CreateFrameCanvas()

  def toSquare(self, img):
    w, h =img.size
    if w == h:
        return img
    elif w > h:
        result = pilI.new(img.mode, (w, w), (0,0,0))
        result.paste(img, (0, (w - h) // 2))
        return result
    else:
        result = pilI.new(img.mode, (h, h), (0,0,0))
        result.paste(img, ((h - w) // 2, 0))
        return result

  def callback(self,data):
    try:
      cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
    except CvBridgeError as e:
      print(e)
    pilImage = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB)
    pilImage = pilI.fromarray(pilImage)
    pilImage.thumbnail((self.matrix.width, self.matrix.height), pilI.ANTIALIAS)
    offset = (int)(self.matrix.height - pilImage.height)/2

    self.double_buffer.SetImage(self.toSquare(pilImage), 0)
    self.double_buffer = self.matrix.SwapOnVSync(self.double_buffer)


def main(args):
  iv = image_viewer()
  rospy.init_node('image_viewer')
  try:
    rospy.spin()
  except KeyboardInterrupt:
    print("Shutting down")

if __name__ == '__main__':
    main(sys.argv)

「sudo python image-viewer-ros.py」のようにして起動します。このスクリプトは、root権限でないとうまく動作しませんが、先に「ROSをsudoで使うために」で設定した内容によりsudoでROSととともに動作させることができます。
動くとこんな感じ。こちらのGIFアニメをお借りしました。

「self.matrix = RGBMatrix(options = self.options)」の部分で、matrix LEDに渡すオプションを指定することができます。
64x64を超える画像は、長辺を64ピクセルに合わせて縮小し、余白を黒で埋めるようにしました。

終わりに

いかがでしたでしょうか。今回の実装では、少しちらつきが気になる部分もあり、改良の余地がありそうです。機会があれば、C++で実装したものも記載します。
また、今回利用したドライバボードは3チャネル同時使用ができるうえ、Matrix LEDとしては数珠繋ぎにして拡張が可能であるため、複数のLEDパネルが手に入れば高解像度化も試してみたいと思います。