基于ROS2和YOLOv5的宇树Go2机器狗人脸表情识别与情感交互系统:开发血泪史-编程阁

文章目录

前言
1.硬件准备与环境搭建
- 1.1 网络环境搭建
- - 1.1.1 实机
  - 1.1.2 虚拟机
- 1.2 ROS2安装
- 1.3宇树ROS2必要环境安装
2.YOLO人脸表情识别
- 2.1摄像头问题和解决方案
- 2.2 信息包和识别包创建
3 情感交互系统
- 3.1 表情控制系统
- 3.2 语音控制系统
总结

前言

今年本科毕业设计做了个宇树Go2机器狗的人脸表情识别与情感交互系统。代码是自己一行行写的，也踩了不少坑，这里把做得还行的部分整理出来，供对四足机器人+ROS2感兴趣的同学参考，请大佬们不要嫌弃我菜。

1.硬件准备与环境搭建

买好亚博智能USB WiFi网卡，这很重要。不然狗网络都连不上。
系统最好是ubuntu20.04（和狗相同）
网线用来和狗内网连接调试。
最早我是在狗内置电脑进行开发，但是后面调试麻烦就采用网线调试了，所以YOLO部分的狗内置电脑开发大家可以参考下面的文章。

https://blog.csdn.net/QshiyiI/article/details/143890569

1.1 网络环境搭建

用狗第一步打开宇树文档中心，配置好网络环境。以下分为实机和虚拟机:

1.1.1 实机

用网线的一端连接Go2机器人，另一端连接用户ubuntu20.04电脑，打开网络设置，添加网络。

机器狗机载电脑的 IP 地地址为 192.168.123.161，故需将电脑 USB Ethernet 地址设置为与机器狗同一网段，如在地址中输入 192.168.123.222 (“222”可以改成其他)。

1.1.2 虚拟机

虚拟机需要改不少东西，最初我不知道的时候还以为和实机一样，气坏我了！！！
首先要修改网络连接，按照以下图片修改网线连接，地址 192.168.123.222 (“222”可以改成其他)

然后我们还要修改虚拟机设置，打开编辑-虚拟网络编辑器，然后如下图添加一个新网络，改为桥接模式，选择网线对应的网卡。

然后我们要在虚拟机中添加两个网络适配器，一个用于联网（NAT），一个用于和狗内网交换(桥接模式)，之后的流程就和实机的流程相同了。

1.2 ROS2安装

然后就是在电脑上安装 ROS2 Foxy版（狗上的版本是这个，最好和狗的相同）。
打开ubuntu终端，输入以下代码

wgethttp://fishros.com/install-Ofishros&&.fishros

选择ROS2 Foxy，下载ROS2时选择国外官方的源（国内的ROS2 Foxy源都炸了，下不了）

1.3宇树ROS2必要环境安装

更详细的安装教程请看宇树文档中心-GO2SDK开发指南-软件服务接口-ROS2服务接口
如果大家访问不了github，在我这下载吧，unitree_go 和 unitree_api 功能包在unitree压缩包里

链接: https://pan.baidu.com/s/1VHCeH6D3KBbybrfmJqMbMw?pwd=mrkq

克隆仓库

gitclone https://github.com/unitreerobotics/unitree_ros2

安装宇树ROS2功能包

sudoaptinstallros-foxy-rmw-cyclonedds-cppsudoaptinstallros-foxy-rosidl-generator-dds-idl

编译 cyclone-dds
编译 cyclonedds 前请确保在启动终端时没有 source ros2 相关的环境变量，否则会导致 cyclonedds 编译报错。如果安装 ROS2 时在~/.bashrc中添加了"source/opt/ros/foxy/setup.bash "，需要修改 ~/.bashrc 文件将其删除

sudoaptinstallgeditsudogedit ~/.bashrc

然后注释掉ROS2 相关的环境变量，例如

# source /opt/ros/foxy/setup.bash

注意，编译好后记得把注释删了！！！
在终端中执行以下操作编译 cyclone-dds

cd~/unitree_ros2/cyclonedds_ws/src#克隆cyclonedds仓库gitclone https://github.com/ros2/rmw_cyclonedds-bfoxygitclone https://github.com/eclipse-cyclonedds/cyclonedds-breleases/0.10.xcd..colcon build --packages-select cyclonedds#编译cyclonedds

编译 unitree_go 和 unitree_api 功能包
编译好 cyclone-dds 后就需要 Ros2 相关的依赖来完成 Go2 功能包的编译，因此编译前需要先 source ROS2 的环境变量。

source/opt/ros/foxy/setup.bash#source ROS2 环境变量colcon build#编译工作空间下的所有功能包

全部完成后，用ifconfig查看网络信息，确认机器人连接到的以太网网卡。
打开 setup.sh 文件

sudogedit ~/unitree_ros2/setup.sh

修改<NetworkInterface name="enp3s0"的“enp3s0”，换成你的网卡
最后

source~/unitree_ros2/setup.sh

如果能加到~/.bashrc 文件最好加，不然你每次都要source。

2.YOLO人脸表情识别

我采用RAF-DB作为训练集，将其全部进行标注后，使用YOLOv5s进行训练。具体的内容在CSDN上有很多，就不进行详细讲解了。
训练效果如下:

识别采用opencv人脸框定小模型框定人脸，然后YOLO进行人脸表情识别。看着很轻松。
然后就遇到了本项目第二个大麻烦，摄像头！！

2.1摄像头问题和解决方案

我的宇树GO2 EDU版的摄像头是特殊的CSI接口摄像头，直接连接到 Tegra 处理器的相机接口，不是标准的 V4L2 USB 摄像头。
就导致一个问题，我们没办法直接用cv,imread()和cv.imshow()，因为它们是针对USB摄像头的。
针对这个问题，走了不少弯路。
最开始采用了宇树SDK上的摄像头示例代码测试，在单独运行时很正常，摄像头正常打开。很兴奋，以为就要解决了，然后但当我们需要将该视频流接入 ROS2 节点并与 YOLOv5 模型联动时，遇到了 DDS 组播冲突问题。
具体而言，Go2 板载系统中同时运行着 ROS2 节点与宇树 SDK 内部的数据分发服务，两者均依赖 CycloneDDS 中间件。在一个进程内重复初始化 DDS 域会导致资源竞争和协议栈冲突，使得视频话题无法正常订阅，OpenCV 也无法获得图像数据。
所以这个方案不可行。
因此后续采用 GStreamer UDP 多播拉流方案：直接通过 UDP 协议从网络接收机器狗摄像头传输的 H.264 编码视频流，再解码成 OpenCV 可处理的 BGR 图像。
首先我们要安装GStreamer

sudoapt-getinstalllibgstreamer1.0-dev libgstreamer-plugins-base1.0-dev libgstreamer-plugins-bad1.0-dev gstreamer1.0-plugins-base gstreamer1.0-plugins-good gstreamer1.0-plugins-bad gstreamer1.0-plugins-ugly gstreamer1.0-libav gstreamer1.0-tools

然后我们要检查我们的opencv是不是支持GStreamer的，在终端输入以下代码。

python3-c"import cv2; print(cv2.getBuildInformation())"

如果是NO，你需要安装或重新编译一个带 GStreamer 支持的 OpenCV。

# 先卸载 pip 安装的版本，避免冲突pip3 uninstall opencv-python opencv-contrib-python# 再安装系统版本sudoaptupdatesudoaptinstallpython3-opencv

完成后，摄像头就能正常使用了，在识别代码里加入以下内容:

# ----- 使用 GStreamer UDP 拉流-----interface="ens33"# 请修改为你的实际网卡名（通过 ifconfig 查看）gst_pipeline=(f"udpsrc address=230.1.1.1 port=1720 multicast-iface={interface}""! application/x-rtp, media=video, encoding-name=H264 ""! rtph264depay ! h264parse ! avdec_h264 ! videoconvert ""! video/x-raw,width=1280,height=720,format=BGR ! appsink drop=1")self.cap=cv2.VideoCapture(gst_pipeline,cv2.CAP_GSTREAMER)

完整的初始化代码如下:

importcv2importtorchimportnumpyasnpimportsys#这里是YOLO的文件路径，按照你自己的改sys.path.append('/home/qi/unitree_ros2/project_ws/src/expression_recognizer/resource/yolov5-master')frommodels.commonimportDetectMultiBackendfromutils.generalimportnon_max_suppression,scale_boxesimportrclpyfromrclpy.nodeimportNode#这个是ros2自定义信息文件，保存识别信息fromgo2_interfaces.msgimportExpressionResultdef__init__(self):#这里是发布ros2节点和话题super().__init__('expression_node')self.publisher_=self.create_publisher(ExpressionResult,'expression_topic',10)# 加载表情识别模型weights='/home/qi/unitree_ros2/project_ws/src/expression_recognizer/resource/yolov5-master/runs/exp2/weights/best.pt'self.device=torch.device('cuda'iftorch.cuda.is_available()else'cpu')self.model=DetectMultiBackend(weights,device=self.device)self.names=self.model.names# 加载人脸检测模型prototxt='/home/qi/unitree_ros2/project_ws/src/expression_recognizer/resource/yolov5-master/models/deploy.prototxt.txt'caffemodel='/home/qi/unitree_ros2/project_ws/src/expression_recognizer/resource/yolov5-master/models/res10_300x300_ssd_iter_140000.caffemodel'self.face_net=cv2.dnn.readNetFromCaffe(prototxt,caffemodel)# ----- 使用 GStreamer UDP 拉流）-----interface="ens33"# 请修改为你的实际网卡名（通过 ifconfig 查看）gst_pipeline=(f"udpsrc address=230.1.1.1 port=1720 multicast-iface={interface}""! application/x-rtp, media=video, encoding-name=H264 ""! rtph264depay ! h264parse ! avdec_h264 ! videoconvert ""! video/x-raw,width=1280,height=720,format=BGR ! appsink drop=1")self.cap=cv2.VideoCapture(gst_pipeline,cv2.CAP_GSTREAMER)ifnotself.cap.isOpened():self.get_logger().error("Failed to open GStreamer pipeline")raiseRuntimeError("Cannot open video stream")self.get_logger().info("GStreamer pipeline opened, waiting for video...")# 定时器处理帧（官方视频帧率约 15fps，这里设为 0.066 秒）self.timer=self.create_timer(0.066,self.process_frame)

2.2 信息包和识别包创建

然后就是弄好ROS2包了
首先在工作空间的src文件下创建好go2_interfaces自定义信息包

ros2 pkg create --build-type ament_cmake go2_interfaces

创建msg文件夹

cd./go2_interfacesmkdirmsg

创建ExpressionResult.msg文件，定义需要传输的表情和置信度：

string expression float32 confidence

修改go2_interfaces包中的package.xml，添加必要的依赖:

<buildtool_depend>rosidl_default_generators</buildtool_depend><exec_depend>rosidl_default_runtime</exec_depend><member_of_group>rosidl_interface_packages</member_of_group>

修改CMakeLists.txt，让ROS 2生成接口代码：

find_package(rosidl_default_generators REQUIRED)rosidl_generate_interfaces(${PROJECT_NAME}"msg/ExpressionResult.msg")

然后编译

colcon build --packages-select go2_interfacessourceinstall/setup.bash

创建人脸表情识别包

ros2 pkg create expression_recognizer --build-type ament_python--dependenciesrclpy go2_interfaces

修改expression_recognizer包中的setup.py，将节点加入:

entry_points={'console_scripts':['vision_detect = expression_recognizer.vision_detect:main',],},

编译

colcon build --packages-select expression_recognizersourceinstall/setup.bash

机器狗识别结果如下:

3 情感交互系统

以下是具体的流程图，简略讲解部分功能。情感交互分为表情控制动作和语音控制动作，动作采用官方提供的 SportClient 封装（位于 common/ros2_sport_client.h）。

3.1 表情控制系统

表情控制系统主要是通过订阅ROS2信息包的表情话题的数据来判断需要执行什么动作。

// 2. 订阅 ROS2 表情识别话题expression_sub_=this->create_subscription<go2_interfaces::msg::ExpressionResult>("expression_topic",10,std::bind(&Go2ControlNode::ExpressionCallback,this,std::placeholders::_1));

下面是宇树官方的部分动作

/********************************************************************** Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. ***********************************************************************/#include"expression_controller/common/ros2_sport_client.h"voidSportClient::Damp(unitree_api::msg::Request&req){req.header.identity.api_id=ROBOT_SPORT_API_ID_DAMP;req_puber_->publish(req);}voidSportClient::BalanceStand(unitree_api::msg::Request&req){req.header.identity.api_id=ROBOT_SPORT_API_ID_BALANCESTAND;req_puber_->publish(req);}voidSportClient::StopMove(unitree_api::msg::Request&req){req.header.identity.api_id=ROBOT_SPORT_API_ID_STOPMOVE;req_puber_->publish(req);}

通过调用实现动作。
最终机器狗识别到表情后做出相应动作。

3.2 语音控制系统

语音控制系统的调用与上面的类似。语音系统同样有一个语音识别包。

voice_sub_=this->create_subscription<go2_interfaces::msg::VoiceCommand>("voice_command",10,std::bind(&Go2ControlNode::VoiceCommandCallback,this,std::placeholders::_1));