ros2 eloquent+Ubuntu18.04でorb_slam_2_rosを動作させてみた

dashingだとros2対応版のORB_SLAMがうまくビルドできなかったので、eloquentに変更して動作させてみた。とりあえず動かしてみた結果のメモ。

ros2 eloquentのソースフェッチ

ここに書いてあるとおりにすればOK。 だけどgeometoryを変更したほうが良いのでInstall dependencies using rosdepまでで止めておく。 ros1_bridgeのビルド方法は後で確認する。前回の環境使用すれば動作するので。

ros2 eloquentのgeometoryを変更

0.12.0でorb_slam_2_rosのビルドに成功したので、変更する。

cd ~/ros2_eloquent/src/ros2/geometory
git checkout 0.12.0

ros2 eloquent+のビルド

cd ~/ros2_eloquent
colcon build --symlink-install --packages-skip ros1_bridge

image_commonとcv_bridgeのビルド

image_commonはちょうど良いタグが見つからなかったのでログかビルド成功しそうなものを選択した。 vision_opencvはとりあえずros2ブランチを選択したところうまく行った。 hashは2026f6d66833e0830f9ce69004d43c9abd1e5fa4。

source ~/ros2_eloquent/local_setup.sh
mkdir -p ~/ros2_ws
cd ~/ros2_ws
git clone https://github.com/ros-perception/image_common.git
cd image_common
git checkout e7f682e68016ea855bf8ef7d0d752c94a4053a61
cd ..
git clone https://github.com/ros-perception/vision_opencv.git -b ros2
colcon build --symlink-install

orb_slam_2_rosのビルド

image_commonとcv_bridgeと同じワークスペースに突っ込んで同時にビルドしてみたけど、 find_packageがどうも失敗する模様。記述上はいいように見えるんだけどなぁ。 colconの動作をよく分かっていないので何故失敗するかは不明だけど、 順番にビルドすればとりあえずは動くようになる。 hashは0f964c618a2e0b7715c1c6fc49e6b70b2ba2c904を使用。

git clone https://github.com/appliedAI-Initiative/orb_slam_2_ros.git -b ros2

動かすだけなので以下のorb_slam_2_ros/ros/launchにTUM1_mono_launch.pyを置く。 ちゃんとやるなら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
        )
    ])

ビルドする。

source install/setup.sh
colcon build --symlink-install

orb_slam_2_rosのテスト

TUM1を流して試してみる。

ターミナル１で以下を実行

cd ~/ros2_ws
source install/setup.sh
ros2 launch orb_slam2_ros TUM1_mono_launch.py

ターミナル2で以下を実行

cd ~/ros2_ws
source install/setup.sh
rviz2

左下のAddボタンからorb_slam2_monoのpointcloud2とimageトピックを追加しておく。
TFも追加しておくとmapとcamera_linkの動く様子が見られる。

ターミナル3で以下を実行。前回と同じ。

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

実行結果

カメラのパラメータが間違ってからか微妙な感じだがキーボードや机の上の障害物等の形は出ている。