ROS 2 环境下使用 Astra Pro 深度相机实现目标距离检测及远程可视化全流程总结
一、环境准备
硬件:
rk3588-elf2开发板(aarch64 Ubuntu22.04 ros2humble) , AstraPro相机
网络:板子和Windows电脑连的同一个手机热点
软件:
- Windows:安装 PuTTY(SSH 工具)、Xming(X11 转发工具)
开发板(Ubuntu):
sudo apt install ros-humble-rviz2 ros-humble-rqt-image-view # 安装可视化工具
注意:
环境配置过程后面再补吧,暂时懒得写
后面的步骤全部建立在环境各种条件依赖什么的都配置好了
二、通过 PuTTY+Xming 实现远程图形界面显示
-
启动 Xming(Windows 端):
- 双击桌面
XLaunch
图标 → 选择Multiple windows → Start no client → Finish,任务栏显示 Xming 图标。
- 双击桌面
-
PuTTY 配置 SSH 连接:
- 打开 PuTTY → 输入开发板 IP 地址 → 展开Connection > SSH > X11 → 勾选Enable X11 forwarding,填写X display location为
localhost:0
→ 保存并连接。
- 打开 PuTTY → 输入开发板 IP 地址 → 展开Connection > SSH > X11 → 勾选Enable X11 forwarding,填写X display location为
-
验证 X11 转发(开发板终端):
ssh -X username@开发板IP # 通过SSH连接开发板(含X11转发)
xclock # 若Windows弹出时钟窗口,说明配置成功
三、启动 Astra 相机并获取深度数据
-
启动相机节点(开发板终端):
ros2 launch astra_camera astra_pro.launch.py # 启动深度相机(默认发布彩色/深度图像)
在 RViz2 中显示相机画面(Windows 端):
- 通过 PuTTY 连接后,在开发板终端执行:
rviz2 # RViz2窗口会通过Xming转发到Windows桌面
-
- 在 RViz2 中配置:
- Global Options > Fixed Frame 设置为
camera_link
(相机 TF 根坐标系)。 - 点击Add > By Topic,选择
/camera/color/image_raw
添加图像显示插件。
- Global Options > Fixed Frame 设置为
- 在 RViz2 中配置:
四、实现目标距离检测(开发板端)
方案:结合深度图像直接计算距离
-
创建 ROS 2 节点(订阅深度图像):
- 在功能包
src
目录下创建depth_distance_node.cpp
(示例代码):
- 在功能包
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "cv_bridge/cv_bridge.h"
using namespace std::chrono_literals; class DepthDistanceNode : public rclcpp::Node {
public: DepthDistanceNode() : Node("depth_distance_node") { depth_sub_ = this->create_subscription<sensor_msgs::msg::Image>( "/camera/depth/image_raw", 10, std::bind(&DepthDistanceNode::depth_callback, this, std::placeholders::_1)); } private: void depth_callback(const sensor_msgs::msg::Image::SharedPtr msg) { cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_16UC1); cv::Mat depth_image = cv_ptr->image; int h = depth_image.rows, w = depth_image.cols; uint16_t depth_value = depth_image.at<uint16_t>(h/2, w/2); // 取中心像素 float distance_m = depth_value / 1000.0f; // 转换为米 if (depth_value != 0 && distance_m < 3.0) { // 过滤无效值 RCLCPP_INFO(this->get_logger(), "Center distance: %.2f m", distance_m); } } rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr depth_sub_;
}; int main(int argc, char * argv[]) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared<DepthDistanceNode>()); rclcpp::shutdown(); return 0;
}
编译节点(开发板终端):
- 修改功能包的
CMakeLists.txt
:
add_executable(depth_distance_node src/depth_distance_node.cpp)
ament_target_dependencies(depth_distance_node rclcpp sensor_msgs cv_bridge)
install(TARGETS depth_distance_node DESTINATION lib/${PROJECT_NAME})
编译并激活环境:
cd ~/工作空间 && colcon build --packages-select your_package
source install/setup.bash
运行节点(开发板终端):
ros2 run your_package depth_distance_node # 实时输出中心像素距离
五、完整操作流程图
Windows主机 开发板(Ubuntu)
├─ 启动Xming/XLaunch ├─ 连接Astra相机
├─ 打开PuTTY并配置X11转发 ├─ 启动相机节点:ros2 launch astra_camera astra_pro.launch.py
└─ 通过SSH连接开发板(含-X参数) ├─ 运行RViz2:rviz2(画面通过Xming显示在Windows) └─ 运行距离检测节点:ros2 run your_package depth_distance_node
六、关键问题与解决方案
-
RViz2 无法显示画面:
- 确保 PuTTY 启用 X11 转发,开发板
/etc/ssh/sshd_config
中X11Forwarding yes
。 - 在开发板终端手动设置
export DISPLAY=localhost:0
。
- 确保 PuTTY 启用 X11 转发,开发板
-
深度值异常(如 8 米以上):
- 相机有效范围为 0.5~3 米,远距离数据无效,需缩短检测距离。
- 避免强光直射,调整相机
ir_intensity
参数(在 launch 文件中添加ir_intensity:=400
)。
-
节点编译失败:
- 确保安装依赖:
sudo apt install ros-humble-cv-bridge ros-humble-image-transport
。 - 检查
CMakeLists.txt
中 OpenCV 链接是否正确:target_link_libraries(node_name ${OpenCV_LIBS})
。
- 确保安装依赖:
七、小笔记
用于查找与深度数据相关的话题(Topic)
ros2 topic list | grep depth
ros2 topic list
ROS 2 的命令,用于列出当前 ROS 2 系统中所有正在发布的话题。话题是 ROS 2 中节点间通信的一种方式,类似于 “消息总线”,节点可以发布(Publish)或订阅(Subscribe)特定话题的数据。
|
管道符号,用于将前一个命令的输出作为后一个命令的输入。
grep depth
Linux 命令,用于在文本中搜索包含 “depth”(深度)的行。在这里,它会筛选出ros2 topic list
输出中所有名称包含 “depth” 的话题。
实际用途
在机器人感知系统中,深度数据通常用于 3D 建模、障碍物检测、导航等任务。常见的深度话题包括:
/camera/depth/image_raw
:原始深度图像(像素值表示距离)/camera/depth_registered/image_raw
:与彩色图像对齐的深度图像/camera/depth/points
:点云数据(3D 空间中的点集合)
运行这个指令后,你可以快速找到与深度相机(如 Astra Pro、Intel RealSense 等)相关的话题,便于后续订阅和处理这些数据。
示例输出:
/camera/depth/camera_info
/camera/depth/image_raw
/camera/depth/image_rect_raw
/camera/depth/metadata
/camera/depth/points
扩展用法
查看话题详情:
ros2 topic info /camera/depth/image_raw # 查看话题的类型和发布者
监听话题数据:
ros2 topic echo /camera/depth/image_raw # 实时打印话题数据
可视化深度图像:
rviz2 # 启动ROS 2可视化工具
# 在RViz中添加Image显示组件,并选择/camera/depth/image_raw话题