ROS2 基于USB单目相机(格式MJPEG)运行ORB_SLAM3及一些问题解决

环境:Ubuntu 20.04、ROS2 Foxy

1.下载编译非ros部分的ORB_SLAM3
我下载的是有详细注释版本的SLAM3:https://github.com/electech6/ORB_SLAM3_detailed_comments
解压并重命名为ORB_SLAM3。
1)修改CMakeLists.txt
/ORB_SLAM3/CMakeLists.txt
/ORB_SLAM3/Thirdparty/DBoW2/CMakeLists.txt
修改这两个CMakeLists.txt中的Opencv版本,我是用的Foxy安装自带的Opencv4.2.0。
2)编译
在ORB_SLAM3文件夹中进入终端:

chmod +x ./build.sh
./build.sh

2.下载安装ros2 foxy版本的orbslam3
如果你的ros2安装过message-filters,忽略此步骤,如果没安装,则:

sudo apt install ros-foxy-message-filters

1)创建ros2工作空间
home文件夹下创建:ROS2_ORB_SLAM3/src
2)下载源码包
网址:https://github.com/zang09/ORB_SLAM3_ROS2
将其解压放入ROS2_ORB_SLAM3/src中并重命名为orbslam3_ros2
3)修改CMakeLists.txt
ROS2_ORB_SLAM3/src/orbslam3_ros2/CMakeLists.txt

set(ENV{PYTHONPATH} "/opt/ros/foxy/lib/python3.8/site-packages/")

改成自己ros2安装的对应路径
4)修改FindORB_SLAM3.cmake
ROS2_ORB_SLAM3/src/orbslam3_ros2/CMakeModules/FindORB_SLAM3.cmake
修改第8行,改为之前非ROS的ORB_SLAM3的安装路径

set(ORB_SLAM3_ROOT_DIR "~/ORB_SLAM3")

5)编译
在ORB_SLAM3_ROS2进入终端:

colcon build

如果编译报错找不到sophus/se3.hpp:

cd ~/ORB_SLAM3/Thirdparty/Sophus/build
sudo make install

再次编译成功。
在home文件夹下ctrl+H,找到./bashrc文件,文件最后添加:

source ~/ROS2_ORB_SLAM3/install/setup.bash
export LD_LIBRARY_PATH=~/ORB_SLAM3/lib:$LD_LIBRARY_PATH

保存并刷新:source ~/.bashrc

3.安装USB相机驱动
我的相机:USB相机Pixel Format: ‘MJPG’ (Motion-JPEG)
注意:在查阅资料时候说ROS1适合使用usb_cam驱动,ROS2适合使用v4l2_camera驱动。
通过实验发现其实都可以但需要修改,在ROS2中:
v4l2_camera驱动:只支持相机的Pixel FormatYUYVGREY 两种格式,默认值为YUYV 。如果想改就得源码编译,将MJPEG格式转变为YUYV。
usb_cam驱动:如果直接安装该驱动,运行SLAM3时就会发现老是重新建图,且报错ERROR:Frame with a timestamp older than previous frame detected!,所以也是需要源码下载修改编译。

下面以usb_cam驱动为例:
1)直接安装usb_cam驱动

sudo apt install ros-foxy-usb-cam

相机什么都正常,但是在运行SLAM3时,报错:
ROS2 基于USB单目相机(格式MJPEG)运行ORB_SLAM3及一些问题解决_第1张图片此时去ros查看相机图像话题发布详情就会发现,时间戳在满位进位时不正常,导致当前时间戳比上一时间戳小的情况发生 进而发生错误。所以不要采用直接安装。如果你安装了请先删除其在ROS2中的安装路径。
2)下载ros2 版本usb_cam驱动源码
下载网址:https://github.com/ros-drivers/usb_cam/tree/ros2
3)创建ros2工作空间
创建usb_camera/src,将下载的usb_cam驱动源码压缩包放入并解压重命名为usb_cam。
4)安装相关依赖
在usb_camera中进入终端:

rosdep install --from-paths src --ignore-src -y

在这里插入图片描述如果有依赖项目报错,请参考:
https://blog.csdn.net/liyuanjunfrank/article/details/122155262
5)修改usb_cam.cpp文件
ROS2 基于USB单目相机(格式MJPEG)运行ORB_SLAM3及一些问题解决_第2张图片找到计算时间的代码处
ROS2 基于USB单目相机(格式MJPEG)运行ORB_SLAM3及一些问题解决_第3张图片删除该段所有的round操作,不采用近似算法计算时间。
6)编译
在usb_camera文件夹中进入终端:

colcon build

7)修改.yaml
我的相机接口是/dev/video4,分辨率是1920x1080
打开usb_camera/src/usb_cam/config/params.yaml文件
修改video_device、image_width、image_height
8)运行
在usb_camera文件夹中进入终端:

 . install/local_setup.sh
 ros2 run usb_cam usb_cam_node_exe --ros-args --params-file /home/fei/usb_camera/src/usb_cam/config/params.yaml

可通过ros2 run rqt_image_view rqt_image_view指令查看画面
9)运行单目ROS2_ORB_SLAM3
保持上述相机在发布图像,修改ROS2_ORB_SLAM3中ROS2_ORB_SLAM3/src/orbslam3_ros2/src/monocular/monocular-slam-node.cpp中的图像话题名称为/image_raw,重新编译。
在ROS2_ORB_SLAM3文件夹中进入终端:

. install/local_setup.sh
ros2 run orbslam3 mono ~/ORB_SLAM3/Vocabulary/ORBvoc.txt ~/ORB_SLAM3/Examples/Monocular/USBcam.yaml

USBcam.yaml是我之前在ROS1上标定好的相机文件,直接拿过来用的。
结果:
没在报错,撒花~

你可能感兴趣的:(ROS2,ORB_SLAM3,Ubuntu,ubuntu,c++)