视觉与三维建图
AprilTag 视觉标签识别
AprilTag 是一种视觉基准标签，由已知编码的黑白图案组成。摄像头拍到标签后，程序可以识别标签 ID，并根据图案角点计算标签在图像中的位置。它常用于视觉识别测试、目标标记、简单交互控制、定位辅助和自动对接等场景。
基础测试只使用 tag36h11 ID 0。先用手机或平板显示标签，确认摄像头画面中能看到完整黑白图案和外侧白边。
手机或平板显示标签时，调高亮度，避免反光，不要裁掉外侧白边。从 30 cm ~ 80 cm 距离开始测试。进行姿态估计、跟踪或控制测试时，使用打印版，按 100% 比例打印并固定在平整纸板上。
有效黑白图案区域才是 AprilTag 姿态估计使用的 tag size；外侧白边和 A4 纸张空白不计入 tag size。如果参数使用 tag size 0.08，打印时让标签有效黑白图案区域接近 80 mm。
- 启动摄像头。在 Docker ROS 2 终端中执行：
cd /home/ws/ugv_ws
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 launch ugv_vision camera.launch.py
保持摄像头终端运行，不要关闭。
- 确认图像。另开 Docker ROS 2 终端检查图像 topic：
cd /home/ws/ugv_ws
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 topic list | grep -E "image|camera|rect"
timeout 5 ros2 topic hz /image_raw
timeout 5 ros2 topic hz /image_raw 会等待约 5 秒。如果看到 average rate，说明 /image_raw 正在发布图像。若一直没有频率输出，说明摄像头节点没有发布图像或摄像头 launch 已退出；先回到摄像头终端检查日志，必要时按 Ctrl + C 停止后重新运行 ros2 launch ugv_vision camera.launch.py。
确认 /image_raw 有频率输出后，继续打开图像查看器：
ros2 run rqt_image_view rqt_image_view
在 rqt_image_view 中选择 /image_raw。能看到 USB 摄像头画面后，再继续下一步启动识别节点。
- 添加安全识别节点。
apriltag_detect_only是本教程用于基础识别的安全节点。该节点只订阅图像、识别 ID、发布结果图像，不调用behavior，也不控制底盘。
另开一个 Docker ROS 2 终端，在设备端工作区执行下面命令。该命令会写入安全识别节点，并把 apriltag_detect_only 加入 ugv_vision 的可执行入口。
cd /home/ws/ugv_ws
mkdir -p src/ugv_main/ugv_vision/ugv_vision
cat > src/ugv_main/ugv_vision/ugv_vision/apriltag_detect_only.py <<'PYFILE'
import cv2
import rclpy
from apriltag import apriltag
from cv_bridge import CvBridge
from rclpy.node import Node
from sensor_msgs.msg import Image
class ApriltagDetectOnly(Node):
def __init__(self):
super().__init__('apriltag_detect_only')
self.declare_parameter('image_topic', '/image_raw')
self.declare_parameter('result_topic', '/apriltag_detect_only/result')
self.declare_parameter('tag_family', 'tag36h11')
self.declare_parameter('draw_result', True)
self.declare_parameter('print_detection', True)
self.declare_parameter('print_interval_sec', 0.5)
image_topic = self.get_parameter('image_topic').value
result_topic = self.get_parameter('result_topic').value
tag_family = self.get_parameter('tag_family').value
self.draw_result = self.get_parameter('draw_result').value
self.print_detection = self.get_parameter('print_detection').value
self.print_interval_sec = float(self.get_parameter('print_interval_sec').value)
self.last_print_time = 0.0
self.bridge = CvBridge()
self.detector = apriltag(tag_family)
self.image_sub = self.create_subscription(Image, image_topic, self.image_callback, 10)
self.result_pub = self.create_publisher(Image, result_topic, 10)
self.get_logger().info(f'Input image topic: {image_topic}')
self.get_logger().info(f'Result image topic: {result_topic}')
self.get_logger().info(f'AprilTag family: {tag_family}')
def image_callback(self, msg):
frame = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
results = self.detector.detect(gray)
now_sec = self.get_clock().now().nanoseconds / 1e9
for result in results:
tag_id = int(result['id'])
center_x = int(result['center'][0])
center_y = int(result['center'][1])
if self.draw_result:
corners = result['lb-rb-rt-lt'].astype(int)
cv2.polylines(frame, [corners], isClosed=True, color=(0, 255, 0), thickness=2)
cv2.circle(frame, (center_x, center_y), 5, (0, 0, 255), -1)
cv2.putText(
frame,
f'ID {tag_id} ({center_x}, {center_y})',
(center_x + 8, center_y - 8),
cv2.FONT_HERSHEY_SIMPLEX,
0.5,
(0, 255, 0),
2,
)
if self.print_detection and now_sec - self.last_print_time >= self.print_interval_sec:
print(f'Tag ID: {tag_id}, Center: ({center_x}, {center_y})')
self.last_print_time = now_sec
result_msg = self.bridge.cv2_to_imgmsg(frame, encoding='bgr8')
self.result_pub.publish(result_msg)
def main(args=None):
rclpy.init(args=args)
node = ApriltagDetectOnly()
try:
rclpy.spin(node)
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
PYFILE
python3 - <<'PYSETUP'
from pathlib import Path
p = Path("src/ugv_main/ugv_vision/setup.py")
text = p.read_text()
entry = "apriltag_detect_only = ugv_vision.apriltag_detect_only:main"
if entry not in text:
old = " 'apriltag_track_2 = ugv_vision.apriltag_track_2:main'\n"
new = (
" 'apriltag_track_2 = ugv_vision.apriltag_track_2:main',\n"
" 'apriltag_detect_only = ugv_vision.apriltag_detect_only:main'\n"
)
if old not in text:
raise SystemExit("未找到 apriltag_track_2 入口，请手动检查 setup.py")
p.write_text(text.replace(old, new))
print("apriltag_detect_only entry is ready")
PYSETUP
- 重新构建
ugv_vision并检查入口：
cd /home/ws/ugv_ws
source /opt/ros/humble/setup.bash
source install/setup.bash
colcon build --packages-select ugv_vision --symlink-install
source install/setup.bash
ros2 pkg executables ugv_vision | grep apriltag
重新检查时应能看到 ugv_vision apriltag_detect_only。
启动安全识别节点
-
启动安全识别节点。保持摄像头终端继续运行，另开一个 Docker ROS 2 终端执行下面命令：
cd /home/ws/ugv_wssource /opt/ros/humble/setup.bashsource install/setup.bashros2 run ugv_vision apriltag_detect_only
检查 AprilTag ID 0 识别结果
-
查看识别结果。将
tag36h11ID 0 标签完整对准摄像头，保持黑白图案和外侧白边都在画面中。先从30 cm ~ 80 cm距离开始，避免反光、遮挡和过度倾斜。识别运行后，终端会先输出输入图像、结果图像和标签家族信息，随后持续打印识别到的标签 ID 和中心坐标：
[INFO] [apriltag_detect_only]: Input image topic: /image_raw[INFO] [apriltag_detect_only]: Result image topic: /apriltag_detect_only/result[INFO] [apriltag_detect_only]: AprilTag family: tag36h11Tag ID: 0, Center: (369, 291)Tag ID: 0, Center: (318, 289)Tag ID: 0, Center: (322, 278)Tag ID: 0, Center: (335, 280)Tag ID: 0, Center: (369, 296)Tag ID: 0, Center: (371, 295)Tag ID: 0, Center: (382, 294)Tag ID: 0, Center: (388, 294)Tag ID: 0, Center: (380, 270)Tag ID: 0, Center: (448, 190)识别结果中会看到：
- 终端输出
Tag ID: 0； - 移动标签时，
Center坐标变化； - 节点会发布
/apriltag_detect_only/result结果图像 topic； - 遮挡、反光、距离过远或图案过小时，识别可能中断；
- 不出现
Action server not available!； - UGV 不运动。
- 终端输出
需要更多 ID 或不同尺寸时，使用标准 AprilTag 生成器或 AprilTag 官方软件页面 提供的资源生成 tag36h11 家族标签。
OAK-D Lite 基础体验
OAK-D Lite 是一款 RGB-D 深度相机，可以同时提供彩色图像和深度图像。RTAB-Map 3D 建图会使用 OAK-D Lite 的 RGB 图像、深度图像和相机参数，再结合雷达、里程计和 TF 生成地图数据库。
本节只用于确认 OAK-D Lite 数据是否正常。后续 RTAB-Map 会自动启动 OAK-D Lite，不需要再单独启动相机。
-
在 Docker ROS 2 终端中启动 OAK-D Lite。
cd /home/ws/ugv_wssource /opt/ros/humble/setup.bashsource install/setup.bashros2 launch ugv_vision oak_d_lite.launch.py -
保持 OAK-D Lite 终端运行，另开 Docker ROS 2 终端检查 topic。
cd /home/ws/ugv_wssource /opt/ros/humble/setup.bashsource install/setup.bashros2 topic list | grep -Ei "oak|rgb|depth|image|camera_info" -
按实际 topic 检查频率。
timeout 8 ros2 topic hz /oak/rgb/image_recttimeout 8 ros2 topic hz /oak/stereo/image_rawtimeout 8 ros2 topic hz /oak/rgb/camera_info如果 topic 名称不同，以
ros2 topic list的实际输出为准。 -
需要查看 RGB 图像时，可以打开
rqt_image_view。ros2 run rqt_image_view rqt_image_view在窗口中选择
/oak/rgb/image_rect，或选择当前实际存在的 RGB 图像 topic。
可以看到 OAK-D Lite 相关 topic；RGB 图像 topic 和 Depth 图像 topic 有频率输出；camera_info topic 存在；rqt_image_view 中可以选择 RGB 图像 topic；终端没有持续出现设备找不到、DepthAI 启动失败或 USB 错误。
OAK-D Lite 单独体验结束后，在 launch 终端按 Ctrl + C 停止。后续启动 RTAB-Map 时，会由 rtabmap_rgbd.launch.py 自动启动 OAK-D Lite，避免重复启动。
RTAB-Map 三维建图与定位
RTAB-Map 在这里用于 RGB-D 建图和定位。它会结合 OAK-D Lite 的 RGB-D 图像、雷达 /scan、底盘 /odom 和 TF，生成 RTAB-Map 数据库。建图结果默认保存在 ~/.ros/rtabmap.db。
RTAB-Map 建图不是自动导航。建图阶段负责采集环境并生成数据库；导航目标点和路径规划属于 Nav2 等导航栈的功能。
RTAB-Map RGB-D 建图
启动 RTAB-Map 后，可能同时看到 RViz 和 RTAB-Map 自带窗口。RViz 适合观察机器人模型、LaserScan、二维地图、点云地图和图结构；RTAB-Map 自带窗口适合观察 RGB 图像、Odometry 图像、3D Map、关键帧 ID 和回环检测状态。
rtabmap_rgbd.launch.py 会启动底盘与雷达 bringup、OAK-D Lite、robot_pose_publisher、rtabmap_slam/rtabmap 和 rtabmap_viz。RTAB-Map 输入 topic 会映射到 OAK-D Lite 的 RGB 图像、深度图像和相机参数。
| RTAB-Map 输入 | OAK-D Lite topic |
|---|---|
rgb/image | oak/rgb/image_rect |
rgb/camera_info | oak/rgb/camera_info |
depth/image | oak/stereo/image_raw |
-
在 Docker ROS 2 终端加载 ROS 2 环境。
cd /home/ws/ugv_wssource /opt/ros/humble/setup.bashsource install/setup.bash -
建图前备份旧数据库。
ls -lh ~/.ros/rtabmap.dbcp ~/.ros/rtabmap.db ~/.ros/rtabmap_backup_$(date +%Y%m%d_%H%M%S).db 2>/dev/null || true当前建图模式会重新生成默认数据库。需要保留旧地图时，先备份
~/.ros/rtabmap.db。 -
设置型号并启动 RTAB-Map RGB-D 建图。
export UGV_MODEL=ugv_roverexport LDLIDAR_MODEL=ld19ros2 launch ugv_slam rtabmap_rgbd.launch.py use_rviz:=true本教程统一显式使用
use_rviz:=true。实际运行时可能同时打开 RViz 和 RTAB-Map 自带窗口，可以保留两个窗口进行对比观察。 -
保持 RTAB-Map 终端运行，另开 Docker ROS 2 终端检查输入数据。
cd /home/ws/ugv_wssource /opt/ros/humble/setup.bashsource install/setup.bashros2 node list | grep -Ei "rtab|oak|depth|rgb|camera|ugv|driver|base|lidar|pose"ros2 topic list | grep -Ei "rtab|oak|rgb|depth|image|camera_info|odom|scan|tf|cloud|map"检查 RTAB-Map 使用的输入 topic。
timeout 8 ros2 topic hz /oak/rgb/image_recttimeout 8 ros2 topic hz /oak/stereo/image_rawtimeout 8 ros2 topic hz /oak/rgb/camera_infotimeout 8 ros2 topic hz /scan如果 topic 名称不同，以当前
ros2 topic list的实际输出为准。 -
另开 Docker ROS 2 终端，用键盘低速移动采集数据。
cd /home/ws/ugv_wssource /opt/ros/humble/setup.bashsource install/setup.bashros2 run ugv_tools keyboard_ctrl短按
J/L小幅转向；短按I/,小幅前后移动；每次动作后按K或空格停止。不要快速原地旋转，不要长时间连续运动。建议先采集1到2分钟的小范围地图。
RViz 中 Fixed Frame 为 map，Global Status 为 Ok；RobotModel 正常显示；LaserScan 随环境变化；Map 或 MapCloud 随机器人移动逐渐增长；MapGraph 或轨迹随移动增加；终端没有持续 TF、No data、RGB-D sync 报错。
RTAB-Map 自带窗口中 RGB 图像刷新；Odometry 图像刷新；右侧 3D Map 出现点云；New ID 数字随着采集增加；下方日志持续出现地图更新信息；回到旧区域附近时，可以观察 Loop closure 相关变化。
结束建图时，先在 keyboard_ctrl 终端按 K 或空格停止底盘，再按 Ctrl + C 停止键盘控制。回到 RTAB-Map launch 终端按 Ctrl + C，等待数据库写入完成。
ls -lh ~/.ros/rtabmap.db
~/.ros/rtabmap.db 存在且文件大小不是 0 时，表示数据库已经写入。重新定位前不要随意删除该文件。
RTAB-Map 定位
RTAB-Map 定位用于加载已有数据库，不是重新建图。启动前确认已经完成 RTAB-Map 建图，~/.ros/rtabmap.db 存在，并尽量在与建图时相近的环境中测试。
cd /home/ws/ugv_ws
source /opt/ros/humble/setup.bash
source install/setup.bash
export UGV_MODEL=ugv_rover
export LDLIDAR_MODEL=ld19
ls -lh ~/.ros/rtabmap.db
ros2 launch ugv_slam rtabmap_rgbd.launch.py localization:=true use_rviz:=true
定位模式同样显式使用 use_rviz:=true。启动日志不应持续提示找不到数据库、No data received、TF 报错或 RGB-D synchronization failed。机器人小幅移动后应仍在已有地图范围内保持定位，地图不应完全从零重新生成。
需要小幅移动验证时，另开 Docker ROS 2 终端运行键盘控制。
cd /home/ws/ugv_ws
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 run ugv_tools keyboard_ctrl
只做小幅转向或短距离移动，测试 30 秒到 1 分钟即可。测试结束前先按 K 或空格停止底盘，再退出键盘控制和 RTAB-Map 定位终端。