ros2单目python实现基于AprilTag空间定位最简全流程
文章目录
- 前言
- 驱动安装
- 配置文档说明
- 相机内参
- 相机采图配置
- 编译运行
- launch文件说明
- 启动AprilTag识别launch文件
- 启动相机launch文件
- 实际运行
前言
在前一篇文章内写了如何实现ros2 AprilTag定位的最简全流程1，不过那篇文章是c++实现的AprilTag的节点，本文使用Python实现全流程，更简单，配置文件也做了json解析实现，便于改变参数。
AprilTag是一种高性能的视觉标记系统，广泛应用于机器人导航、增强现实和自动化领域。类似于二维码，AprilTag标记由黑白图案组成，但其设计旨在提供更高的精度和更强的鲁棒性。通过相机识别和解码这些标记，可以实现精确的位置和姿态估计，从而使机器人或计算机视觉系统能够准确地感知和定位自身在环境中的位置。AprilTag因其高效性和可靠性，在现代机器人技术和自动化应用中扮演着重要角色。
本篇文章主要做了该套代码整体实现，整体架构如下：
本项目源代码在gitee2可以找到。
项目整体文件结构如下：
April_tag/
├── apriltag_usb_cam
│ ├── apriltag_app
│ │ ├── apriltag_app
│ │ │ ├── configs.py
│ │ │ ├── __init__.py
│ │ │ └── main.py
│ │ ├── package.xml
│ │ ├── resource
│ │ │ └── apriltag_app
│ │ ├── setup.cfg
│ │ ├── setup.py
│ │ └── test
│ │ ├── test_copyright.py
│ │ ├── test_flake8.py
│ │ └── test_pep257.py
│ ├── apriltag_bringup
│ │ ├── CMakeLists.txt
│ │ ├── launch
│ │ │ ├── app_launch
│ │ │ │ └── app.launch.py
│ │ │ └── node_launch
│ │ │ ├── apriltag_app.launch.py
│ │ │ ├── camera_config.py
│ │ │ └── camera.launch.py
│ │ └── package.xml
│ ├── apriltag_interface
│ │ ├── CMakeLists.txt
│ │ ├── msg
│ │ │ ├── AprilTagDetectionArray.msg
│ │ │ └── AprilTagDetection.msg
│ │ ├── package.xml
│ │ └── srv
│ │ └── AprilTagDetect.srv
│ ├── apriltag_setup
│ │ ├── CMakeLists.txt
│ │ ├── config
│ │ │ ├── camera
│ │ │ │ ├── camera_info_1.yaml
│ │ │ │ └── params_1.yaml
│ │ │ └── vision
│ │ │ └── tags_36h11_all.json
│ │ └── package.xml
│ ├── LICENSE
│ ├── README.en.md
│ ├── README.md
│ ├── requirements.txt
│ └── scripts
│ ├── install_pydeps.sh
│ └── install_usb_cam.sh
├── calibrationdata
│ └── calibrationdata.tar.gz
└── lib└── python3.10└── site-packages
驱动安装
博主写了一个脚本便于大家安装：scripts/install_usb_cam.sh
# scripts/install_usb_cam.sh
sudo apt-get update
sudo apt-get install -y \
ros-${ROS_DISTRO}-usb-cam \
ros-${ROS_DISTRO}-camera-calibration \
ros-${ROS_DISTRO}-camera-calibration-parsers \
ros-${ROS_DISTRO}-camera-info-manager \
ros-${ROS_DISTRO}-rqt-image-view \
ros-${ROS_DISTRO}-cv-bridge \
ros-${ROS_DISTRO}-image-geometry \
ros-${ROS_DISTRO}-launch-testing-ament-cmake
pip需要进入特定目录执行，具体如下：
cd apriltag_usb_cam
./scripts/install_pydeps.sh
pip相关依赖安装：scripts/install_pydeps.sh
#!/bin/sh -epip3 install -r requirements.txt -t ../lib/python3.10/site-packages# pip3 install --no-deps -r requirements.txt -t ../lib/python3.10/site-packages -i https://pypi.mirrors.ustc.edu.cn/simple/配置python环境变量，其中/home/dev/workspace/换为自己的本地路径
export PYTHONPATH=/home/dev/workspace/April_tag/lib/python3.10/site-packages:${PYTHONPATH}
配置文档说明
config/
├── camera
│ ├── camera_info_1.yaml # 存放相机内参，usbcam使用
│ └── params_1.yaml # 存放相机采图配置，usbcam使用
└── vision└── tags_36h11_all.json # 存放AprilTag相关参数
相机内参
下述数据是ros2 run camera_calibration cameracalibrator跑出来的，具体可以参考前一篇文章。
image_width: 1280
image_height: 720
camera_name: narrow_stereo
camera_matrix:rows: 3cols: 3data: [1264.85961, 0. , 710.17684,0. , 1266.78899, 335.77006,0. , 0. , 1. ]
distortion_model: plumb_bob
distortion_coefficients:rows: 1cols: 5data: [0.053912, -0.015197, -0.007168, -0.004231, 0.000000]
rectification_matrix:rows: 3cols: 3data: [1., 0., 0.,0., 1., 0.,0., 0., 1.]
projection_matrix:rows: 3cols: 4data: [1289.90588, 0. , 704.04374, 0. ,0. , 1292.6897 , 330.71303, 0. ,0. , 0. , 1. , 0. ]相机采图配置
这里固化了相机的设备名为/dev/camera_gf100，如何固定可以参考：Linux（Ubuntu 22.04）系统中固定串口、usb相机等usb接入外部设备_固定串口设备-CSDN博客
/**:ros__parameters:video_device: "/dev/camera_gf100"framerate: 30.0io_method: "mmap"frame_id: "camera"pixel_format: "mjpeg2rgb" # see usb_cam/supported_formats for list of supported formatsav_device_format: "YUV422P"image_width: 1280image_height: 720camera_name: "camera"camera_info_url: "package://apriltag_setup/config/camera/camera_info_1.yaml"brightness: -1contrast: -1saturation: -1sharpness: -1gain: -1auto_white_balance: truewhite_balance: 4000autoexposure: trueexposure: 100autofocus: falsefocus: -1
这里可以配置你的相机相关参数，如分辨率，帧率，曝光，fram_id等，用于连接相机及后续发布topic数据。
编译运行
切换到特定路径：
cd /home/dev/workspace/April_tag
colcon build --packages-up-to apriltag_bringup --cmake-args -DCMAKE_BUILD_TYPE=Release
launch文件说明
可一次启动整个app，ros2 launch apriltag_bringup app.launch.py
apriltag_bringup/
├── CMakeLists.txt
├── launch
│ ├── app_launch
│ │ └── app.launch.py # 同时启动相机和AprilTag识别
│ └── node_launch
│ ├── apriltag_app.launch.py # 启动AprilTag识别
│ ├── camera_config.py # 配置 remaping usbcamera 移植自ros-${ROS_DISTRO}-usb-cam
│ └── camera.launch.py # 启动usbcamera 移植自ros-${ROS_DISTRO}-usb-cam
└── package.xml
启动AprilTag识别launch文件
可单独启动ros2 launch apriltag_bringup apriltag_app.launch.py
import osfrom ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (OpaqueFunction,Shutdown,
)from launch_ros.actions import Nodedef launch_setup(context, *args, **kwargs):setups_dir = get_package_share_directory("apriltag_setup") # 配置文件所在pkgsetup_dir = os.path.join(setups_dir, "config") # 配置文件结构config_file = os.path.join(setup_dir, "vision/tags_36h11_all.json") # 想要加载的AprilTag族相关参数apriltag_app_node = Node(package="apriltag_app",executable="apriltag_app",arguments=[config_file,],output="full",emulate_tty=True,on_exit=Shutdown(),)return [apriltag_app_node,]def generate_launch_description():return LaunchDescription([OpaqueFunction(function=launch_setup),])启动相机launch文件
可单独启动ros2 launch apriltag_bringup camera.launch.py
# Copyright 2018 Lucas Walter
# All rights reserved.
#
# Software License Agreement (BSD License 2.0)
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of Lucas Walter nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.import argparse
import os
from pathlib import Path # noqa: E402
import sys# Hack to get relative import of .camera_config file working
dir_path = os.path.dirname(os.path.realpath(__file__))
sys.path.append(dir_path)from camera_config import CameraConfig, USB_CAM_DIR # noqa: E402from launch import LaunchDescription # noqa: E402
from launch.actions import GroupAction # noqa: E402
from launch_ros.actions import Node # noqa: E402CAMERAS = []
CAMERAS.append(CameraConfig(name='camera1', # 这里配置发布topic时camera的名字param_path=Path(USB_CAM_DIR, 'config', 'camera', 'params_1.yaml') # 配置相机连接及发布的参数)# Add more Camera's here and they will automatically be launched below
)def generate_launch_description():ld = LaunchDescription()parser = argparse.ArgumentParser(description='usb_cam demo')parser.add_argument('-n', '--node-name', dest='node_name', type=str,help='name for device', default='usb_cam')camera_nodes = [Node(package='usb_cam', executable='usb_cam_node_exe', output='screen',name=camera.name,namespace=camera.namespace,parameters=[camera.param_path],remappings=camera.remappings)for camera in CAMERAS]camera_group = GroupAction(camera_nodes)ld.add_action(camera_group)return ld实际运行
同样实现了tf发布，不过由原版的持续跟踪，修改为呼叫后更新，更贴近博主实际使用需求，所以rviz中显示一段时间后tf会消失。
ROS2 2D相机基于AprilTag实现3D空间定位最简流程-CSDN博客 ↩︎
AprilTag usb cam: 本文用来记录全套配置usb相机如何实现连接，标定，apriltag空间定位全流程。 (gitee.com) ↩︎