ラベル foxy の投稿を表示しています。 すべての投稿を表示
ラベル foxy の投稿を表示しています。 すべての投稿を表示

2021年1月12日火曜日

ubuntu 20.04+ros2 foxyでorab_slam2_rosを動作させてみた

ubuntu 20.04とros2 foxyでカメラを動作させてorb_slam2_rosを試してみた際のメモ。 カメラで試したのはros2_v4l2_cameraとopencv_cam。 ほぼリンク先の通り実行しただけ。

opencvの準備

ubuntu 20.04標準のopencvではバージョン起因と思われるエラーが出たので、 v4.5.0をソースからビルドした。 特別問題なくビルドできた。

cd <path-to-opencv-download-dir>
unzip opencv-4.5.0.zip
cd opencv-4.5.0
mkdir build
cd build
cmake .. -GNinja -DCMAKE_INSTALL_PREFIX=<opencv-installation-path>/local
ninja -j4
ninja install

私の環境では既にopencv3.4.12が入っているので、v4.5.0が先に見つかるように環境変数を設定する。 colcon実行前に以下のコマンドが必要。

export PATH=<opencv-installation-path>/local/bin:$PATH
export LD_LIBRARY_PATH=<opencv-installation-path>/local/lib:$LD_LIBRARY_PATH
export PYTHONPATH=<opencv-installation-path>/local/lib/python3.8/dist-packages:$PYTHONPATH

ros2_v4l2_cameraの準備

こちらは記載の通りに実行して特に問題なかった。 ~/ros_work/srcにリポジトリをクローンする。

使用したhashは以下の通り

ros2_v4l2_camera:65b072565f8e4520d166b725d91d5f31acd97610
image_common:4c5053ecf703a58c25a712d1d4a80012f5076a1f
image_transport_plugins:0565f23dc955fa2d56cffcfebc63de5a97cd005f
vision_opencv:2026f6d66833e0830f9ce69004d43c9abd1e5fa4

opencv_camの準備

ここを参考にしてビルドした。 image_pipelineのビルドはros2ブランチのhash:b4fbbae6463fb3baaa4b36a0a7517d0b0395e837を使用する。 foxyっぽいブランチを使用すると2021/1/12時点ではcalibrationを実行する際にros-argsを指定すると実行できない問題がある模様。 (https://github.com/ros-perception/image_pipeline/pull/597) image_commonはros2_v4ls_cameraと同じ。 ~/ros_work/srcにリポジトリをクローンする。

使用したhashは以下の通り。

opencv_cam:f4487b0de651a4323a535f8e8d0e86b147cc81c2
ros2_shared:02433ef4f873876c3dd3ab2925987cf04d224660
image_common:4c5053ecf703a58c25a712d1d4a80012f5076a1f
image_pipeline:b4fbbae6463fb3baaa4b36a0a7517d0b0395e837

ビルド

パスの設定をしてビルドする。

cd ~/ros_work
export PATH=<opencv-installation-path>/local/bin:$PATH
export LD_LIBRARY_PATH=<opencv-installation-path>/local/lib:$LD_LIBRARY_PATH
export PYTHONPATH=<opencv-installation-path>/local/lib/python3.8/dist-packages:$PYTHONPATH
colcon build --symlink-install

videoの出力

cam2imageを試す場合は以下。 /dev/video0のみが出力できる。

source ~/ros_work/install/setup.sh
ros2 run image_tools cam2image

v4l2_cameraを試す場合は以下。 video_deviceのパラメータで指定可能。 videoデバイスが複数合ってどれを使えばいいかわからない場合は、 videoデバイスの情報の調べ方を使って調べられる。 USBなら挿して増えたものでいいとは思う。 eloquent以前のビルドはできそうになかったので、 ultra96v2への採用は見送った。

source ~/ros_work/install/setup.sh
ros2 run v4l2_camera v4l2_camera_node --ros-args -p video_device:=/dev/video14

opencv_camを試す場合は以下。 indexでvideoデバイスファイルの番号を指定できる。

source ~/ros_work/install/setup.sh
ros2 run opencv_cam opencv_cam_main --ros-args --param index:=14

videoデバイスの情報の調べ方

v4l2-ctlを使用する。 ない場合にはv4l-utilsをインストールすると使用できる。

sudo apt install v4l-utils

logicoolのc270nに試してみた結果は以下の通り。

# v4l2-ctl -d /dev/video14 --all
driver Info:
    Driver name      : uvcvideo
    Card type        : UVC Camera (046d:0825)
    Bus info         : usb-0000:00:14.0-1.1
    Driver version   : 5.8.18
    Capabilities     : 0x84a00001
        Video Capture
        Metadata Capture
        Streaming
        Extended Pix Format
        Device Capabilities
    Device Caps      : 0x04200001
        Video Capture
        Streaming
        Extended Pix Format
Media Driver Info:
    Driver name      : uvcvideo
    Model            : UVC Camera (046d:0825)
    Serial           : 98251310
    Bus info         : usb-0000:00:14.0-1.1
    Media version    : 5.8.18
    Hardware revision: 0x00000012 (18)
    Driver version   : 5.8.18
Interface Info:
    ID               : 0x03000002
    Type             : V4L Video
Entity Info:
    ID               : 0x00000001 (1)
    Name             : UVC Camera (046d:0825)
    Function         : V4L2 I/O
    Flags         : default
    Pad 0x01000007   : 0: Sink
      Link 0x02000019: from remote pad 0x100000a of entity 'Extension 4': Data, Enabled, Immutable
Priority: 2
Video input : 0 (Camera 1: ok)
Format Video Capture:
    Width/Height      : 640/480
    Pixel Format      : 'YUYV' (YUYV 4:2:2)
    Field             : None
    Bytes per Line    : 1280
    Size Image        : 614400
    Colorspace        : sRGB
    Transfer Function : Default (maps to sRGB)
    YCbCr/HSV Encoding: Default (maps to ITU-R 601)
    Quantization      : Default (maps to Limited Range)
    Flags             : 
Crop Capability Video Capture:
    Bounds      : Left 0, Top 0, Width 640, Height 480
    Default     : Left 0, Top 0, Width 640, Height 480
    Pixel Aspect: 1/1
Selection Video Capture: crop_default, Left 0, Top 0, Width 640, Height 480, Flags: 
Selection Video Capture: crop_bounds, Left 0, Top 0, Width 640, Height 480, Flags: 
Streaming Parameters Video Capture:
    Capabilities     : timeperframe
    Frames per second: 30.000 (30/1)
    Read buffers     : 0
                     brightness 0x00980900 (int)    : min=0 max=255 step=1 default=128 value=128
                       contrast 0x00980901 (int)    : min=0 max=255 step=1 default=32 value=32
                     saturation 0x00980902 (int)    : min=0 max=255 step=1 default=32 value=32
 white_balance_temperature_auto 0x0098090c (bool)   : default=1 value=1
                           gain 0x00980913 (int)    : min=0 max=255 step=1 default=64 value=64
           power_line_frequency 0x00980918 (menu)   : min=0 max=2 default=2 value=2
                0: Disabled
                1: 50 Hz
                2: 60 Hz
      white_balance_temperature 0x0098091a (int)    : min=0 max=10000 step=10 default=4000 value=4000 flags=inactive
                      sharpness 0x0098091b (int)    : min=0 max=255 step=1 default=24 value=24
         backlight_compensation 0x0098091c (int)    : min=0 max=1 step=1 default=0 value=0
                  exposure_auto 0x009a0901 (menu)   : min=0 max=3 default=3 value=3
                1: Manual Mode
                3: Aperture Priority Mode
              exposure_absolute 0x009a0902 (int)    : min=1 max=10000 step=1 default=166 value=166 flags=inactive
         exposure_auto_priority 0x009a0903 (bool)   : default=0 value=1

videoの確認

上記でvideoの出力したものはrqt_image_viewで確認できる。 rviz2だとカメラ画像はうまく表示されなかった。(camera_infoの出力が必要とのこと)

ros2 run rqt_image_view rqt_image_view

 

calibration

カメラのキャリブレーションしないとopencv_camを実行した際にcamera_infoのtopicを出力できない旨のエラーメッセージが出力される。

[ERROR] [1610449586.191007718] [camera_calibration_parsers]: Failed to detect content in .ini file
[ERROR] [1610449586.191095947] [opencv_cam]: cannot get camera info, will not publish

キャリブレーション方法はこちらに記載の内容に従って行った。 途中でフリーズしてCalibrateボタンが押せないことが何度かあったが、頑張ってプログラムを完了させた。

#ros2 run camera_calibration cameracalibrator \
  --size=8x6 \
  --square=0.063 \
  --approximate=0.3 \
  --no-service-check \
  --ros-args --remap /image:=/image_raw
*** Added sample 1, p_x = 0.479, p_y = 0.527, p_size = 0.602, skew = 0.051
*** Added sample 2, p_x = 0.790, p_y = 0.645, p_size = 0.634, skew = 0.083
*** Added sample 3, p_x = 0.758, p_y = 0.556, p_size = 0.634, skew = 0.164
*** Added sample 4, p_x = 0.599, p_y = 0.417, p_size = 0.627, skew = 0.155
*** Added sample 5, p_x = 0.529, p_y = 0.281, p_size = 0.622, skew = 0.103
*** Added sample 6, p_x = 0.435, p_y = 0.000, p_size = 0.627, skew = 0.128
*** Added sample 7, p_x = 0.463, p_y = 0.776, p_size = 0.621, skew = 0.057
*** Added sample 8, p_x = 0.630, p_y = 0.905, p_size = 0.627, skew = 0.056
*** Added sample 9, p_x = 0.465, p_y = 0.997, p_size = 0.629, skew = 0.101
*** Added sample 10, p_x = 0.787, p_y = 0.888, p_size = 0.667, skew = 0.104
*** Added sample 11, p_x = 0.336, p_y = 0.914, p_size = 0.647, skew = 0.025
*** Added sample 12, p_x = 0.355, p_y = 0.735, p_size = 0.666, skew = 0.005
*** Added sample 13, p_x = 0.460, p_y = 0.585, p_size = 0.688, skew = 0.012
*** Added sample 14, p_x = 0.255, p_y = 0.577, p_size = 0.683, skew = 0.032
*** Added sample 15, p_x = 0.371, p_y = 0.504, p_size = 0.706, skew = 0.054
*** Added sample 16, p_x = 0.640, p_y = 0.781, p_size = 0.737, skew = 0.073
*** Added sample 17, p_x = 0.402, p_y = 0.322, p_size = 0.719, skew = 0.036
*** Added sample 18, p_x = 0.534, p_y = 0.101, p_size = 0.747, skew = 0.069
*** Added sample 19, p_x = 0.689, p_y = 0.225, p_size = 0.764, skew = 0.129
*** Added sample 20, p_x = 0.632, p_y = 0.539, p_size = 0.733, skew = 0.140
*** Added sample 21, p_x = 0.527, p_y = 0.628, p_size = 0.711, skew = 0.133
*** Added sample 22, p_x = 0.241, p_y = 0.822, p_size = 0.620, skew = 0.040
*** Added sample 23, p_x = 0.524, p_y = 0.801, p_size = 0.745, skew = 0.015
*** Added sample 24, p_x = 0.603, p_y = 0.734, p_size = 0.669, skew = 0.157
*** Added sample 25, p_x = 0.404, p_y = 0.131, p_size = 0.690, skew = 0.103
*** Added sample 26, p_x = 0.301, p_y = 0.048, p_size = 0.690, skew = 0.082
*** Added sample 27, p_x = 0.743, p_y = 0.106, p_size = 0.692, skew = 0.131
*** Added sample 28, p_x = 0.788, p_y = 0.305, p_size = 0.689, skew = 0.128
*** Added sample 29, p_x = 0.423, p_y = 0.621, p_size = 0.631, skew = 0.084
*** Added sample 30, p_x = 0.638, p_y = 0.191, p_size = 0.651, skew = 0.117
*** Added sample 31, p_x = 0.410, p_y = 0.404, p_size = 0.629, skew = 0.059
*** Added sample 32, p_x = 0.700, p_y = 0.411, p_size = 0.678, skew = 0.244
*** Added sample 33, p_x = 0.577, p_y = 0.321, p_size = 0.674, skew = 0.193
*** Added sample 34, p_x = 0.416, p_y = 0.468, p_size = 0.664, skew = 0.171
*** Added sample 35, p_x = 0.348, p_y = 0.709, p_size = 0.778, skew = 0.065
*** Added sample 36, p_x = 0.546, p_y = 0.666, p_size = 0.784, skew = 0.035
*** Added sample 37, p_x = 0.545, p_y = 0.369, p_size = 0.769, skew = 0.006
*** Added sample 38, p_x = 0.640, p_y = 0.493, p_size = 0.598, skew = 0.063
*** Added sample 39, p_x = 0.814, p_y = 0.795, p_size = 0.610, skew = 0.071
*** Added sample 40, p_x = 0.730, p_y = 0.839, p_size = 0.600, skew = 0.166
*** Added sample 41, p_x = 0.587, p_y = 0.679, p_size = 0.570, skew = 0.052
D =  [0.45527714279447146, 0.29663576559913546, -0.023494638427063057, -0.03623143280219075, 0.0]
K =  [1594.715058900246, 0.0, 214.64225194625027, 0.0, 1535.171197718767, 174.6268394034711, 0.0, 0.0, 1.0]
R =  [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P =  [1628.6416015625, 0.0, 210.49766367948905, 0.0, 0.0, 1574.3720703125, 171.79825469629577, 0.0, 0.0, 0.0, 1.0, 0.0]
None
# oST version 5.0 parameters


[image]

width
640

height
480

[narrow_stereo]

camera matrix
1594.715059 0.000000 214.642252
0.000000 1535.171198 174.626839
0.000000 0.000000 1.000000

distortion
0.455277 0.296636 -0.023495 -0.036231 0.000000

rectification
1.000000 0.000000 0.000000
0.000000 1.000000 0.000000
0.000000 0.000000 1.000000

projection
1628.641602 0.000000 210.497664 0.000000
0.000000 1574.372070 171.798255 0.000000
0.000000 0.000000 1.000000 0.000000


D =  [0.45527714279447146, 0.29663576559913546, -0.023494638427063057, -0.03623143280219075, 0.0]
K =  [1594.715058900246, 0.0, 214.64225194625027, 0.0, 1535.171197718767, 174.6268394034711, 0.0, 0.0, 1.0]
R =  [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P =  [1628.6416015625, 0.0, 210.49766367948905, 0.0, 0.0, 1574.3720703125, 171.79825469629577, 0.0, 0.0, 0.0, 1.0, 0.0]
None
# oST version 5.0 parameters


[image]

width
640

height
480

[narrow_stereo]

camera matrix
1594.715059 0.000000 214.642252
0.000000 1535.171198 174.626839
0.000000 0.000000 1.000000

distortion
0.455277 0.296636 -0.023495 -0.036231 0.000000

rectification
1.000000 0.000000 0.000000
0.000000 1.000000 0.000000
0.000000 0.000000 1.000000

projection
1628.641602 0.000000 210.497664 0.000000
0.000000 1574.372070 171.798255 0.000000
0.000000 0.000000 1.000000 0.000000


('Wrote calibration data to', '/tmp/calibrationdata.tar.gz')
D =  [0.45527714279447146, 0.29663576559913546, -0.023494638427063057, -0.03623143280219075, 0.0]
K =  [1594.715058900246, 0.0, 214.64225194625027, 0.0, 1535.171197718767, 174.6268394034711, 0.0, 0.0, 1.0]
R =  [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P =  [1628.6416015625, 0.0, 210.49766367948905, 0.0, 0.0, 1574.3720703125, 171.79825469629577, 0.0, 0.0, 0.0, 1.0, 0.0]
# oST version 5.0 parameters


[image]

width
640

height
480

[narrow_stereo]

camera matrix
1594.715059 0.000000 214.642252
0.000000 1535.171198 174.626839
0.000000 0.000000 1.000000

distortion
0.455277 0.296636 -0.023495 -0.036231 0.000000

rectification
1.000000 0.000000 0.000000
0.000000 1.000000 0.000000
0.000000 0.000000 1.000000

projection
1628.641602 0.000000 210.497664 0.000000
0.000000 1574.372070 171.798255 0.000000
0.000000 0.000000 1.000000 0.000000

/tmp/calibrationdata.tar.gzに保存されたものを使用する。 解凍すると、ost.txtがあり、これがiniファイルとして使用できる。(コンソールの結果も同じ) なので、camera-info-logicool-c270n.iniとして保存した。 使うときには以下の通り。 ただし、rviz2では表示されなかった。あと何がいるんだ？

ros2 run opencv_cam opencv_cam_main --ros-args --param index:=14 --param camera_info_path:=camera-info-logicool-c270n.ini

orb_slam2実行

以下のコマンドでorb_slam2_rosを実行する。

ros2 run orb_slam2_ros orb_slam2_ros_mono --ros-args --remap /camera/image_raw:=/image_raw --remap /camera/camera_info:=/camera_info --param params_file:=install/orb_slam2_ros/share/orb_slam2_ros/ros/config/params_d435_mono.yaml --param voc_file:=install/orb_slam2_ros/share/orb_slam2_ros/orb_slam2/Vocabulary/ORBvoc.txt

ultra96v2で実行する場合は以下。機能の実装方法は次回。

ros2 run orb_slam2_ros orb_slam2_ros_mono --ros-args --remap /camera/image_raw:=/image_raw --remap /camera/camera_info:=/camera_info --param params_file:=/usr/share/orb_slam2_ros/ros/config/params_d435_mono.yaml --param voc_file:=/usr/share/orb_slam2_ros/orb_slam2/Vocabulary/ORBvoc.txt

以下のような結果が得られた。 なんだか微妙な感じだけどキャリブレーションうまくいってないのか？


 

参考

http://joe.ash.jp/program/ros/tutorial/tutorial_camera.htm
https://medium.com/swlh/raspberry-pi-ros-2-camera-eef8f8b94304
https://jeffzzq.medium.com/ros2-image-pipeline-tutorial-3b18903e7329

投稿者 時刻: 0 件のコメント:
ラベル: , , ,

2020年12月26日土曜日

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

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


 

投稿者 時刻: 0 件のコメント:
ラベル: , ,
登録: 投稿 (Atom)