OrinNX上部署运行ROS2版本的vinsfusiongpu 20240429
OrinNX部署运行ROS2版本的vinsfusiongpu步骤整理
OrinNX ubuntu20.04上前提已经装好ros foxy
ros2版本的vinsfusiongpu仓库为：https://github.com/zinuok/VINS-Fusion-ROS2
装opencv3.4.1+opencv_contrib3.4.1
新建一个opencv341文件夹，文件夹内打开终端运行下面命令，下载opencv和opencv_contrib
git clone -b 3.4.1 https://github.com/opencv/opencv.git
git clone -b 3.4.1 https://github.com/opencv/opencv_contrib.git
在/opencv341/opencv/文件夹下新建一个build文件夹，并在build文件夹下打开一个终端运行下面命令
注意不同平台CUDA_ARCH_BIN的值不同，比如Xavier NX上CUDA_ARCH_BIN=7.2
cmake -D CMAKE_BUILD_TYPE=RELEASE \
-D CMAKE_INSTALL_PREFIX=/usr/local \
-D WITH_CUDA=ON \
-D CUDA_ARCH_BIN=8.7 \
-D CUDA_ARCH_PTX="" \
-D ENABLE_FAST_MATH=ON \
-D CUDA_FAST_MATH=ON \
-D WITH_CUBLAS=ON \
-D WITH_LIBV4L=ON \
-D WITH_GSTREAMER=ON \
-D WITH_GSTREAMER_0_10=OFF \
-D WITH_QT=ON \
-D WITH_OPENGL=ON \
-D CUDA_NVCC_FLAGS="--expt-relaxed-constexpr" \
-D CUDA_nppicom_LIBRARY=stdc++ \
-D WITH_TBB=ON \
-D OPENCV_EXTRA_MODULES_PATH=../../opencv_contrib/modules \
../
make -j4
sudo make install
打开jtop看到opencv版本显示为这样代表OK，注意with CUDA为yes
装ceres 2.1.0
sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3 libgflags-dev libgoogle-glog-dev libeigen3-dev libgtest-dev
git clone -b 2.1.0 https://github.com/ceres-solver/ceres-solver.git
cd ceres-solver/
mkdir build
cd build
cmake ..
make
sudo make install
源码编译安装ros2的cv_bridge
mkdir cv_bridge_ros2_ws/src
cd cv_bridge_ros2_ws
cd src
git clone https://github.com/ros-perception/vision_opencv.git -b foxy
把cv_bridge功能包里的cmakelists（/cv_bridge_ros2_ws/src/vision_opencv/cv_bridge/CMakeLists.txt）里这里的OpenCV 4改为OpenCV 3
改完后保存编译
cd ..
colcon build
部署ros2的vins fusion gpu
cd $(PATH_TO_YOUR_ROS2_WS)/src
git clone https://github.com/zinuok/VINS-Fusion-ROS2
在ros2的vins fusion的三个用到opencv的功能包(vins loop_fusion camera_models)里面的cmakelist里面加上这两句，这么做是确保其链接的是opencv3.4.1
set(OpenCV_DIR "your-path/opencv-x.x.x/build")
set(cv_bridge_DIR "your-path/cv_bridge_ros2_ws/install/cv_bridge/share/cv_bridge/cmake")
比如我的是加这两句
set(OpenCV_DIR "/home/nvidia/opencv341/opencv/build")
set(cv_bridge_DIR "/home/nvidia/cv_bridge_ros2_ws/install/cv_bridge/share/cv_bridge/cmake")
改完保存编译
cd ..
colcon build --symlink-install && source ./install/setup.bash && source ./install/local_setup.bash
此时可以在config文件夹下放入自己对应的比如D435i的双目imu的标定参数文件，此操作同普通vins-fusion一样。
启动命令示例，后面跟实际路径下的yaml文件
run vins vins_node ***/realsense_stereo_imu_config.yaml
如果要GPU加速，yaml文件里面记得加上这三句，这三个参数都置为1。
# GPU acceleration
use_gpu : 1
use_gpu_acc_flow: 1
use_gpu_ceres : 1
官方仓库给的启动rviz的命令是，这样启动后rviz需要手动修改坐标系添加话题，比较麻烦，而且作者说 Rviz config cannot be saved due to some issue.. still fixing
ros2 launch vins vins_rviz.launch.xml
个人建议用这个命令启动rviz，配置可以更改保存。
ros2 run rviz2 rviz2 -d config.rviz
把ros1的bag包转为ros2的（不基于bag包跑vins可以跳过这步）
推荐使用一个叫 rosbags 的工具进行转换，它不依赖于 ROS1 和 ROS2 环境，可以非常快速地实现 .bag 和 .db3 格式的相互转换。
rosbags 是一个 Python 编写的工具，可通过 pip 进行安装：
pip install rosbags
以后如果需要更新 rosbags，可以执行下面命令：
pip install --upgrade rosbags
安装好之后，系统会增加一个 rosbags-convert 命令，转换方法非常简单。例如：
将 ROS2 的 rosbag 转换为 ROS1 格式：
rosbags-convert <your-ros2-bag>
将 ROS1 的 rosbag 转换为 ROS2 格式：
rosbags-convert <your-ros1-bag>
这个命令运行完之后会在bag包相同目录下生成一个和bag包名称相同的文件夹，里面是一个.db3文件和一个yaml文件
播放ros2的bag包用命令
ros2 bag play *****.db3
装 realsense 的ros2功能包
sudo apt-get install ros-foxy-realsense2-camera
安装好后，可以在/opt/ros/foxy/share/realsense2_camera/launch目录下，找到rs_launch.py，修改rs_launch.py使得其启动D435i可以发出30hz的分辨率640*480双目灰度图话题和200hz的/camera/imu话题，给vins节点订阅使用。
修改rs_launch.py前需要先赋予权限
sudo chmod 777 rs_launch.py
修改后的可以发出30hz的分辨率640*480双目灰度图话题和200hz的/camera/imu话题和rs_launch.py如下
目前没有实现通过rs_launch.py关闭红外光点，所以需要物理手段也就是通过胶带把发射红外光点的地方遮挡住，避免影响灰度图，进而影响vins。
# License: Apache 2.0. See LICENSE file in root directory.
# Copyright(c) 2022 Intel Corporation. All Rights Reserved.
"""Launch realsense2_camera node."""
import os
from launch import LaunchDescription
from ament_index_python.packages import get_package_share_directory
import launch_ros.actions
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch.conditions import IfCondition
configurable_parameters = [{'name': 'camera_name', 'default': 'camera', 'description': 'camera unique name'},
{'name': 'serial_no', 'default': "''", 'description': 'choose device by serial number'},
{'name': 'usb_port_id', 'default': "''", 'description': 'choose device by usb port id'},
{'name': 'device_type', 'default': "''", 'description': 'choose device by type'},
{'name': 'config_file', 'default': "''", 'description': 'yaml config file'},
{'name': 'unite_imu_method', 'default': "2", 'description': '[0-None, 1-copy, 2-linear_interpolation]'},
{'name': 'json_file_path', 'default': "''", 'description': 'allows advanced configuration'},
{'name': 'log_level', 'default': 'info', 'description': 'debug log level [DEBUG|INFO|WARN|ERROR|FATAL]'},
{'name': 'output', 'default': 'screen', 'description': 'pipe node output [screen|log]'},
{'name': 'depth_module.profile', 'default': '640,480,30', 'description': 'depth module profile'},
{'name': 'enable_depth', 'default': 'false', 'description': 'enable depth stream'},
{'name': 'rgb_camera.profile', 'default': '0,0,0', 'description': 'color image width'},
{'name': 'enable_color', 'default': 'false', 'description': 'enable color stream'},
{'name': 'enable_infra1', 'default': 'true', 'description': 'enable infra1 stream'},
{'name': 'enable_infra2', 'default': 'true', 'description': 'enable infra2 stream'},
{'name': 'infra_rgb', 'default': 'false', 'description': 'enable infra2 stream'},
{'name': 'tracking_module.profile', 'default': '0,0,0', 'description': 'fisheye width'},
{'name': 'enable_fisheye1', 'default': 'true', 'description': 'enable fisheye1 stream'},
{'name': 'enable_fisheye2', 'default': 'true', 'description': 'enable fisheye2 stream'},
{'name': 'enable_confidence', 'default': 'true', 'description': 'enable depth stream'},
{'name': 'gyro_fps', 'default': '0', 'description': "''"},
{'name': 'accel_fps', 'default': '0', 'description': "''"},
{'name': 'enable_gyro', 'default': 'true', 'description': "''"},
{'name': 'enable_accel', 'default': 'true', 'description': "''"},
{'name': 'enable_pose', 'default': 'true', 'description': "''"},
{'name': 'pose_fps', 'default': '200', 'description': "''"},
{'name': 'pointcloud.enable', 'default': 'false', 'description': ''},
{'name': 'pointcloud.stream_filter', 'default': '2', 'description': 'texture stream for pointcloud'},
{'name': 'pointcloud.stream_index_filter','default': '0', 'description': 'texture stream index for pointcloud'},
{'name': 'enable_sync', 'default': 'true', 'description': "''"},
{'name': 'align_depth.enable', 'default': 'false', 'description': "''"},
{'name': 'colorizer.enable', 'default': 'false', 'description': "''"},
{'name': 'clip_distance', 'default': '-2.', 'description': "''"},
{'name': 'linear_accel_cov', 'default': '0.01', 'description': "''"},
{'name': 'initial_reset', 'default': 'false', 'description': "''"},
{'name': 'allow_no_texture_points', 'default': 'false', 'description': "''"},
{'name': 'ordered_pc', 'default': 'false', 'description': ''},
{'name': 'calib_odom_file', 'default': "''", 'description': "''"},
{'name': 'topic_odom_in', 'default': "''", 'description': 'topic for T265 wheel odometry'},
{'name': 'tf_publish_rate', 'default': '0.0', 'description': 'Rate of publishing static_tf'},
{'name': 'diagnostics_period', 'default': '0.0', 'description': 'Rate of publishing diagnostics. 0=Disabled'},
{'name': 'decimation_filter.enable', 'default': 'false', 'description': 'Rate of publishing static_tf'},
{'name': 'rosbag_filename', 'default': "''", 'description': 'A realsense bagfile to run from as a device'},
{'name': 'depth_module.exposure.1', 'default': '7500', 'description': 'Initial value for hdr_merge filter'},
{'name': 'depth_module.gain.1', 'default': '16', 'description': 'Initial value for hdr_merge filter'},
{'name': 'depth_module.exposure.2', 'default': '1', 'description': 'Initial value for hdr_merge filter'},
{'name': 'depth_module.gain.2', 'default': '16', 'description': 'Initial value for hdr_merge filter'},
{'name': 'wait_for_device_timeout', 'default': '-1.', 'description': 'Timeout for waiting for device to connect (Seconds)'},
{'name': 'reconnect_timeout', 'default': '6.', 'description': 'Timeout(seconds) between consequtive reconnection attempts'},
]
def declare_configurable_parameters(parameters):
return [DeclareLaunchArgument(param['name'], default_value=param['default'], description=param['description']) for param in parameters]
def set_configurable_parameters(parameters):
return dict([(param['name'], LaunchConfiguration(param['name'])) for param in parameters])
def generate_launch_description():
log_level = 'info'
if (os.getenv('ROS_DISTRO') == "dashing") or (os.getenv('ROS_DISTRO') == "eloquent"):
return LaunchDescription(declare_configurable_parameters(configurable_parameters) + [
# Realsense
launch_ros.actions.Node(
condition=IfCondition(PythonExpression([LaunchConfiguration('config_file'), " == ''"])),
package='realsense2_camera',
node_namespace=LaunchConfiguration("camera_name"),
node_name=LaunchConfiguration("camera_name"),
node_executable='realsense2_camera_node',
prefix=['stdbuf -o L'],
parameters=[set_configurable_parameters(configurable_parameters)
],
output='screen',
arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')],
),
launch_ros.actions.Node(
condition=IfCondition(PythonExpression([LaunchConfiguration('config_file'), " != ''"])),
package='realsense2_camera',
node_namespace=LaunchConfiguration("camera_name"),
node_name=LaunchConfiguration("camera_name"),
node_executable='realsense2_camera_node',
prefix=['stdbuf -o L'],
parameters=[set_configurable_parameters(configurable_parameters)
, PythonExpression([LaunchConfiguration("config_file")])
],
output='screen',
arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')],
),
])
else:
return LaunchDescription(declare_configurable_parameters(configurable_parameters) + [
# Realsense
launch_ros.actions.Node(
condition=IfCondition(PythonExpression([LaunchConfiguration('config_file'), " == ''"])),
package='realsense2_camera',
namespace=LaunchConfiguration("camera_name"),
name=LaunchConfiguration("camera_name"),
executable='realsense2_camera_node',
parameters=[set_configurable_parameters(configurable_parameters)
],
output='screen',
arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')],
emulate_tty=True,
),
launch_ros.actions.Node(
condition=IfCondition(PythonExpression([LaunchConfiguration('config_file'), " != ''"])),
package='realsense2_camera',
namespace=LaunchConfiguration("camera_name"),
name=LaunchConfiguration("camera_name"),
executable='realsense2_camera_node',
parameters=[set_configurable_parameters(configurable_parameters)
, PythonExpression([LaunchConfiguration("config_file")])
],
output='screen',
arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')],
emulate_tty=True,
),
])
用rs_launch.py启动D435i的命令如下
ros2 launch realsense2_camera rs_launch.py
OrinNX上跑ROS2版本vinsfusiongpu启动命令示例
运行前可以先把OrinNX的CPU频率和GPU频率提到最大，用下面这两个脚本
sudo sh max_cpu_freq.sh
sudo sh max_gpu_freq.sh
max_cpu_freq.sh内如如下
# Set CPU frequency to maximum
sudo echo "***** Set CPU frequency to maximum *****"
sudo echo "- Previous frequency of cores"
sudo cat /sys/devices/system/cpu/cpufreq/policy0/scaling_cur_freq
sudo echo "- Previous CPU governor of cores"
sudo cat /sys/devices/system/cpu/cpufreq/policy0/scaling_governor
sudo echo
sudo echo "- The list of available frequencies"
sudo cat /sys/devices/system/cpu/cpufreq/policy0/scaling_available_frequencies
sudo echo userspace > /sys/devices/system/cpu/cpufreq/policy0/scaling_governor
sudo echo 1971200 > /sys/devices/system/cpu/cpufreq/policy0/scaling_setspeed
sudo echo 115200 > /sys/devices/system/cpu/cpufreq/policy0/scaling_min_freq
sudo echo 1971200 > /sys/devices/system/cpu/cpufreq/policy0/scaling_max_freq
sudo echo
sudo echo "- Changed frequency of cores"
sudo cat /sys/devices/system/cpu/cpufreq/policy0/scaling_cur_freq
sudo echo "- Changed CPU governor of cores"
sudo cat /sys/devices/system/cpu/cpufreq/policy0/scaling_governor
max_gpu_freq.sh内容如下
#!/bin/bash
# Set GPU frequency to maximum
sudo echo "***** Set GPU frequency to maximum *****"
sudo echo "- Previous frequency of GPU"
sudo cat /sys/devices/17000000.ga10b/devfreq/17000000.ga10b/cur_freq
sudo echo "- The list of available frequencies"
sudo cat /sys/devices/17000000.ga10b/devfreq/17000000.ga10b/available_frequencies
sudo echo 765000000 > /sys/devices/17000000.ga10b/devfreq/17000000.ga10b/max_freq
sudo echo 765000000 > /sys/devices/17000000.ga10b/devfreq/17000000.ga10b/min_freq
sudo echo "- Changed frequency of GPU"
sudo cat /sys/devices/17000000.ga10b/devfreq/17000000.ga10b/cur_freq
基于bag包跑ROS2的vinsfusiongpu总的启动命令sh脚本示例
gnome-terminal --window -e 'bash -c " source ~/vins_ros2_ws/install/setup.bash;ros2 run vins vins_node /home/nvidia/vins_ros2_ws/src/VINS-Fusion-ROS2/config/euroc/euroc_stereo_imu_config.yaml; exec bash"' \
--tab -e 'bash -c "sleep 10;ros2 run rviz2 rviz2 -d /home/nvidia/vins_ros2_ws/src/VINS-Fusion-ROS2/vins/launch/config.rviz; exec bash"' \
--tab -e 'bash -c "sleep 16; ros2 bag play /home/nvidia/MH_01_easy/MH_01_easy.db3; exec bash"' \
--window -e 'bash -c "sleep 25; ros2 topic echo /odometry; exec bash"' \
基于D435i跑ROS2的vinsfusiongpu总的启动命令sh脚本示例
注意D435i插在OrinNX USB3.0的口上，而且用胶带把发红外光点的位置遮挡住
gnome-terminal --window -e 'bash -c " source ~/vins_ros2_ws/install/setup.bash;ros2 run vins vins_node /home/nvidia/vins_ros2_ws/src/VINS-Fusion-ROS2/config/realsense_d435i/realsense_stereo_imu_config.yaml; exec bash"' \
--tab -e 'bash -c "sleep 10;ros2 run rviz2 rviz2 -d /home/nvidia/vins_ros2_ws/src/VINS-Fusion-ROS2/vins/launch/config.rviz; exec bash"' \
--tab -e 'bash -c "sleep 16;ros2 launch realsense2_camera rs_launch_maxi.py ; exec bash"' \
--window -e 'bash -c "sleep 25; ros2 topic echo /odometry; exec bash"' \
1403
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
