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 Format是YUYV和 GREY 两种格式，默认值为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时，报错：
此时去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文件
找到计算时间的代码处
删除该段所有的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上标定好的相机文件，直接拿过来用的。
结果：
没在报错，撒花～