ORB_SLAM2与ROS2集成：新一代机器人操作系统适配指南
项目地址: https://gitcode.com/gh_mirrors/or/ORB_SLAM2 引言：ROS2时代的SLAM挑战
你是否仍在为ROS1到ROS2的迁移而烦恼？作为实时定位与地图构建（SLAM）领域的经典方案，ORB_SLAM2在ROS1环境下表现卓越，但ROS2带来的DDS通信架构、节点生命周期管理和参数系统重构，使其原生支持成为迫切需求。本文将系统讲解如何将ORB_SLAM2无缝集成到ROS2生态，解决异步通信适配、节点生命周期管理和多传感器数据流同步三大核心痛点，最终实现一个兼容Foxy及后续版本的新一代SLAM系统。
读完本文你将获得：
- 掌握ROS2与ORB_SLAM2通信层适配的关键技术
- 学会使用ament_cmake构建系统重构编译流程
- 理解节点生命周期管理在SLAM系统中的实践应用
- 解决多传感器数据时间同步的工程方案
- 完整的单目/双目/RGB-D相机适配代码框架
技术背景：从ROS1到ROS2的架构演进
ROS1与ROS2核心差异对比
| 特性 | ROS1 | ROS2 | 对ORB_SLAM2的影响 |
|---|---|---|---|
| 通信层 | TCPROS/UDPROS | DDS | 需要重构话题订阅机制 |
| 节点管理 | 主从架构 | 分布式发现 | 无需roscore，需处理节点状态 |
| 生命周期 | 无统一管理 | Node Lifecycle | 需要适配激活/关闭状态 |
| 参数系统 | 动态重配置 | 统一参数服务 | 配置文件读取方式变更 |
| 时间同步 | message_filters | 内置时间同步 | 需重构多传感器同步逻辑 |
ORB_SLAM2系统架构回顾
ORB_SLAM2采用模块化设计，主要包含以下核心组件：
适配实战：ROS2节点开发全流程
环境准备与依赖安装
首先确保系统已安装ROS2（推荐Foxy或Galactic版本）及必要依赖：
# 安装基础依赖
sudo apt install ros-foxy-desktop python3-colcon-common-extensions
# 安装SLAM相关依赖
sudo apt install ros-foxy-cv-bridge ros-foxy-image-transport ros-foxy-sensor-msgs
sudo apt install libopencv-dev libeigen3-dev libpangolin-dev
项目结构重构
将ORB_SLAM2源码重构为ROS2包结构：
orb_slam2_ros2/
├── CMakeLists.txt # ament_cmake构建文件
├── package.xml # ROS2包信息
├── src/
│ ├── orb_slam2_node.cpp # 主节点实现
│ ├── mono_node.cpp # 单目相机节点
│ ├── stereo_node.cpp # 双目相机节点
│ └── rgbd_node.cpp # RGB-D相机节点
├── include/orb_slam2_ros2/
│ ├── node_base.hpp # 节点基类
│ └── image_grabber.hpp # 图像抓取器
├── config/ # 相机配置文件
└── launch/ # 启动文件
核心适配代码实现
1. 节点基类设计
创建node_base.hpp实现ROS2节点生命周期管理：
#ifndef ORB_SLAM2_ROS2_NODE_BASE_HPP_
#define ORB_SLAM2_ROS2_NODE_BASE_HPP_
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "cv_bridge/cv_bridge.h"
#include "ORB_SLAM2/System.h"
namespace orb_slam2_ros2
{
class NodeBase : public rclcpp_lifecycle::LifecycleNode
{
public:
explicit NodeBase(const std::string & node_name, bool intra_process_comms = false);
// 生命周期回调函数
CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override;
CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override;
protected:
std::unique_ptr<ORB_SLAM2::System> slam_system_;
std::string voc_file_path_;
std::string settings_file_path_;
ORB_SLAM2::System::eSensor sensor_type_;
// 参数声明
void declare_parameters();
};
} // namespace orb_slam2_ros2
#endif // ORB_SLAM2_ROS2_NODE_BASE_HPP_
2. 单目节点实现
创建mono_node.cpp实现单目相机SLAM节点：
#include "orb_slam2_ros2/node_base.hpp"
#include "orb_slam2_ros2/image_grabber.hpp"
namespace orb_slam2_ros2
{
class MonoNode : public NodeBase
{
public:
explicit MonoNode(const rclcpp::NodeOptions & options)
: NodeBase("orb_slam2_mono_node", options)
{
sensor_type_ = ORB_SLAM2::System::MONOCULAR;
declare_parameters();
}
protected:
void declare_parameters() override
{
NodeBase::declare_parameters();
// 单目相机特有参数
this->declare_parameter("camera.topic", "/camera/image_raw");
}
CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override
{
NodeBase::on_activate(previous_state);
// 创建图像抓取器
image_grabber_ = std::make_unique<ImageGrabber>(slam_system_.get());
// 创建订阅者（使用生命周期节点的订阅接口）
auto qos = rclcpp::QoS(rclcpp::KeepLast(1)).best_effort();
image_sub_ = this->create_subscription<sensor_msgs::msg::Image>(
this->get_parameter("camera.topic").as_string(),
qos,
std::bind(&ImageGrabber::GrabImage, image_grabber_.get(), std::placeholders::_1)
);
return CallbackReturn::SUCCESS;
}
CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override
{
image_sub_.reset();
image_grabber_.reset();
return NodeBase::on_deactivate(previous_state);
}
private:
std::unique_ptr<ImageGrabber> image_grabber_;
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
};
} // namespace orb_slam2_ros2
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(orb_slam2_ros2::MonoNode)
3. 图像抓取器实现
创建image_grabber.hpp处理ROS图像消息转换：
#ifndef ORB_SLAM2_ROS2_IMAGE_GRABBER_HPP_
#define ORB_SLAM2_ROS2_IMAGE_GRABBER_HPP_
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "cv_bridge/cv_bridge.h"
#include "ORB_SLAM2/System.h"
namespace orb_slam2_ros2
{
class ImageGrabber
{
public:
explicit ImageGrabber(ORB_SLAM2::System * slam_system)
: slam_system_(slam_system) {}
void GrabImage(const sensor_msgs::msg::Image::ConstSharedPtr & msg)
{
// 将ROS图像消息转换为OpenCV格式
cv_bridge::CvImageConstPtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvShare(msg);
}
catch (const cv_bridge::Exception & e)
{
RCLCPP_ERROR(rclcpp::get_logger("image_grabber"), "cv_bridge exception: %s", e.what());
return;
}
// 调用ORB_SLAM2单目跟踪接口
slam_system_->TrackMonocular(cv_ptr->image, cv_ptr->header.stamp.sec + cv_ptr->header.stamp.nanosec * 1e-9);
}
private:
ORB_SLAM2::System * slam_system_;
};
} // namespace orb_slam2_ros2
#endif // ORB_SLAM2_ROS2_IMAGE_GRABBER_HPP_
CMakeLists.txt配置
使用ament_cmake构建系统替代原有的ROS1构建系统：
cmake_minimum_required(VERSION 3.5)
project(orb_slam2_ros2)
# C++标准
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# 依赖
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(image_transport REQUIRED)
find_package(OpenCV REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(Pangolin REQUIRED)
# ORB_SLAM2库
add_subdirectory(ORB_SLAM2)
# 包含目录
include_directories(
include
ORB_SLAM2/include
${EIGEN3_INCLUDE_DIRS}
${Pangolin_INCLUDE_DIRS}
)
# 节点可执行文件
add_executable(mono_node src/mono_node.cpp)
ament_target_dependencies(mono_node
rclcpp
rclcpp_lifecycle
sensor_msgs
cv_bridge
OpenCV
)
target_link_libraries(mono_node
ORB_SLAM2
${Pangolin_LIBRARIES}
)
# 安装
install(TARGETS
mono_node
DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY
launch
config
DESTINATION share/${PROJECT_NAME})
ament_package()
关键技术解析
1. 通信层适配：从ros::Subscriber到rclcpp::Subscription
ROS1中的订阅代码：
// ROS1代码
ros::NodeHandle nodeHandler;
ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage, &igb);
ROS2中的对应实现：
// ROS2代码
auto qos = rclcpp::QoS(rclcpp::KeepLast(1)).best_effort();
image_sub_ = this->create_subscription<sensor_msgs::msg::Image>(
"/camera/image_raw",
qos,
std::bind(&ImageGrabber::GrabImage, image_grabber_.get(), std::placeholders::_1)
);
主要变化点：
- 使用QoS策略替代队列大小参数
- 通过std::bind绑定成员函数回调
- 生命周期节点使用create_subscription接口
- 支持多配置文件的话题名称参数化
2. 节点生命周期管理
ROS2引入的节点生命周期管理允许SLAM系统优雅地处理激活/关闭过程：
实现代码片段：
CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override
{
// 激活时创建SLAM系统
slam_system_ = std::make_unique<ORB_SLAM2::System>(
voc_file_path_, settings_file_path_, sensor_type_, true);
return CallbackReturn::SUCCESS;
}
CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override
{
// 停用时保存轨迹并关闭SLAM系统
slam_system_->SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
slam_system_->Shutdown();
slam_system_.reset();
return CallbackReturn::SUCCESS;
}
3. 多传感器时间同步
对于双目和RGB-D相机，需要同步处理不同话题的图像数据。ROS2提供了message_filters的替代方案：
#include "message_filters/subscriber.h"
#include "message_filters/synchronizer.h"
#include "message_filters/sync_policies/approximate_time.h"
// 定义同步策略（近似时间同步）
using SyncPolicy = message_filters::sync_policies::ApproximateTime<
sensor_msgs::msg::Image, sensor_msgs::msg::Image>;
// 创建同步器
auto sync = std::make_shared<message_filters::Synchronizer<SyncPolicy>>(SyncPolicy(10),
left_sub_, right_sub_);
sync->registerCallback(std::bind(&StereoGrabber::GrabStereoImage, this,
std::placeholders::_1, std::placeholders::_2));
测试与验证
编译与运行
# 创建工作空间
mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src
git clone https://gitcode.com/gh_mirrors/or/ORB_SLAM2.git
# 拷贝本文实现的ROS2适配代码到ORB_SLAM2目录
# 编译
cd ~/ros2_ws
colcon build --packages-select orb_slam2_ros2
# 运行单目节点
source install/setup.bash
ros2 run orb_slam2_ros2 mono_node --ros-args -p vocabulary:=Vocabulary/ORBvoc.txt -p settings:=config/TUM1.yaml
轨迹评估
使用evo工具评估SLAM精度：
# 安装evo
pip install evo --upgrade
# 评估TUM数据集轨迹
evo_ape tum groundtruth.txt KeyFrameTrajectory.txt -va --plot
常见问题解决方案
1. DDS兼容性问题
不同DDS实现可能导致图像传输延迟，推荐使用FastDDS并配置：
<!-- fastdds.xml -->
<profiles>
<data_writer profile_name="image_writer" is_default_profile="true">
<qos>
<publish_mode>
<mode>SYNCHRONOUS</mode>
</publish_mode>
</qos>
</data_writer>
</profiles>
2. 内存泄漏问题
确保在节点停用时正确释放SLAM系统资源：
CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override
{
image_sub_.reset();
image_grabber_.reset();
slam_system_.reset();
return CallbackReturn::SUCCESS;
}
3. 性能优化
对于资源受限设备，可通过以下方式优化性能：
// 降低图像分辨率
cv::Mat resized_image;
cv::resize(cv_ptr->image, resized_image, cv::Size(), 0.5, 0.5);
mpSLAM->TrackMonocular(resized_image, timestamp);
总结与展望
本文详细介绍了ORB_SLAM2与ROS2集成的关键技术，包括通信层适配、生命周期管理、时间同步和构建系统重构。通过这些改造，经典的ORB_SLAM2得以在新一代机器人操作系统上焕发新生。
未来工作可关注：
- 基于ROS2组件的SLAM模块拆分
- 利用ROS2 DDS的分布式SLAM实现
- 集成ROS2导航栈进行闭环测试
- 基于composition的多传感器融合架构
ROS2生态系统正在快速发展，ORB_SLAM2作为视觉SLAM的经典方案，其与ROS2的结合将为机器人导航、增强现实等领域带来更多可能。完整代码已开源，欢迎社区贡献和改进。
附录：完整项目文件结构
orb_slam2_ros2/
├── CMakeLists.txt
├── package.xml
├── ORB_SLAM2/ # 原ORB_SLAM2源码
│ ├── include/
│ ├── src/
│ ├── Thirdparty/
│ └── Vocabulary/
├── src/
│ ├── orb_slam2_node.cpp
│ ├── mono_node.cpp
│ ├── stereo_node.cpp
│ ├── rgbd_node.cpp
│ └── image_grabber.cpp
├── include/orb_slam2_ros2/
│ ├── node_base.hpp
│ └── image_grabber.hpp
├── config/
│ ├── TUM1.yaml
│ ├── TUM2.yaml
│ ├── KITTI00-02.yaml
│ └── EuRoC.yaml
└── launch/
├── mono_launch.py
├── stereo_launch.py
└── rgbd_launch.py
通过以上结构，开发者可以快速搭建ROS2环境下的ORB_SLAM2系统，为机器人应用提供精确的定位与地图构建能力。
创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考
