本文主要讲解如何在宇树机器狗上使用 OpenCV 调用摄像头并进行 Apriltag 识别，再通过运动 sdk 实现位姿调整。

一、前期准备

宇树 Go1 机器狗。
ROS 基础使用及话题通信机制基础了解，可以通过文字教程和视频教程进行相关知识的学习。

二、调用相机发布话题功能包（camera_move）的创建、配置和代码编写

创建工作空间（workspace，可以自定义名称，这里用缩写 wksp），并初始化
打开命令行，输入：

1
2
3

mkdir -p  自定义空间名称（以 wksp 为例，下文不再提及）/src
cd wksp
catkin_make

进入 src 创建 ros 功能包并添加依赖
命令行输入：

1 2	cd src catkin_create_pkg 自定义 ros 包名（这里名字为 camera_move，后文不再强调） roscpp rospy std_msgs cv_bridge image_transport sensor_msgs apriltag_ros

这里的 roscpp、rospy、std_msgs 是不同的库：

roscpp 是使用 C++实现的库

cospy 是使用 python 实现的库

std_msgs 是标准信息库

cv_bridge 用于提供 ros 和 opencv 之间的接口，我们用 opencv 获取图像

image_transport 用来发布和接收图像

sensor_msgs 是一种消息类型

apriltag_ros 是识别 aptiltag 码的 ros 包

进入 src 目录编辑源文件（C++代码进入 src，python 文件需要另外创建 script 文件夹）

命令行输入：

1 2	cd .. 退回上级目录 wksp code . 打开 vscode

点击信任文件夹

在左侧文件管理器 src/camera_move/src 目录处右键，创建 cpp 文件（文件名为 camera_pub，用于实现相机图像信息发布

注意 cpp 后缀

编写代码如下，链接打开为代码，解析在注释中：
camera_pub.cc 代码：Ubuntu Pastebin

配置 CmakeLists.txt 文件夹：

140 行左右：
add_executable(camera_pub src/camera_pub.cpp)
154 行左右：
target_link_libraries(camera_pub
${catkin_LIBRARIES}
)

编译运行：

1	catkin_make

问题 1：若出现报错，提示 apriltag_ros 功能包没有安装，如图：

则安装 apriltag_ros 包，将源码拷贝到 ros 工作空间中的 src 目录下：

1	git clone https://github.com/AprilRobotics/apriltag\_ros.git #在 src 文件夹内

若一直 clone 失败，注意检查机器狗对应板卡是否插入网卡，再通过 ifconfig 查看 eth0 是否存在，若存在，使用 sudo ifconfig eth0 down 命令关闭 eth0（密码 123），再重试 git clone 命令，可以尝试先 ping github.com，ping 通后，再进行 git clone
如果一直访问 github 网址失败，也可以直接下载压缩包，U 盘传入再解压；
成功后，工作空间下的 src 目录会多出一个 apriltag_ros 功能包；
这时重新编译：

1	catkin_make

问题 2：若出现类似的缺少功能包问题，下载需要的功能包即可

1	sudo apt-get install ros-melodic-apriltag

使用 apt-get 需要机器狗联网，否则需要上 github 搜索对应功能包，用压缩包的方式安装，若出现类似套娃式的依赖包缺失，可以先

1	sudo apt-get install aptitude

然后用 aptitude 代替 apt-get 下载，这个工具会帮助下载所有需要的依赖包

问题 3：若出现类似的 opencv 路径问题，是因为 NVIDIA 的 32.3.1image 自带的是 opencv4 版本，把 opencv 命名成了 opencv4

解决办法可以根据博客：Project ‘cv_bridge‘ specifies ‘/usr/include/opencv‘ as an include dir, which is not found.的解决方法_project ‘cv_bridge’ specifies ‘/usr/include/opencv-CSDN 博客
找到对应的文件，修改 opencv 为 opencv4，这是让功能包适应系统环境
也可以如博客内评论所说，将 opencv 的命名软连接到 opencv4 上，这是修改系统环境，可以适配大部分功能包

问题 4：会发现之前的 apriltag_ros 功能包成功通过编译，但在宇树相机部分出现错误，这是因为没有配置好宇树相关的 sdk 文件

将相机 sdk/include 中的三个文件复制进 camera_move 功能包的 include 中
将相机 sdk 的 lib 文件夹复制进 camera_move 功能包中
最后根据相机 sdk 中的 CMakeLists.txt 配置功能包中的 CMakeLists.txt：
复制蓝字部分
取消 include 的注释
在这后面增加${SDKLIBS}
最后成功编译
这里提示的 warning 暂且不管，先尝试运行，发现失败：

问题 5：这是 cv_bridge 与 opencv4 版本冲突导致的，解决办法是将 cv_bridge 和自己功能包的依赖设为同一版本的 opencv，这里参考的是这篇文章，写得很详细：

解决 cv_bridge 与 opencv4 版本冲突问题_Thoven 的博客-CSDN 博客

先下载 noetic 版本的 cv_bridge（狗上的 ubuntu 是 melodic 版，但不影响，noetic 版本的 cv_bridge 经过一定修改后在 melodic 上也可以使用）
在 https://github.com/ros-perception/vision_opencv/tree/noetic 处下载，解压缩后将里面的 cv_bridge 文件夹拷贝到工作空间的 src 文件夹下，并将这个包重命名为 cv_bridge_new，以防混淆
同时修改 cv_bridge_new 功能包中 package.xml 和 CMakeLists.txt 中的包名
在 cv_bridge_new 中的 CMakeLists.txt 中指定为 camera_move 的 CMakeLists.txt 中指定的 opencv4 路径
注意，一般都存放在/usr/local/share/opencv4 中，可以通过

1	sudo find / -iname “\opencv4\”

查找确认，这里 opencv4 的子目录没有 build 文件夹，因此省略后面的路径
然后在工作空间编译，会出现错误：
需要在 CMakeLists.txt 文件中修改 python37 为 python3

再编译出现第二个错误：

直接修改 cv_bridge_new/src/module.hpp 文件下 void* do_numpy_import()函数如下：
最后编译成功：
最后一步，在 camera_move 功能包中的 CMakeList.txt 和 package.xml（有三个地方需要修改）中修改 cv_bridge 名字：

问题 6：如果运行时一直出现 invalid devicenode 错误，反复检查配置文件都没有问题，检查是否添加了 stereo_camera_config.yaml 文件到 wksp 目录下。

因为是在 wksp 路径处进行的 rosrun，因此代码中寻找这个文件时，会以 wksp 路径作为源路径，所以这个 yaml 文件会在 wksp 中找，一旦找不到就会程序崩溃。

最后成功启动得到的画面：
这样相机的图像就能获取到了。

三、Apriltag 信息接收功能包（detection_sub）的创建、配置和代码编写

在 src 目录创建第二个 cpp 文件：detection_sub，用于接收 apriltag_ros 识别完 tag 后发出的话题，并且进行运动控制：

代码如下，解析在注释中：
detection_sub 代码：Ubuntu Pastebin

之后需要根据宇树运动 sdk 配置 CMakeList.txt 文件：
然后将需要的 lib 等文件添加，同样在 sdk 中寻找：
最后 catkin_make 编译成功：
用 rosrun camera_move detection_sub 尝试运行，若成功运行会出现：

四、Launch 文件一键启动（start.launch）的创建、配置和代码编写

接下来编写一个 launch 文件用于一次性启动三个程序，camera_pub，apriltag_tag 识别，detection_sub：

先在 sksp 路径下创建一个 launch 文件夹，再在文件夹里创建一个 start.launch 文件，编写：

意思是按顺序启动三个程序，这里需要注意，用 launch 文件启动时，启动路径与 rosrun 不同，因此之前设置的 yaml 文件路径最好修改成绝对路径：

最后需要配置 apriltag_ros 中关于标签的配置信息，用于识别到规定好的标签：
编译后运行 launch 文件：
roslaunch camera_move start.launch
就能启动所有程序，实现根据 apriltag 识别结果进行位姿调整。

五、总结

结果：

项目最终能够成功通过 apriltag 让机器狗调整位姿，通过参数控制可以保持规定好的距离、角度，且准确度不错，并且延迟在可接受范围内。
不足：受到机器狗本身相机的分辨率限制，5cm 的 apriltag 码识别范围只在 1m 左右；另外，狗的位姿调节方式，即运动控制还存在一定问题，例如向右平移和向右转向的速度没有达到预期效果，pid 的效果也并不明显。

改进：

研究清楚机器狗对向右方向的运动存在什么问题，尝试解决；在项目的基础上仔细调参，使 pid 真正发挥作用，让位姿调节的时间达到最低，最终能够应用在集群控制上。

neckprotecter

基于Apriltag实现位姿调整

目录

一、前期准备

二、调用相机发布话题功能包（camera_move）的创建、配置和代码编写

三、Apriltag 信息接收功能包（detection_sub）的创建、配置和代码编写

四、Launch 文件一键启动（start.launch）的创建、配置和代码编写

五、总结