如何在ROS2中通过librealsense获取T265最大帧率的图像与位姿流

设备参数 #

• 相机型号：T265
• 固件版本：0.2.0.951
• 操作系统：Linux Ubuntu 20.04LTS
• 内核版本：4.9.253
• 平台：NVIDIA Jetson
• SDK版本：2.53.1
• 开发语言：C++
• 应用场景：Segment Robot

问题总结 #

1. 双节点双管线：单个节点可跑满对应流的帧率，但同时启动时其中一个节点触发No Device Connected Error
2. 单节点单管线：可获取所有流数据，但整体帧率被限制在约60Hz，无法达到30Hz图像流+200Hz位姿流的需求
3. 本地基于回调的C++代码可跑满帧率，但移植到ROS2环境后失效

解决方案 #

1. 双节点方案不可行的原因 #

RealSense T265硬件层面不支持多个独立rs2::pipeline同时访问设备，设备资源为独占模式，第二个节点启动时会因无法获取设备句柄报错，直接放弃该方案即可。

2. 单节点单管线的优化方案 #

核心思路是异步回调+多线程分离处理+ROS2 QoS适配，避免单线程阻塞拖慢整体帧率：

（1）采用异步回调模式启动管线

不要用同步wait_for_frames()，改用管线的异步回调接口，让librealsense在后台线程推送帧数据，避免主线程阻塞：

rs2::pipeline pipe;
rs2::config cfg;

// 配置位姿流（200Hz）和图像流（30Hz）
cfg.enable_stream(RS2_STREAM_POSE, RS2_FORMAT_6DOF);
cfg.enable_stream(RS2_STREAM_FISHEYE, 1, 848, 800, RS2_FORMAT_Y8, 30);
cfg.enable_stream(RS2_STREAM_FISHEYE, 2, 848, 800, RS2_FORMAT_Y8, 30);

// 启动管线并注册异步回调
pipe.start(cfg, [&](const rs2::frame& frame) {
    // 分类型处理不同帧
    if (auto pose_frame = frame.as<rs2::pose_frame>()) {
        // 位姿帧处理逻辑，放到单独线程
        std::thread(&YourNode::process_pose, this, pose_frame).detach();
    } else if (auto video_frame = frame.as<rs2::video_frame>()) {
        // 图像帧处理逻辑，放到单独线程
        std::thread(&YourNode::process_video, this, video_frame).detach();
    }
});

（2）多线程分离流的发布逻辑

在回调中只做帧数据的浅拷贝（rs2::frame本身是轻量级引用），把ROS2消息转换和发布操作放到独立线程执行，避免回调阻塞导致帧丢失或帧率下降：

// 位姿处理线程函数
void YourNode::process_pose(const rs2::pose_frame& pf) {
    // 拷贝位姿数据（避免原帧被释放）
    auto pose_data = pf.get_pose_data();
    
    // 转换为ROS2的PoseStamped消息
    auto msg = std::make_unique<geometry_msgs::msg::PoseStamped>();
    msg->header.stamp = this->get_clock()->now();
    msg->header.frame_id = "t265_pose_frame";
    msg->pose.position.x = pose_data.translation.x;
    msg->pose.position.y = pose_data.translation.y;
    msg->pose.position.z = pose_data.translation.z;
    msg->pose.orientation.w = pose_data.rotation.w;
    msg->pose.orientation.x = pose_data.rotation.x;
    msg->pose.orientation.y = pose_data.rotation.y;
    msg->pose.orientation.z = pose_data.rotation.z;
    
    // 发布到ROS2话题
    pose_publisher_->publish(std::move(msg));
}

// 图像处理线程函数（以左鱼眼为例）
void YourNode::process_video(const rs2::video_frame& vf) {
    if (vf.get_profile().stream_index() != 1) return; // 过滤左鱼眼
    
    // 转换为ROS2的Image消息
    auto msg = std::make_unique<sensor_msgs::msg::Image>();
    msg->header.stamp = this->get_clock()->now();
    msg->header.frame_id = "t265_fisheye_left";
    msg->width = vf.get_width();
    msg->height = vf.get_height();
    msg->encoding = "mono8";
    msg->step = vf.get_stride_in_bytes();
    msg->data.resize(vf.get_stride_in_bytes() * vf.get_height());
    std::memcpy(msg->data.data(), vf.get_data(), msg->data.size());
    
    // 发布到ROS2话题
    fisheye_left_pub_->publish(std::move(msg));
}

（3）适配ROS2 QoS参数

针对不同流的频率和需求调整QoS，避免消息队列阻塞：

• 位姿流（200Hz）：用Best Effort可靠性策略，保留最近100帧，避免队列堆积：

  auto pose_qos = rclcpp::QoS(rclcpp::KeepLast(100)).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);
  pose_publisher_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("t265/pose", pose_qos);
  

• 图像流（30Hz）：用Reliable策略，保留最近10帧即可：

  auto img_qos = rclcpp::QoS(rclcpp::KeepLast(10)).reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE);
  fisheye_left_pub_ = this->create_publisher<sensor_msgs::msg::Image>("t265/fisheye_left", img_qos);
  

（4）平台与代码优化

• Jetson性能调优：设置CPU为性能模式，关闭不必要的后台进程：

  sudo nvpmodel -m 0
  sudo jetson_clocks
  

• 减少回调内耗时操作：回调中只做帧类型判断和数据拷贝，所有耗时的转换、计算、发布都放到独立线程；避免在回调中调用ROS2的同步API（如get_parameter()）。
• 禁用冗余功能：在rs2::config中只启用需要的流，关闭T265的其他传感器（如IMU），减少设备负载。

3. ROS2节点循环处理 #

不要直接用rclcpp::spin(node_ptr)阻塞主线程，改用rclcpp::spin_some配合自定义循环，保证异步回调的执行优先级：

while (rclcpp::ok()) {
    rclcpp::spin_some(this->get_node_base_interface());
    std::this_thread::sleep_for(std::chrono::milliseconds(1));
}

内容的提问来源于stack exchange，提问作者Giovanni Casini