Ubuntu 20.04 + ros2 foxyでORB_SLAM2を使ってみた

Ubuntu 20.04 + ros2 foxyでORB_SLAM2を使ってみた話。

環境

Ubuntu 20.04
ros2 foxy ros noetic(rosbag2によるデータセット再生用)

ros2はソースビルドをした場合にrosbag2_bag_v2をビルドするのがうまくできなかったので、 パッケージ版を使用する。 今回はros1で保存されているbagファイルを再生させるためにrosbag2_bag_v2プラグインを使用したいので、 noeticの環境をセットアップした後にfoxyとrosbag2の環境を準備した。

まずはnoeticのセットアップを行う。 公式ページにしたがって環境セットアップだけする。 必要なパッケージは後でインストールされるのでここでは不要。

sudo apt update && sudo apt install curl gnupg2 lsb-release
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654

次はros2側のセットアップを行う。foxyを使用し、rosbag2とbag_v2プラグインをセットアップする。 インストールするのはros-foxy-ros2bagとros-foxy-rosbag2*なので注意。 公式ページにしたがって以下のように準備。

curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
sudo sh -c 'echo "deb [arch=$(dpkg --print-architecture)] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" > /etc/apt/sources.list.d/ros2-latest.list'
sudo apt update
sudo apt install ros-foxy-base
sudo apt update && apt-get install -y ros-foxy-ros2bag ros-foxy-rosbag2*

ORB_SLAM2のビルド

必要なソースを取得してきてビルドを行う。

cd <work-dir>
mkdir src
cd src
git clone https://github.com/ros-perception/vision_opencv.git -b ros2
git clone https://github.com/ros-perception/image_common.git -b ros2
git clone https://github.com/appliedAI-Initiative/orb_slam_2_ros.git -b ros2

今回試した際のHash値は次の通りだった。 image_common:4c5053ecf703a58c25a712d1d4a80012f5076a1f
vision_opencv:2026f6d66833e0830f9ce69004d43c9abd1e5fa4 orb_slam_2_ros:0f964c618a2e0b7715c1c6fc49e6b70b2ba2c904

orb_slam_2_rosのLaunchファイルを作成する。 なぜかは分からないけど、コマンドラインからのRemapオプションが受け付けられないので。

今回は動かすだけなので、params_fileはデフォルトのparams_d435_mono.yamlを使用する。 オリジナルのTUM1.yamlを使用しても良い。

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():

    use_sim_time = LaunchConfiguration('use_sim_time')
    params_file = LaunchConfiguration('params_file')
    voc_file = LaunchConfiguration('voc_file')

    remappings = [
        ('/camera/image_raw', '/camera/rgb/image_color'),
        ('/camera/camera_info', '/camera/rgb/camera_info'),
    ]

    return LaunchDescription([
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='false',
            description='Use simulation (Gazebo) clock if true'),

        DeclareLaunchArgument(
            'params_file',
            default_value=os.path.join(
                get_package_share_directory("orb_slam2_ros"),
                'ros', 'config', 'params_d435_mono.yaml'),
            description='Full path to the ROS2 parameters file to use for all launched nodes'),

        DeclareLaunchArgument(
            'voc_file',
            default_value=os.path.join(
                get_package_share_directory("orb_slam2_ros"),
                'orb_slam2', 'Vocabulary', 'ORBvoc.txt'),
            description='Full path to vocabulary file to use'),

        Node(
            parameters=[
              params_file,
              {"voc_file": voc_file,
               "use_sim_time": use_sim_time},
            ],
            package='orb_slam2_ros',
            node_executable='orb_slam2_ros_mono',
            node_name='orb_slam2_mono',
            output='screen',
            remappings=remappings
        )
    ])

あとは依存関係正しく無いと思われるので、orb_slam_2_rosのpackage.xmlを修正する。
image_commonを足しておく。

cd <work-dir>
source /opt/ros/foxy/setup.sh
colcon build --symlink-install

ORB_SLAM2の動作確認

• window1

source install/setup.sh
ros2 launch orb_slam2_ros TUM1_mono_launch.py

• window2

source /opt/ros/foxy/setup.sh
rviz2

debug_imageと、point_cloudをAddする。

bagファイルをここから取得する。 テストにはrgbd_dataset_freiburg1_xyzを使用した。

• window3

source /opt/ros/noetic/setup.sh
source /opt/ros/foxy/setup.sh
ros2 bag play -s rosbag_v2 rgbd_dataset_freiburg1_xyz.bag --read-ahead-queue-size 1000000

以下の通りの結果が得られた。