ROS2 dashing + Ubuntu18.04でORB_SLAM2を動かしてみた

ORB_SLAM2をros2+ubuntu18.04で動かしてみた話。

ORB_SLAM2をubuntu18.04で動かした記事が一番参考になる。 ros2でORB_SLAM2を動かしてみたので動かし方を残す。

環境

ROS2 dashing Ubuntu 18.04 Intel® Core™ i7-6600U CPU @ 2.60GHz × 4 メモリ 16G

ros2のインストール

ここを参考にする。

Pangolinのインストール

昔インストールしていたので、特に何もしていない。 ここに記載の方法と同じで良い。

ORB_SLAM2のインストール

ここに記載の通り。

cd ~/ros_dev
git clone https://github.com/raulmur/ORB_SLAM2.git ORB_SLAM2
cd ORB_SLAM2
chmod +x build.sh
./build.sh

usleepのエラーが出るので対処する。 個人的にはこの対処法が好きなので、 自分のリポジトリに取り込む。 もう一度build.shを実行し、待つ。 libORB_SLAM2.soと各種exampleが出来上がる。

ORB_SLAM2を試す

特に何も考えずにTUM1を使用する。 ダウンロードはここから。 fr1がTUM1に相当している模様。 .tgzがSEQUENCEファイルのようなので、解凍したパスを覚えておく。 サンプルの実行は以下の通り。

./Examples/Monocular/mono_tum Vocabulary/ORBvoc.txt Examples/Monocular/TUM1.yaml PATH_TO_SEQUENCE_FOLDER

実行結果は以下の通りであった。トラッキングにかかる時間が33msec程度ってことのように見える。 論文だと他の処理時間も計測しているみたいだけど、どうやっているんだ？

median tracking time: 0.0328202
mean tracking time: 0.0341189

ros2_ORB_SLAM2のビルド

ここを使用する。 書いてあるとおりにビルドする。特に問題は出なかった。

ros2_ORB_SLAM2のテスト１

書いてある通りに実行すると画面が立ち上がる。 カメラのキャリブレーション方法は別途確認する。 image_toolsはid=0のvideoデバイスのみをpublishするので、 所望の結果を得るにはデバイスの認識順をどうにかするか、 image_toolsのソースを変更するかが必要。

• ターミナル1

  source <path-to-ros>/dashing/setup.sh
  ros2 run image_tools cam2image -t camera

• ターミナル2

  cd <path-to-ros2_ORB_SLAM2>
  source install/setup.sh
  export LD_LIBRARY_PATH=~/Pangolin/build/src/:~/ORB_SLAM2/Thirdparty/DBoW2/lib:~/ORB_SLAM2/Thirdparty/g2o/lib:~/ORB_SLAM2/lib:$LD_LIBRARY_PATH
  ros2 run ros2_orbslam mono ~/ORB_SLAM2/Vocabulary/ORBvoc.txt ~/ORB_SLAM2/Examples/Monocular/TUM1.yaml

ros2_ORB_SLAMのテスト２

テスト１同様にros2_orbslamを起動しておくが、購読するtopicは/cameraではなく、/camera/rgb/image_colorなので、ソースを修正して再ビルド。 (ros2にtopic_toolがあればそれを使っても良いが。それか知らないだけで実行時に変更できたりするのかも) 変更はsrc/monocular/monocular-slam-node.cpp内のcamera => camera/rgb/image_colorとすれば良い。 再ビルドして同じように実行する。
2020/11/10追記
実行時にremapオプション付加するのが正解みたい。
(ros2 run ros2_orbslam mono ~/ORB_SLAM2/Vocabulary/ORBvoc.txt ~/ORB_SLAM2/Examples/Monocular/TUM1.yaml --ros-args -r camera:=camera/rgb/image_color)

次はrosbagを使用してTUM1を流す。 リンク先でbagファイルを入手すること。

rosbag2のインストールは以下の通り。 ros1のbagイメージを再生するにはros1_bridgeが必要で、これのインストールにはmelodicのapt設定が必要(dashingの場合)

sudo su -
sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key  421C365BD9FF1F717815A3895523BAEEB01FA116
apt update
apt upgrade
apt install ros-dashing-ros2bag ros-dashing-rosbag2*

TUM1のデータは以下の通り再生可能。そのままだと再生できないのでここを参考にした。

source /opt/ros/melodic/setup.sh
source /opt/ros/dashing/setup.sh
ros2 bag play -s rosbag_v2 rgbd_dataset_freiburg1_xyz.bag -r 1000000

テスト１と同じように動作が確認できる。

その他

ros2_ORB_SLAM2から各種topicが出ていないようなので調べてみると、 どうも/cameraでイメージをsubscribeだけして動作させるだけの模様。 元のORB_SLAM2のros1向けのノード実装も同じ動作だったようなので、これの移植かと思う。 本格的には多分これのros2ブランチを使用する必要があるかも。 これは後で試してみる。

カメラの動作確認メモ

対象:logicool c270
cheeseで動作しない。
fswebcamでは動作確認できた。
qv4l2では動作確認できた。

http://joe.ash.jp/program/ros/tutorial/tutorial_camera.htm https://qiita.com/naoppy/items/74bdfa8216c7223f584b

ultra96+petalinux2019.2+ros2 dashingでpwm_controlをしてみた

これの続き。

subscriberを改造し、uio経由でPWM制御します。uioはここを参考にしています。

ultra96v2_oobでは以下の通りの準備が必要です。

ros_controlの作法とかc++の作法は知らないのであしからず。

1.configでCONFIG_SUBSYSTEM_REMOVE_PL_DTBのオプションを無効にし、BOOTARGSでuioを有効にするオプションを追加する。

diff --git a/project-spec/configs/config b/project-spec/configs/config
index 8663a10..806558c 100644
--- a/project-spec/configs/config
+++ b/project-spec/configs/config
@@ -154,7 +154,7 @@ CONFIG_SUBSYSTEM_BOOTARGS_EARLYPRINTK=y

 CONFIG_SUBSYSTEM_DEVICETREE_FLAGS=""
 # CONFIG_SUBSYSTEM_DTB_OVERLAY is not set
-CONFIG_SUBSYSTEM_REMOVE_PL_DTB=y
+# CONFIG_SUBSYSTEM_REMOVE_PL_DTB is not set

 #
 # ARM Trusted Firmware Compilation Configuration
@@ -241,4 +241,4 @@ CONFIG_YOCTO_ENABLE_DEBUG_TWEAKS=y
 # User Layers
 #
 CONFIG_USER_LAYER_0=""
-CONFIG_SUBSYSTEM_BOOTARGS_GENERATED="earlycon console=ttyPS0,115200 clk_ignore_unused root=/dev/mmcblk0p2 rw rootwait"
+CONFIG_SUBSYSTEM_BOOTARGS_GENERATED="earlycon console=ttyPS0,115200 clk_ignore_unused root=/dev/mmcblk0p2 rw rootwait uio_pdrv_genirq.of_id=generic-uio"
diff --git a/project-spec/meta-plnx-generated/recipes-bsp/device-tree/device-tree.bbappend b/project-spec/meta-plnx-generated/recipes-bsp/device-tree/device-tree.bbappend
index 2bce90c..b5f8609 100644
--- a/project-spec/meta-plnx-generated/recipes-bsp/device-tree/device-tree.bbappend
+++ b/project-spec/meta-plnx-generated/recipes-bsp/device-tree/device-tree.bbappend
@@ -3,7 +3,6 @@ YAML_MAIN_MEMORY_CONFIG_forcevariable = "PSU_DDR_0"
 XSCTH_WS = "${TOPDIR}/../components/plnx_workspace/device-tree"
 FILESEXTRAPATHS_append := ":${sysconf}"
 YAML_CONSOLE_DEVICE_CONFIG_forcevariable = "psu_uart_1"
-YAML_REMOVE_PL_DT = "1"
 export PETALINUX
 SRC_URI_append ="\
     file://config\

device-treeに自動生成されたPL設定を反映するためにCONFIG_SUBSYSTEM_REMOVE_PL_DTBを無効にし、uio使用するためのパラメータを追加しました。

2.設定が衝突するdevice-treeの記述の修正とPWM_w_Int_0をuioで使用する設定を行う

diff --git a/project-spec/meta-user/recipes-bsp/device-tree/files/system-user.dtsi b/project-spec/meta-user/recipes-bsp/device-tree/files/system-user.dtsi
index d7be975..cb4ce5e 100755
--- a/project-spec/meta-user/recipes-bsp/device-tree/files/system-user.dtsi
+++ b/project-spec/meta-user/recipes-bsp/device-tree/files/system-user.dtsi
@@ -194,18 +194,22 @@
                 reg = <0>;
         };
 };
-&amba {
-       axi_intc_0: axi-interrupt-ctrl {
-               #interrupt-cells = <2>;
-               compatible = "xlnx,xps-intc-1.00.a";
-               interrupt-controller;
-               reg = <0x0 0x80020000 0x0 0x1000>;
-               xlnx,kind-of-intr = <0x0>;
-               xlnx,num-intr-inputs = <0x20>;
-               interrupt-parent = <&gic>;
-               interrupts = <0 89 4>;
-       };

+&axi_intc_0 {
+       #interrupt-cells = <2>;
+       compatible = "xlnx,xps-intc-1.00.a";
+       interrupt-controller;
+       reg = <0x0 0x80020000 0x0 0x1000>;
+       xlnx,kind-of-intr = <0x0>;
+       xlnx,num-intr-inputs = <0x20>;
+       interrupt-parent = <&gic>;
+       interrupts = <0 89 4>;
+};
+&PWM_w_Int_0 {
+       compatible = "generic-uio";
+};
+
+&amba {
        zyxclmm_drm {
                compatible = "xlnx,zocl";
                status = "okay";
                                

axi_intc_0の設定が衝突するので、system-user.dtsiのaxi_intc_0を参照して設定を上書きするように変更ました。(もしかしたらaxi_intc_0の設定は消すだけでもいいかもしれないけど調べていない)

ultra96v2_oobではPWM用のIPがPWM_w_Int_0として用意してあり、0x80070000にアサインされています。

この信号の出力はLow Speed Expansion Connectorの29ピンに接続されているので、テスターで設定に従って動作しているか調べることが可能です。

3.ros2 dashingからアクセスできるようにpwm_controlノードを準備する。

rclcppとstd_msgsに依存したament_cmakeでbuildするpwm_controlパッケージを作成します。pwm_controlのソースは以下の通りとしました。ホストのcolconでbuild可能なことを確認し、git commit=>hashを指定しての.bbファイルを作成して、petalinux-user-image.bbのインストール対象にpwm_controlを含めました。

//pwm_control.cpp
#include <cstdio>
#include <memory>
#include <fcntl.h>
#include <unistd.h>
#include <string>
#include <sys/mman.h>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/int32.hpp>

class PwmControl : public rclcpp::Node
{
public:
  explicit PwmControl(const std::string & topic_name)
  : Node("pwm_control")
  {
    /* メモリアクセス用デバイスファイルを開く */
    int fd;
    const char* file_name = "/dev/uio4";
    const int size = 0x1000;
    if ((fd = open(file_name, O_RDWR | O_SYNC)) < 0) {
      throw std::runtime_error("Failed to init Pwm_W_Int");
    }

    /* ARM(CPU)から見た物理アドレス → 仮想アドレスへのマッピング */
    uio_ptr = static_cast<int*>(mmap(NULL, size, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0));
    if (uio_ptr == MAP_FAILED) {
      close(fd);
      throw std::runtime_error("Failed to init Pwm_W_Int mmap");
    }
    
    // pwm_valueトピックのコールバック関数
    auto callback =
      [this](const std_msgs::msg::Int32::UniquePtr msg) -> void
      {
	*(volatile unsigned int*)(uio_ptr) = msg->data;
	RCLCPP_INFO(this->get_logger(), "pwm_value:%d", msg->data);
      };

    // pwm_valueトピックの受信設定
    rclcpp::QoS qos(rclcpp::KeepLast(10));
    sub_ = create_subscription<std_msgs::msg::Int32>(
      topic_name, qos, callback);
  }

  ~PwmControl(){
    delete(uio_ptr);
  }

private:
  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr sub_;
  int* uio_ptr;
};

int main(int argc, char * argv[])
{
  setvbuf(stdout, NULL, _IONBF, BUFSIZ);
  rclcpp::init(argc, argv);

  auto node = std::make_shared<PwmControl>("pwm_value");
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

4.bitbakeからの動作確認。

sdに書き込みultra96を起動後以下のコマンドを実行。

AvnetのPWM_w_Intは先頭レジスタでDuty比を指定する仕組みのようなので、topicでDuty比が指定できることを確認しました。

ちなみにDuty比の上限は99%(990000)となっているようです。

#source ros2_setup.sh
#ros2 run pwm_control pwm_control &
#ros2 topic pub /pwm_value std_msgs/Int32 "{data: 800000}"

テスターでLow Speed Expansion Connectorの29ピンをチェックし、指定した値に応じた電圧値が出ていることを確認しました。

ros2からは普通にuio経由でアクセスすればPL部の機能にアクセス可能ということがわかりました。

次はデバイスファイルとトピック名をパラメーター指定するのと、Lチカさせてみるのが目標かな。

参考ページ

https://qiita.com/iwatake2222/items/da91ce4dc2a8a8df3c0a

ultra96v2_oobにros2-dashingを導入する方法その１〜

meta-ros導入のメモやu-bootやqemu-devicetreeの設定変更など書き散らしたものを整理していきます。

環境作成するための方法をまとめて後で使用できるようにすることが目的です。
petalinuxとvitisのインストールは省略し、Vivadoプロジェクトを作成した後からの続きの手順を記載します。




petalinuxのプロジェクトを作成します。ただし、sstateはmeta-rosの導入時に問題が出たので設定しません。buildはまだです。

次にu-bootのconfig_sys_text_baseを修正します。
パッチファイル作成し、以下に保存します。

<petalinux_prj>/project-spec/meta-user/recipes-bsp/u-boot/files/config_sys_text_base_amend.patch

<petalinux_prj>/project-spec/meta-user/recipes-bsp/u-boot/u-boot-xlnx_%.bbappendを以下の通りにしてパッチを追加します。

FILESEXTRAPATHS_prepend := "${THISDIR}/files:"

SRC_URI += "file://platform-top.h"
SRC_URI += "file://bsp.cfg"
SRC_URI += "file://config_sys_text_base_amend.patch"

u-bootの次はqemu用のhw-dtbを修正するパッチを当てます。
結局petalinux-buildではrosを導入できなかったのでzcu102のqemu用dtbを使用すればいいような気もするけど、せっかくなので。
ファイルを置く場所は<petalinux_prj>/project-spec/meta-user/recipes-devtools/qemu/files/0001-enable-gems-for-zcu100-arm.dts.patchです。
後述するbitbakeを使用してbitbake -e qemu-devicetrees | grep ^FILEでレベルを合わせてmeta-userに配置することで適用できます。
パッチの内容は以下のようにgemのinvalidをコメントアウトしているだけです。
これでqemu動作させた際にgem3を使用して通信可能にし、rosの模擬動作やgazeboとの接続ができるようになるはずです。

From 4b5e8412beaff0a773ac4c960344e661e460ac65 Mon Sep 17 00:00:00 2001
From: Akira Nishiyama <xxxx@xxx.xxx>
Date: Wed, 29 Jan 2020 08:04:19 +0900
Subject: [PATCH] enable gems for zcu100-arm.dts

---
 zcu100-arm.dts | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/zcu100-arm.dts b/zcu100-arm.dts
index 4eee783..28351dc 100644
--- a/zcu100-arm.dts
+++ b/zcu100-arm.dts
@@ -40,6 +40,7 @@
 };

 /* Invalidate all GEMs as there is no Ethernet */
+/*
 &gem0 {
     compatible = "invalid";
 };
@@ -55,7 +56,7 @@
 &gem3 {
     compatible = "invalid";
 };
-
+*/
 &ps7_qspi_0 {
     qspi-flash@0 {
         compatible = "n25q128a13";
-- 
2.17.1

この後はpetalinuxツールではなくyoctoを使用していくための設定を行うのと、
meta-ros導入時に発生したtaskhash mismatchの問題に対応するためにソースからビルドしていくように設定します。
長くなるので続く。