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

2021年1月19日火曜日

ros2 eloquent + orb_slam2_rosをクロスコンパイルして動かした話

ultra96v2とros2 eloquentでorb_slam2を動かした際のメモ。 能力不足なのか、マッピングが動かなかった。

vitisで性能改善させるのにsdkでのクロスコンパイル環境必要かと思ったので整えた。

レシピはここのfeature-v2020.2ブランチから一式取得できる。 もしくはここのfeature-v2020.2ブランチにレシピ類がまとめてある。

レシピの変更

クロスコンパイル用のパッケージにはnativesdkをBBCLASSEXTENDに追加する必要がある。 そのため、/sources/meta-ros/meta-ros-common/classes/ros_superflore_generated.bbclassを変更する。

# BBCLASSEXTEND_append = "${@bb.utils.contains('ROS_SUPERFLORE_GENERATED_BUILDTOOLS', '${BPN}-native', ' native', '', d)}"
BBCLASSEXTEND_append = "${@bb.utils.contains('ROS_SUPERFLORE_GENERATED_BUILDTOOLS', '${BPN}-native', ' native nativesdk', '', d)}"

次のファイルはFILES_${PN} += "*"を追加する。 そのままだとament-cmake向けファイル等がインストールされない模様。
recipes-ros2/ament-index-python/ament-index-python_%.bbappend
recipes-ros2/ament-package/ament-package_%.bbappend
recipes-ros2/domain-coordinator/domain-coordinator_%.bbappend

python3-lark-parserはBsuperfloreのBUILDTOOL登録されていないため、nativesdk化されない。 なのでbbappendでBBCLASSEXTEND_appendにnativesdkを追加する。 recipes-ros2/python3-lark-parser/python3-lark-parser_%.bbappend

rviz-ogre-vendorがmesaに依存しているけど、 zynqではmesa-glを使用するので、mesaを削除してmesa-glを追加する。
recipes-ros2/rviz/rviz-ogre-vendor_%.bbappend

ROS_BUILD_DEPENDS_remove = "mesa"
ROS_BUILD_DEPENDS_append = " mesa-gl"
ROS_EXPORT_DEPENDS_remove = "mesa"
ROS_EXPORT_DEPENDS_append = " mesa-gl"
ROS_EXEC_DEPENDS_remove = "mesa"
ROS_EXEC_DEPENDS_append = " mesa-gl"

あとはpackage.xmlを参考にopencv-camとros2-sharedのレシピを作成する。
recipes-ros2/opencv-cam/opencv-cam_git.bb
recipes-ros2/ros2-shared/ros2-shared_git.bb

あとはSDKのためにTOOLCHAIN_TARGET_TASK_appendとTOOLCHAIN_HOST_TASK_appendを recipes-core/images/petalinux-image-minimal-ultra96v2ros2.bbに追加していく。
yoctoは自動的に依存関係収集してくれないようなので、クロスコンパイルしたい対象の依存関係を手動で解決していく。
*-nativeに依存している場合はnativesdk-*をTOOLCHAIN_HOST_TASK_appendに追加する。
無印の場合にはTOOLCHAIN_TARGET_TASK_appendに該当パッケージを追加する。
クロスコンパイル実行時にライブラリが不足などのメッセージが出た場合には-staticdevや-devなどをTOOLCHAIN_TARGET_TASK_appendに追加する。
(で合ってるはず。自信は無い。)

ビルド

MACHINE=ultra96v2-zynqmp bitbake petalinux-image-minimal-ultra96v2ros2
MACHINE=ultra96v2-zynqmp bitbake petalinux-image-minimal-ultra96v2ros2 -c do_populate_sdk

orb-slam2-rosの実行

まずはカメラ画像の確認。 USBのHOSTにwebカメラを接続する。 使用するのはlogicoolのc270n。 ついでにCPU使用率を確認する。

ssh 192.168.2.1 -l root
root@ultra96v2-zynqmp:~# source ros_setup.sh
root@ultra96v2-zynqmp:~# ros2 run opencv_cam opencv_cam_main --ros-args --param index:=0 --param camera_info_path:=camera-info-logicool-c270n.ini &
root@ultra96v2-zynqmp:~# top

topで動作を確認。

top - 23:11:54 up 11:33,  2 users,  load average: 0.86, 1.13, 1.63
Tasks: 103 total,   3 running, 100 sleeping,   0 stopped,   0 zombie
%Cpu(s):  0.2 us,  2.3 sy,  0.0 ni, 97.5 id,  0.0 wa,  0.0 hi,  0.0 si,  0.0 st
MiB Mem :   1974.4 total,   1682.3 free,    124.8 used,    167.3 buff/cache
MiB Swap:      0.0 total,      0.0 free,      0.0 used.   1780.4 avail Mem

大したことはなさそう。

orb-slam2-rosを動かす。

root@ultra96v2-zynqmp:~# 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 &

同じくtopで確認。1オプションでCPU毎の負荷も確認しておく。

top - 23:50:13 up 12:11,  2 users,  load average: 3.71, 3.16, 2.74
Tasks: 108 total,   2 running, 106 sleeping,   0 stopped,   0 zombie
%Cpu0  : 85.9 us,  2.0 sy,  0.0 ni, 11.8 id,  0.0 wa,  0.0 hi,  0.3 si,  0.0 st
%Cpu1  : 21.5 us,  5.0 sy,  0.0 ni, 73.2 id,  0.0 wa,  0.0 hi,  0.3 si,  0.0 st
%Cpu2  : 48.1 us,  6.5 sy,  0.0 ni, 45.4 id,  0.0 wa,  0.0 hi,  0.0 si,  0.0 st
%Cpu3  :  3.2 us, 26.4 sy,  0.0 ni, 70.4 id,  0.0 wa,  0.0 hi,  0.0 si,  0.0 st
MiB Mem :   1974.4 total,   1089.0 free,    665.8 used,    219.6 buff/cache
MiB Swap:      0.0 total,      0.0 free,      0.0 used.   1239.2 avail Mem 

  PID USER      PR  NI    VIRT    RES    SHR S  %CPU  %MEM     TIME+ COMMAND                                                                                 
 1628 root      20   0 1790424 505660  21916 R 121.2  25.0  26:20.67 orb_slam2_ros_m                                                                         
 1277 root      20   0  516600  47812  23240 S  47.4   2.4  12:44.68 opencv_cam_main                                                                         
 4034 root       0 -20       0      0      0 I  31.8   0.0   3:38.79 kworker/u9:2-uvcvideo                                                                   
 1258 root      20   0  415052  37020  15196 S   1.0   1.8   0:24.32 python3                                                                                 
 6384 root      20   0    3320   2024   1564 R   0.7   0.1   0:02.39 top

CPUごとに見ると１つのCPUの負荷が高く、orb_slam2_rosプロセスの負荷が高い。 load averageも高めで能力足りてないと見ればいいかな。

クロスコンパイル

sdkをインストールしたあと

/sysroots/x86_64-petalinux-linux/usr/share/cmake/OEToolchainConfig.cmake を次のように編集する。 $ENV{OECORE_NATIVE_SYSROOT}が無いとnativeツールが発見できないため

set( CMAKE_FIND_ROOT_PATH "\$ENV{OECORE_TARGET_SYSROOT};\$ENV{OECORE_NATIVE_SYSROOT}" )

sysroots/aarch64-xilinx-linux/usr/share/fastrtps/cmake/fastrtps-targets.cmakeの検索パスが間違っているので修正。 レシピから修正したほうがいいだろうけど、とりあえずなんとかなるから後回し。

#set_target_properties(fastrtps PROPERTIES
#  INTERFACE_INCLUDE_DIRECTORIES "${_IMPORT_PREFIX}/include"
#  INTERFACE_LINK_LIBRARIES "fastcdr;foonathan_memory;-lpthread;dl;/home/akira/work/ultra96v2/petalinux/build/tmp/work/aarch64-xilinx-linux/fastrtps/1.9.3-2-r0/recipe-sysroot/usr/lib/libtinyxml2.so;\$<\$<BOOL:1>:OpenSSL::SSL\$<SEMICOLON>OpenSSL::Crypto>;\$<\$<BOOL:>:iphlpapi\$<SEMICOLON>Shlwapi>"
#)
set_target_properties(fastrtps PROPERTIES
  INTERFACE_INCLUDE_DIRECTORIES "${_IMPORT_PREFIX}/include"
  INTERFACE_LINK_LIBRARIES "fastcdr;foonathan_memory;-lpthread;dl;$ENV{OECORE_TARGET_SYSROOT}/usr/lib/libtinyxml2.so;\$<\$<BOOL:1>:OpenSSL::SSL\$<SEMICOLON>OpenSSL::Crypto>;\$<\$<BOOL:>:iphlpapi\$<SEMICOLON>Shlwapi>"
)

buildの実行。インストールパスはsd-cardのrootをマウントした場所を指定する。

source <sdk-installation-path>/environment-setup-aarch64-xilinx-linux
export AMENT_PREFIX_PATH=$OECORE_NATIVE_SYSROOT/usr:$OECORE_TARGET_SYSROOT/usr
export PYTHONPATH=$OECORE_NATIVE_SYSROOT/usr/lib/python3.7/site-packages:$OECORE_TARGET_SYSROOT/usr/lib/python3.7/site-packages
cd <path-to-orb_slam2_ros-repository>
mkdir build
cd build
cmake .. -DCMAKE_INSTALL_SO_NO_EXE=0 -DBUILD_TESTING=OFF -DCMAKE_NO_SYSTEM_FROM_IMPORTED=1 -GNinja -DCMAKE_INSTALL_PREFIX=<sd-card-root>
cmake --build
sudo ninja install

同じようにtopを実行してみると似た結果が得られている。

top - 00:12:13 up 12 min,  2 users,  load average: 3.12, 1.92, 0.98
Tasks: 109 total,   4 running, 105 sleeping,   0 stopped,   0 zombie
%Cpu(s): 45.3 us, 14.8 sy,  0.0 ni, 39.5 id,  0.0 wa,  0.0 hi,  0.3 si,  0.0 st
MiB Mem :   1974.4 total,   1059.6 free,    636.4 used,    278.5 buff/cache
MiB Swap:      0.0 total,      0.0 free,      0.0 used.   1249.7 avail Mem 

  PID USER      PR  NI    VIRT    RES    SHR S  %CPU  %MEM     TIME+ COMMAND                                                                                 
 1212 root      20   0 1785152 499480  21788 R 125.2  24.7   0:15.17 orb_slam2_ros_m                                                                         
  812 root      20   0  516588  47672  22840 S  67.5   2.4   1:58.91 opencv_cam_main                                                                         
  133 root       0 -20       0      0      0 R  46.4   0.0   0:50.66 kworker/u9:0+uvcvideo                                                                   
 1211 root      20   0    3320   2064   1616 R   0.7   0.1   0:00.10 top

また、rviz2でdebug_imageを確認するとorb_slam2_rosから出力されているのでクロスコンパイル版もしっかり動作している模様。

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

2020年11月10日火曜日

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

実行結果

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

 




 

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