はじめに
こんにちは、にっしゃんです。こちらはROS Advent Calendar 2021の22日目の記事です。
この記事の3行サマリーは次のようになっております。
- Realsense T265で簡単に自己位置推定できるよ。
- ラズパイ4で持ち運べるロガーを作ったよ。
- この装置を使って、ディズニーランドのアトラクションの乗り物のルートが可視化できたよ
縦に長い記事なので、興味のあるところを好きに読んでいただければ幸いです。
目次
1.構想：「自己位置推定の自己って私のことだよね？」
2.制作：ラズパイなら持ち運べる
3.ディズニーランドでrosbagの取得＆再生してルートの可視化
#1. 構想：「自己位置推定の自己って私のことだよね？」
#1-1. Realsense T265
INTELのRealsense T265は先日ディスコン(=生産終了)が発表され、一部のユーザからは落胆と継続販売の声があがったことも記憶に新しいです。このT265の良いところ/悪いところを挙げますと
良いところ
- 3D自己位置が簡単に取れる
- 精度が高い(HMDの位置トラッキングに使えるほど)
- 乱暴に使っても自己位置がはずれない
- IMUが載ってる
- コンパクトに自己位置推定を実現できる（本来、魚眼ステレオカメラ+GPUが必要）
- ROS対応
- 安価(販売時は2.5万円ほど)
悪いところ
- 長時間使ってると急に自己位置がはずれることがある
- 振動には弱く自己位置がはずれる
- ソフトの改善がなされなかった（例：covarianceが常に固定だったり）
- ホストPCやUSBハブとの相性により、電源ON時にUSBケーブルを挿していると認識しない。挿し直す必要あり…。
- 既に市場在庫が品薄
というように、デメリットをカバーできれば「つなぐだけで自己位置が取れる」というメリットを享受できる製品です。T265のなにか良い使い方はないかとずっと考えておりました。
##1-2. 「私はどこ？」
自分の視覚や平衡感覚を超えて激しく動く遊園地のアトラクションに乗っていると、「この乗り物はいったいどういうルートを通ってきたのか？」と分からなくなりませんか？ ジェットコースターやウォータースライダーのような屋外にそびえ立つアトラクションならば、レールや水路を見ると一目瞭然なのです。一方、建物に密閉された屋内の乗り物の場合、俯瞰的に見ることができませんので、もうそのルートは設計者やスタッフしか分かりません。
屋内のアトラクションがたくさんある遊園地といえば東京ディズニーランド。ネットで検索しても乗り物が通る道筋を示したマップのようなものはヒットしませんでした。これはルートの可視化にチャレンジしたい。
#2. 制作：ラズパイなら持ち運べる
##2-1. 要求と要件
使い方(=要求)は以下のように決めました。
- 手で持って自己位置のログを取る。
- スマホやカメラで撮影しながら乗っても許される乗り物だけで使う。ジェットコースターのような激しく動く乗り物では使わない。
- ボタン操作でログの取得を開始/終了する
- ディズニーランドには朝から晩まで1日中居る。
この使い方から要件は以下のように決めました。
- 手で持てるように、大きさはコンパクトな方が良い。デジカメぐらいのサイズを想定。
- Realsense T265を使って自己位置を把握する。
- 自己位置のログはrosbagで取る。
- ログを取るときだけシステムの電源をONする。
- 数十回のログが録れるだけのストレージ容量とバッテリーを確保する。
外でrosbagを録るなら、ましてディズニーランドで使うなら、ディスプレイやキーボードが無くても動かせるようにしないといけません。最低限の入出力なら、決められた手順であれば出来るでしょう。
##2-2. 設計
以上の要件を満たすシステム構成は以下のようにしました。
ハード
- Realsense T265 + 純正USBケーブル
- Raspberry Pi 4
- モバイルバッテリー (Anker PowerCore, 10000mAh, 出力 5V 2.4A)
- タクトスイッチ 2個
- LED 1個
ソフト
- ROS1 Melodic on Ubuntu 18
- realsense_rosパッケージ
ラズパイ4は3Aの容量を流せる電源が必要なのですが、手持ちのAnkerのモバイルバッテリー(2.4A)を試したところ、T265を接続しても問題なく動作しました。動作中に不安定になることもなかったのは意外でした。
具体的に、以下のような使い方となるよう機能を盛り込みます。
- ラズパイは、電源USBケーブルから電源供給されて立ち上がる。つまり、電源OFF時は電源USBケーブルを抜いておく。
- Ubuntuが立ち上がったら、systemdで自動で必要なものを起動する。
- rosbag recordの開始/終了はタクトスイッチを長押しする。
- rosbag record中はLEDを点灯する。
- シャットダウン用のタクトスイッチを押すとシャットダウン。 ラズパイの黄色LEDが消えたら、電源USBケーブルを抜く。
##2-3. 実装
ハード
ラズパイケースに穴を2つ開けて、T265をネジ止めしています。
リアルセンスはINTELから「付属のUSBケーブルを使うように」と指示があるため、短い物が使えず、写真のように長い水色のケーブルがグルグル巻になってしまいました。でも結束バンドで固定したので大丈夫。
上記に挙げたT265の欠点「電源ONしてからUSBケーブルを挿す」ために、使わないUSB2.0側のポートをテープで塞いでいます。フールプルーフですね。あと、余ってた携帯電話のストラップを落下防止のために付けています。
うしろから見たところ。バッテリーとラズパイのケースは両面テープでガッチリくっつけています。
タクトスイッチはrec(rosbag record用)とShutdown用の2つ。ラズパイケースに穴を開けて取り付けました。
ラズパイのピンにはタクトスイッチの端子とLEDが刺さっております。LEDはrosbag record中に光って知らせます。
ラズパイのピン配置は次のようにしました。
- GPIO 23 (INPUT) ： シャットダウン用スイッチ
- GPIO 22 (INPUT) ： rosbag record start/stop スイッチ
- GPIO 25 (OUTPUT) ： rosbag record中を示すLED
ソフト
systemdで3つ自動起動しています。
T265の起動
/opt/username/bin/t265_logger_launch.sh を作成
#!/bin/sh
export PATH=/home/ubuntu/catkin_ws/devel/bin:/opt/ros/melodic/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin:$PATH
source /home/username/t265_logger_ws/devel/setup.bash
roslaunch realsense2_camera rs_t265.launch
/opt/username/sysconfig/ubuntu を作成
# cat <_EOF_ > /etc/sysconfig/ubuntu
PATH=/home/username/catkin_ws/devel/bin:/opt/ros/melodic/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin
_EOF_
/etc/systemd/system/T265Logger.service を作成
[Unit]
Description = T265 realsense logger launch service.
After=local-fs.target
ConditionPathExists=/opt/username/bin
[Service]
User=ubuntu
EnvironmentFile=/opt/username/sysconfig/ubuntu
ExecStart=/bin/bash -c 'bash /opt/username/bin/t265_logger_launch.sh'
Restart=no
Type=simple
[Install]
WantedBy=multi-user.target
~/t265_logger_ws/src/realsense-ros/realsense2_camera/launch/rs_t265.launch をカスタマイズ
<launch>
<node pkg="tf" type="static_transform_publisher" name="map2odm_broadcaster" args="0 0 0 0 0 0 map camera_odom_frame 100" />
<arg name="serial_no" default=""/>
<arg name="usb_port_id" default=""/>
<arg name="device_type" default="t265"/>
<arg name="json_file_path" default=""/>
<arg name="camera" default="camera"/>
<arg name="tf_prefix" default="$(arg camera)"/>
<arg name="tf_publish_rate" default="100" />
<arg name="fisheye_width" default="848"/>
<arg name="fisheye_height" default="800"/>
<arg name="enable_fisheye1" default="true"/>
<arg name="enable_fisheye2" default="true"/>
<arg name="fisheye_fps" default="30"/>
<arg name="gyro_fps" default="200"/>
<arg name="accel_fps" default="62"/>
<arg name="enable_gyro" default="true"/>
<arg name="enable_accel" default="true"/>
<arg name="enable_pose" default="true"/>
<arg name="enable_sync" default="false"/>
<arg name="linear_accel_cov" default="0.01"/>
<arg name="initial_reset" default="false"/>
<arg name="unite_imu_method" default="linear_interpolation"/>
<arg name="publish_odom_tf" default="true"/>
<group ns="$(arg camera)">
<include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
<arg name="tf_prefix" value="$(arg tf_prefix)"/>
<arg name="tf_publish_rate" value="$(arg tf_publish_rate)" />
<arg name="serial_no" value="$(arg serial_no)"/>
<arg name="usb_port_id" value="$(arg usb_port_id)"/>
<arg name="device_type" value="$(arg device_type)"/>
<arg name="json_file_path" value="$(arg json_file_path)"/>
<arg name="enable_sync" value="$(arg enable_sync)"/>
<arg name="fisheye_width" value="$(arg fisheye_width)"/>
<arg name="fisheye_height" value="$(arg fisheye_height)"/>
<arg name="enable_fisheye1" value="$(arg enable_fisheye1)"/>
<arg name="enable_fisheye2" value="$(arg enable_fisheye2)"/>
<arg name="fisheye_fps" value="$(arg fisheye_fps)"/>
<arg name="gyro_fps" value="$(arg gyro_fps)"/>
<arg name="accel_fps" value="$(arg accel_fps)"/>
<arg name="enable_gyro" value="$(arg enable_gyro)"/>
<arg name="enable_accel" value="$(arg enable_accel)"/>
<arg name="enable_pose" value="$(arg enable_pose)"/>
<arg name="linear_accel_cov" value="$(arg linear_accel_cov)"/>
<arg name="initial_reset" value="$(arg initial_reset)"/>
<arg name="unite_imu_method" value="$(arg unite_imu_method)"/>
<arg name="publish_odom_tf" value="$(arg publish_odom_tf)"/>
</include>
</group>
</launch>
pigpioのデーモンの起動
/etc/systemd/system/pigpiod.serviceを作成。
[Unit]
Description = pigpio daemon
[Service]
ExecStart = /usr/local/bin/pigpiod
Restart = always
Type = forking
[Install]
WantedBy = multi-user.target
タクトスイッチとLEDを操作するpythonスクリプトの起動
/opt/username/bin/t265_logger_gpio.sh を作成
#!/bin/sh
export PATH=/home/ubuntu/catkin_ws/devel/bin:/opt/ros/melodic/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin:$PATH
source /home/username/t265_logger_ws/devel/setup.bash
python2 /home/username/t265_logger_ws/gpio_rosrecord.py
/etc/systemd/system/T265LoggerGpio.service を作成
[Unit]
Description = T265 realsense logger gpio service.
After=local-fs.target
ConditionPathExists=/opt/username/bin
[Service]
User=ubuntu
EnvironmentFile=/opt/username/sysconfig/ubuntu
ExecStart=/bin/bash -c 'bash /opt/username/bin/t265_logger_gpio.sh'
Restart=no
Type=simple
[Install]
WantedBy=multi-user.target
~/t265_logger_ws/gpio_rosrecord.pyを作成
#!/usr/bin/python
# coding:utf-8
# gpio_triggered_rosrecord.py
import time
import pigpio
import os
class GPIO_CONTROL:
shutdown_pin_number = 23 # shutdown button
rosbag_pin_number = 22 # rosbag record start/end button
led_pin_number = 25 # LED shows status of rosbag recording
recording_status = False
sw_counter = 0
rec_counter = 0
sw_press_tick = 0
rec_press_tick = 0
def __init__(self):
self.pi_sw = pigpio.pi()
self.pi_sw.set_mode(self.shutdown_pin_number, pigpio.INPUT)
self.pi_sw.set_pull_up_down(self.shutdown_pin_number, pigpio.PUD_UP)
self.pi_rec = pigpio.pi()
self.pi_rec.set_mode(self.rosbag_pin_number, pigpio.INPUT)
self.pi_rec.set_pull_up_down(self.rosbag_pin_number, pigpio.PUD_UP)
self.pi_led = pigpio.pi()
self.pi_led.set_mode(self.led_pin_number, pigpio.OUTPUT)
self.pi_led.write(self.led_pin_number, 0)
self.cb_sw = self.pi_sw.callback(self.shutdown_pin_number,
pigpio.EITHER_EDGE, self.cb_interrupt_sw)
self.cb_rec = self.pi_rec.callback(
self.rosbag_pin_number, pigpio.EITHER_EDGE, self.cb_interrupt_rec)
def cb_interrupt_sw(self, gpio, level, tick):
# print(gpio, level, tick)
if level == 0:
self.sw_press_tick = tick
elif level == 1:
diff = pigpio.tickDiff(self.sw_press_tick, tick)
if diff > 3000 * 1000: # 3秒以上長押し
print("長押し検知！ Shutdown")
os.system("sudo shutdown -h now")
def cb_interrupt_rec(self, gpio, level, tick):
# print(gpio, level, tick)
if level == 0:
self.rec_press_tick = tick
elif level == 1:
diff = pigpio.tickDiff(self.rec_press_tick, tick)
if diff > 3000 * 1000: # 3秒以上長押し
print("長押し検知！ rosbag record")
if self.recording_status: # Start -> End
self.recording_status = not self.recording_status
self.pi_led.write(self.led_pin_number,
self.recording_status)
os.system("rosnode kill /my_bag")
else: # Stop -> Start
self.recording_status = not self.recording_status
self.pi_led.write(self.led_pin_number,
self.recording_status)
os.system(
"rosbag record -o ~/rosbag_log/test.bag -a __name:=my_bag &")
def main():
gc = GPIO_CONTROL()
while True:
time.sleep(0.01)
if __name__ == "__main__":
main()
pythonはROS melodicと合わせてpython2を使ってますが、python3でも良かったですね。
サービスの登録
以上の3つのサービスを以下の手順で登録しました。
$ sudo systemctl daemon-reload
$ sudo systemctl enable xxx.service
##2-4. テスト：動かしてみる
自宅の階段でテスト
階段を登って降りた軌跡の動画
— nisshan_ にっしゃん (@nisshan_) November 13, 2021
(8倍速で再生) pic.twitter.com/aO9c6TH7Ey
道路を歩行してテスト
Realsense T265を持って街を歩いた軌跡。
— nisshan_ にっしゃん (@nisshan_) November 19, 2021
(8倍速で再生)
ループクローズしなかったけど、カメラの向きがスタート時とゴール時で逆だったことが原因かな、多分。#ros pic.twitter.com/dsBkoH5sJI
いい感じですね。
#3. ディズニーランドでrosbagの取得＆再生してルートの可視化
完成したT265搭載自己位置ロガーを持って、ディスニーランドで乗り物に乗りながらデータを記録しました。自宅で ~/rosbag_log にあるrosbagを別のUbuntu PCに移し、rosbagを再生したrvizで表示しました。
いずれの動画も、視点は上空の鳥瞰で見ています。再生速度は10倍です。
あと、自己位置だけでなく、移動速度も知りたかったので、簡単なノードを作成してvelocityを計算して表示しています。表示にはjsk_rviz_pluginのplotter2Dを使っています。
##3-1. 成功：イッツ・ア・スモールワールド
綺麗に自己位置が取れ、ボートが進むルートが良く分かります。建物内の限られた空間を目一杯利用して、なるべく長いルートとなるよう工夫していることが伺えます。
##3-2. やや成功：プーさんのハニーハント
スタート地点とゴール地点は近いはずなのですが、このデータはやや離れております。おそらく、次のような理由でどこか途中でズレてしまったのだと思います。
- アトラクションの最中は比較的暗い。Visual Odometryがおかしくなったかも。
- 急に旋回する動きに、追従できなかったのではないか。
とはいえ、特徴は良く出てると思いますね。
途中にティガーがピョンピョン跳ねると地面と車両がボヨンボヨン揺れる演出があるのですが、そこではT265はトリックに惑わされていないですね。面白いです。
##3-3. 失敗
最後に上手くいかなかったものも紹介します。グリッドの中心がスタート地点です。
↑大きな蒸気機関船、マーク・トゥエイン号です。ウェスタンリバーという川を（実際には池なのですが）、ぐるっと大きく緩やかに一周して帰ってきます。まったく違う軌跡になってしまってますね。船の最上段かつ最前列でデータ録りしたのですが、広大な池だとダメなのかな。あと魚眼レンズの映像には船も写ってたのでそれが影響しているかもしれません。
↑ウェスタンリバー鉄道。乗ったのが日没後だったので、真っ暗闇でした。Visual Odometryには全く向かない環境ですね。左のウネウネと往復している部分は搭乗前に並んでいる通路(照明で明るい)です。そこからスタートしてすぐにダメになってますね。
↑カリブの海賊。ウェスタンリバー鉄道同様に中が暗く、ダメでした。
まとめ
Realsense T265による自己位置推定をラズパイ4でロギングできる装置を作りました。それを使ってディズニーランドの乗り物が動く軌跡を可視化しました。センサーの性能の限界は最初から想定していましたので、「全部失敗するかもな」と不安でしたが、成功したものがあって良かったです。
今回は自己位置推定できるT265だけを搭載していましたが、D435も載せて点群を取ってみるのも面白そうです。
さて、これを持って今度はどこにでかけましょうかね。
参考にしたサイト
- systemd
- rosbag recordの開始と停止の方法
- シャットダウンのスイッチ
- ラズパイのGPIOをread/writeする
- Raspberry PiのGPIO制御の決定版 pigpio を試す - karaage. [からあげ] -
- 2019/6/27(raspberry piのROS上でpigpioを使ってLチカしてみる) - へっぽこ元ロボコニスト -
- Raspberry Pi & Ubuntu 18.04 でGPIOを使う - Qiita -
- [電子工作ファン向けのRaspberry Pi・セットアップ（Ubuntu 18.04LTS編） - Qiita -] (https://qiita.com/myasu/items/7c61750000c2e8b06fe1)
- Raspberry Pi 3 systemdでpigpiodの自動起動 | 定年まで泣くんじゃない -
- scp転送