简介：这是一套面向机器人方向学习者与开发者的ROS2自主导航视觉系统完整工程包，适合作为毕业设计、课程设计或进阶练手项目，帮助读者理解视觉感知、路径规划与运动控制如何在同一框架下协同工作。压缩包共821个文件，约58.56MB，以188个py与137个cpp源码、130个h和73个hpp头文件为主体，辅以48个msg与7个srv接口定义、18个xacro与14个rviz配置、6个yaml参数文件及Dockerfile等环境脚本，覆盖感知、标定、导航与仿真多个模块。资源围绕相机标定、BEV感知、DWA导航等典型环节展开，包含可复用的消息接口、可视化配置与部署脚本，便于读者对照目录结构梳理工程组织方式，快速搭建自己的导航视觉实验环境。目前已有54人学习下载，适合希望把ROS2理论落到实处的同学参考。
1. 从一张压缩包说起：ROS2 自主导航视觉系统到底在做什么
很多人第一次看到「基于ROS2的自主导航视觉系统」这个标题，脑子里浮现的是那种能自己绕开椅子、走到指定位置的机器人。但真把压缩包解压开，你会发现里面大概率是几个 ROS2 功能包、一堆 launch 文件、几份 YAML 参数，外加一个用 OpenCV 或深度模型写的视觉节点。它要解决的核心问题其实很朴素：让机器人靠摄像头看懂前方有什么，再结合里程计和激光雷达，规划出一条不撞墙的路，最后把速度指令发给底盘。
这套东西适合两类人。一类是刚学完 ROS2 话题、服务、动作，想找个完整项目把知识串起来的开发者；另一类是在做 AGV、巡检车、教学小车，需要一套能跑通、能改参数的导航视觉方案。它不追求在开放道路跑，而是在室内结构化环境里稳定工作。ROS2 Humble 是目前最常被选中的版本，Ubuntu 22.04 是它的默认搭档，这两个组合在社区里资料最多，踩坑后最容易搜到答案。
视觉系统在这里不是单纯做识别，它承担的是「感知—定位—决策」链路里的感知层。摄像头给出图像，视觉节点输出障碍物距离或可通行区域，导航栈拿到这些信息后更新代价地图，再交给 Nav2 规划器。整条链路里，任何一个环节的参数没调好，机器人就会表现出「原地转圈」「贴着墙走」「到了目标点不停」这些玄学行为。下面按实际落地顺序，把选型、搭建、参数和排错一层层拆开。
2. 视觉感知与导航栈的选型：为什么不是随便接个摄像头就行
2.1 视觉方案的三条路线与适用边界
在 ROS2 里做自主导航视觉，常见路线有三条。第一条是纯几何视觉，用 OpenCV 做边缘检测、连通域分析，输出障碍物在图像中的位置，再通过相机标定换算成机器人坐标系下的距离。这条路对算力要求低，树莓派 4B 就能跑，但受光照影响极大，地面反光或阴影会被误判成障碍。第二条是深度相机路线，用 RealSense 或奥比中光这类设备直接拿深度图，转成点云后喂给 Nav2 的 voxel layer 或 obstacle layer。这条路稳定得多，但深度相机在强光下会失效，且价格比普通 USB 摄像头高一个量级。第三条是视觉模型路线，用 YOLO 或轻量分割网络识别可通行区域，输出语义栅格地图。这条路效果上限高，但需要 GPU，且模型推理延迟会直接影响导航实时性。
我一般会建议：如果是教学或验证算法，先用普通 USB 摄像头加几何视觉跑通链路；如果是产品原型，直接上深度相机，省掉大量调参时间；如果场景里有动态行人或特定障碍物，再考虑加视觉模型做语义补充。三条路线不是互斥的，可以在同一个 launch 文件里同时启动，用不同的 costmap layer 叠加。
2.2 ROS2 Humble 下的导航栈组成与话题衔接
ROS2 Humble 的导航栈核心是 Nav2。它由多个生命周期节点组成：map_server 提供静态地图，amcl 做粒子滤波定位，controller_server 输出速度指令，planner_server 做全局路径规划，behavior_server 处理恢复行为。视觉系统要接入的是 costmap 的 observation source。具体来说，视觉节点发布一个sensor_msgs/PointCloud2或sensor_msgs/LaserScan话题，Nav2 的 obstacle layer 订阅这个话题，把障碍物信息写进局部代价地图。
这里的关键是坐标系。视觉节点输出的障碍物坐标必须转换到base_link或odom坐标系下，否则代价地图上会出现「幽灵障碍物」。常见做法是在视觉节点里用tf2_ros查询camera_link到base_link的变换，把检测结果变换过去再发布。如果相机是固定在云台上的，还要考虑云台角度变化对变换的影响。
2.3 最小可跑通的视觉节点代码与参数说明
下面是一个用 Python 写的视觉障碍物检测节点，它订阅摄像头图像，做简单的地面分割，把障碍物像素投影成PointCloud2发布出去。这段代码在 ROS2 Humble 下可以直接运行，前提是相机已经标定好，且camera_info话题有数据。
import rclpy from rclpy.node import Node from sensor_msgs.msg import Image, PointCloud2, PointField from cv_bridge import CvBridge import cv2 import numpy as np import struct class VisualObstacleNode(Node): def __init__(self): super().__init__('visual_obstacle_node') # 订阅原始图像和相机内参 self.sub_img = self.create_subscription(Image, '/camera/image_raw', self.img_cb, 10) self.sub_info = self.create_subscription(Image, '/camera/camera_info', self.info_cb, 10) # 发布障碍物点云，供 Nav2 obstacle layer 使用 self.pub_cloud = self.create_publisher(PointCloud2, '/visual_obstacles', 10) self.bridge = CvBridge() self.fx = self.fy = self.cx = self.cy = None # 地面分割的 ROI 区域，根据实际安装高度调整 self.roi_top = 0.55 # 图像高度比例，0 为顶部 self.roi_bottom = 0.95 self.distance_threshold = 2.0 # 只保留 2 米内的障碍物 def info_cb(self, msg): # 从 camera_info 中提取内参，只取一次 if self.fx is None: k = msg.k self.fx, self.fy = k[0], k[4] self.cx, self.cy = k[2], k[5] self.get_logger().info(f'Camera intrinsics: fx={self.fx}, fy={self.fy}') def img_cb(self, msg): if self.fx is None: return frame = self.bridge.imgmsg_to_cv2(msg, 'bgr8') h, w = frame.shape[:2] # 只处理地面附近的 ROI，减少天空和远处干扰 y1, y2 = int(h * self.roi_top), int(h * self.roi_bottom) roi = frame[y1:y2, :] gray = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY) # 自适应阈值提取暗色障碍物，实际场景可换成深度或语义分割 mask = cv2.adaptiveThreshold(gray, 255, cv2.ADAPTIVE_THRESH_MEAN_C, cv2.THRESH_BINARY_INV, 31, 10) contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) points = [] for cnt in contours: if cv2.contourArea(cnt) < 300: continue x, y, bw, bh = cv2.boundingRect(cnt) # 取障碍物底部中心作为接地点 u = x + bw / 2.0 v = y + bh + y1 # 假设地面平坦，用针孔模型反投影到相机坐标系 z = self.fy * 0.3 / (v - self.cy) if v > self.cy else 0.0 if z <= 0 or z > self.distance_threshold: continue x_cam = (u - self.cx) * z / self.fx y_cam = (v - self.cy) * z / self.fy points.append((x_cam, y_cam, z)) cloud = self.make_cloud(points, msg.header) self.pub_cloud.publish(cloud) def make_cloud(self, points, header): # 手动组装 PointCloud2，字段为 xyz fields = [ PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), ] data = [] for p in points: data.append(struct.pack('fff', *p)) cloud = PointCloud2() cloud.header = header cloud.height = 1 cloud.width = len(points) cloud.fields = fields cloud.is_bigendian = False cloud.point_step = 12 cloud.row_step = 12 * len(points) cloud.data = b''.join(data) return cloud def main(): rclpy.init() node = VisualObstacleNode() rclpy.spin(node) rclpy.shutdown() if __name__ == '__main__': main()这段代码的逻辑是：先拿相机内参，再对图像下半部分做自适应阈值分割，把暗色区域当成障碍物，然后用针孔模型反投影到相机坐标系，最后打包成PointCloud2发布。参数里roi_top和roi_bottom控制处理区域，装得低就往下调；distance_threshold限制最远检测距离，太大容易把远处噪声引进来。z = self.fy * 0.3 / (v - self.cy)里的 0.3 是假设相机离地高度，实际要按你的安装尺寸改。这个节点跑起来后，在 RViz2 里能看到障碍物点云，但别急着开导航，先确认点云位置和实际障碍物对得上。
3. 从零搭一套能跑的导航视觉系统：环境、标定与 launch 编排
3.1 ROS2 Humble 安装与工作空间初始化
Ubuntu 22.04 上装 ROS2 Humble，最稳的方式还是官方 apt 源。先设置 locale 和源，再装ros-humble-desktop。如果你在 Ubuntu 24.04 上，官方对应的是 Jazzy，但很多视觉包和 Nav2 插件在 Humble 上验证最充分，所以我一般会在 22.04 的 Docker 容器里做开发，宿主机用 24.04 也不影响。装完后记得 source 环境，并且把 source 命令写进.bashrc，否则每个新终端都要手动来一遍。
# 设置 locale，避免中文环境导致的编码问题 sudo apt update && sudo apt install locales sudo locale-gen en_US en_US.UTF-8 sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 # 添加 ROS2 apt 源 sudo apt install software-properties-common sudo add-apt-repository universe sudo apt update && sudo apt install curl -y sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null # 安装 desktop 版本，包含 RViz2、demo 节点和常用工具 sudo apt update sudo apt install ros-humble-desktop python3-colcon-common-extensions -y # 初始化工作空间 mkdir -p ~/nav_ws/src cd ~/nav_ws colcon build --symlink-install echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc echo "source ~/nav_ws/install/setup.bash" >> ~/.bashrccolcon build --symlink-install的好处是 Python 文件修改后不用重新编译，直接重启节点就生效，调试视觉算法时能省不少时间。工作空间建好后，把视觉节点包和 Nav2 配置包都放进src下，再统一编译。
3.2 相机标定与 TF 树配置
视觉导航翻车最多的地方就是标定和 TF。相机内参不准，反投影出来的距离就会系统性偏大或偏小；TF 树缺一环，RViz2 里点云直接飘到地图外面。标定用 ROS2 的camera_calibration包，打一张棋盘格，跑标定节点，移动棋盘格直到采样足够，然后保存 YAML。标定完成后，在 launch 文件里用camera_info发布内参，同时用static_transform_publisher发布base_link到camera_link的固定变换。
# 启动标定节点，--size 是棋盘格内角点数，--square 是格子边长（米） ros2 run camera_calibration cameracalibrator --size 8x6 --square 0.025 \ image:=/camera/image_raw camera:=/camera # 标定完成后保存 # 在标定界面点击 SAVE，生成 /tmp/calibrationdata.tar.gz # 解压后把 ost.yaml 放到相机配置目录，并在 launch 中加载TF 配置里，base_link到camera_link的平移量要拿尺子量准，旋转量用roll pitch yaw描述。如果相机有俯仰角，pitch不能填 0，否则点云会整体倾斜。发布静态 TF 的命令行方式适合快速验证，正式项目里建议写在 launch 文件里，用tf2_ros的static_transform_publisher节点。
3.3 Nav2 参数文件里视觉层的接入点
Nav2 的nav2_params.yaml里，局部代价地图的obstacle_layer和voxel_layer是视觉点云的入口。你需要把observation_sources指向视觉节点发布的话题，并设置data_type为PointCloud2。marking和clearing两个开关要同时打开，否则障碍物只增不减，机器人会以为路一直被堵着。raytrace_max_range和obstacle_max_range根据你的检测距离设，一般比视觉节点的distance_threshold略大一点。
local_costmap: local_costmap: ros__parameters: plugins: ["obstacle_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True observation_sources: visual_cloud visual_cloud: topic: /visual_obstacles max_obstacle_height: 1.0 clearing: True marking: True data_type: "PointCloud2" raytrace_max_range: 2.5 obstacle_max_range: 2.0 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" inflation_radius: 0.35 cost_scaling_factor: 3.0inflation_radius决定障碍物周围膨胀多大一圈，太小机器人会擦着走，太大窄通道过不去。cost_scaling_factor控制代价衰减速度，值越大衰减越快，机器人越敢靠近障碍物。这两个参数没有标准答案，得在 RViz2 里看代价地图的实际效果反复调。
4. 避坑与排查：视觉导航里那些让人怀疑人生的瞬间
4.1 点云在 RViz2 里位置对但导航时被忽略
现象是 RViz2 能看到/visual_obstacles点云，位置也正确，但局部代价地图上不显示障碍物，机器人直接撞上去。原因通常是observation_sources的话题名写错，或者data_type不匹配。Nav2 对话题名大小写敏感，/visual_obstacles和/Visual_Obstacles是两个话题。解决方法是先用ros2 topic list确认话题存在，再用ros2 topic echo /visual_obstacles --once看有没有数据，最后检查 YAML 里的topic字段是否和实际发布名完全一致。
4.2 机器人原地转圈不前进
现象是规划出了路径，但控制器输出一直让机器人旋转。原因多半是视觉点云把机器人自身周围也标成了障碍物，代价地图上机器人起点被高代价包围，控制器找不到可行方向。解决方法是检查视觉节点的 ROI 是否包含了机器人本体，或者点云的最小距离阈值设得太小。可以在视觉节点里加一个min_distance过滤，把 0.3 米以内的点丢掉，同时在 Nav2 的obstacle_layer里设置min_obstacle_height和max_obstacle_height，把地面和机器人本体排除掉。
4.3 光照变化导致障碍物时有时无
现象是白天跑得好好的，晚上开灯后障碍物检测全乱，或者地面反光被当成障碍物。原因是几何视觉对光照太敏感，自适应阈值的参数在光照变化后失效。解决方法是改用深度相机，或者在视觉节点里加自动曝光和伽马校正，再不行就上轻量语义分割模型，用可通行区域代替障碍物检测。如果坚持用几何视觉，至少要把阈值参数做成动态可调的 ROS2 参数，方便现场调。
4.4 TF 报错导致点云飘到地图外
现象是 RViz2 里点云位置和实际障碍物差了好几米，或者直接飞到天上。原因是camera_link到base_link的变换不对，或者时间戳不同步。解决方法是先用ros2 run tf2_tools view_frames生成 TF 树图，确认base_link到camera_link的链路完整。如果用了static_transform_publisher，检查平移和旋转参数是否和实际安装一致。时间戳问题可以在视觉节点里用message_filters做时间同步，或者把PointCloud2的 header 时间戳设成和图像一致。
4.5 导航到目标点附近开始震荡
现象是机器人接近目标点时左右摇摆，迟迟不停。原因是局部代价地图的膨胀层参数和控制器参数不匹配，机器人认为目标点被障碍物包围。解决方法是减小inflation_radius，或者调整控制器的xy_goal_tolerance和yaw_goal_tolerance，让机器人更容易判定到达。另外检查视觉点云是否在目标点附近产生了虚假障碍物，如果有，在视觉节点里加一个目标点附近的过滤区域。
5. 进阶技巧：用八叉树地图和动态参数把视觉导航调稳
5.1 八叉树地图做三维障碍物持久化
普通代价地图是二维的，视觉点云投影到地面后，矮障碍物和高障碍物看起来一样。如果场景里有悬空物体或桌面，二维地图会误判。八叉树地图（OctoMap）能把三维点云体素化，区分不同高度的障碍物。在 ROS2 里可以用octomap_server订阅视觉点云，生成三维栅格地图，再投影到二维代价地图。这样机器人能知道桌子下面可以走，桌面以上不能走。
# 安装 octomap_server sudo apt install ros-humble-octomap-server -y # 启动 octomap_server，订阅视觉点云 ros2 run octomap_server octomap_server_node --ros-args \ -p frame_id:=map \ -p resolution:=0.05 \ -p sensor_model.maxrange:=3.0 \ -r cloud_in:=/visual_obstaclesresolution是体素边长，0.05 米适合室内导航，太小会吃内存，太大丢失细节。sensor_model.maxrange要和视觉检测距离匹配，超过这个范围的点云不参与建图。生成的八叉树地图可以在 RViz2 里用octomap插件查看，确认障碍物高度分层是否正确。
5.2 动态参数调整与运行时调参
ROS2 支持运行时改参数，不用重启节点。调视觉导航时，我习惯用ros2 param set在线改 Nav2 的膨胀半径和控制器速度限制，看机器人反应。比如窄通道过不去，就把inflation_radius从 0.35 降到 0.25；机器人太激进，就把max_vel_x从 0.5 降到 0.3。改完参数后，用ros2 param dump把当前配置存下来，避免下次启动又回到默认值。
# 在线修改局部代价地图的膨胀半径 ros2 param set /local_costmap/local_costmap inflation_layer.inflation_radius 0.25 # 修改控制器最大线速度 ros2 param set /controller_server FollowPath.max_vel_x 0.3 # 把当前所有参数保存到文件 ros2 param dump /local_costmap/local_costmap > local_costmap_params.yaml5.3 用 ros2 bag 录数据做离线复现
视觉导航的问题往往在现场一闪而过，事后很难复现。我一般会在机器人跑的时候用ros2 bag record把图像、点云、里程计、TF 和速度指令全录下来，回到工位再回放。回放时启动 Nav2 和 RViz2，用ros2 bag play喂数据，就能反复观察问题出现的瞬间。录数据时注意用--max-cache-size限制内存占用，长时间跑要分段录。
# 录制导航相关话题，输出到指定目录 ros2 bag record -o nav_debug \ /camera/image_raw \ /visual_obstacles \ /odom \ /tf \ /tf_static \ /cmd_vel \ /local_costmap/costmap # 回放时用 --clock 发布仿真时间，保证 TF 时间戳一致 ros2 bag play nav_debug --clock回放前记得把use_sim_time参数设为true，否则 TF 会报时间戳不匹配。这套流程跑熟之后，调参效率会高很多，不用每次都把机器人搬到现场。
我自己的习惯是：每次改完视觉节点或 Nav2 参数，先录一段 bag，在离线环境里确认没问题再上实车。这样翻车的次数少了很多，也省了反复搬机器人的体力。希望帮到你。
本文还有配套的精品资源，点击获取
