#冲刺创作新星#宇树A1机器狗手势控制 原创

在上一篇博客的基础上，结合手势识别和实验室的unitreeA1机器狗做了一个机器狗的手势控制,可以实现手势控制机器狗的前后左右平动。

前期准备

1、需要对ROS的话题通信机制及其环境配置有一定了解。
2、建议先在Windows上跑通上一篇博客的手势识别模型再来进行机器狗的手势控制。
3、宇树A1机器狗，GO1以及其他机器狗的配置同理。
4、机器狗内置电脑上的环境配置：
①Python版本应为3.8（必要条件）
②视觉识别依赖包：Opencv,Mediapipe,;
③Unitree官方发布的ROS_to_Real包，版本选择最早发布的版本（或unitree后续发布的支持A1机器狗的版本）。
④ROS开发环境建议用VScode。
⑤在Windows下载VncViewer以进行远程桌面连接，防止调试运动程序过程中损坏机器狗。
⑥由于ROS Melodic所用为Python2，但Mediapipe和Opencv支持的版本为Python3.8，因此需要进行虚拟环境配置，下文会详细展开。

环境配置教程

2.1 ROS_to_Real包以及SDK的编译安装

这一部分需要先认真阅读官方的Readme文档，确认各种依赖以及版本都是正确的。
官方网址：unitreerobotics/unitree_ros_to_real (github.com)
首先进入主文件夹，输入 ：

git clone https://github.com/unitreerobotics/unitree_legged_sdk.git
//也可以采用镜像源
// git clone https://github.com.cnpmjs.org/unitreerobotics/unitree_legged_sdk.git
//也可以直接用自己的电脑到官网下载，然后再用U盘拷贝到机器狗上即可，记得先看Readme里支持的版本。

输入cd unitree_legged_sdk/进入文件夹
然后按照readme里的步骤，在终端依次输入

mkdir build
cd build
cmake ../
make

接着创建工作空间，安装宇树的功能包

mkdir unitree
cd unitree
mkdir src
cd src
git clone https://github.com.cnpmjs.org/unitreerobotics/unitree_ros.git
//同样可以到自己电脑上下载完拷贝

接下来按照文档里说的，打开主文件夹里的.bashrc文件，
在主文件夹打开终端，输入：

gedit .bashrc

在文档末尾添加

source /opt/ros/melodic/setup.bash
source /usr/share/gazebo-8/setup.sh//如果报错的话，可以删掉这一行
source ~/catkin_ws/devel/setup.bash
export ROS_PACKAGE_PATH=~/catkin_ws:${ROS_PACKAGE_PATH}
export GAZEBO_PLUGIN_PATH=~/catkin_ws/devel/lib:${GAZEBO_PLUGIN_PATH}
export LD_LIBRARY_PATH=~/catkin_ws/devel/lib:${LD_LIBRARY_PATH}
#3_1, 3_2
export UNITREE_SDK_VERSION=3_2//看是SDK的版本是3_1还是3_2.
export UNITREE_LEGGED_SDK_PATH=~/unitree_legged_sdk
export ALIENGO_SDK_PATH=~/aliengo_sdk
#amd64, arm32, arm64
export UNITREE_PLATFORM="amd64"//A1前面的板子是arm,后面的是amd

注意：这里要把把上述部分catkin_ws全部改成你工作空间名（如果是直接复制上面代码的话就不用改），如我的工作空间叫unitree就把catkin_ws改成unitree
另外如果需要开发Aliengo则把export UNITREE_SDK_VERSION=3_2改export UNITREE_SDK_VERSION=3_1即可
如果没有安装Aliengo的SDK可以把exportALIENGO_SDK_PATH=~/aliengo_sdk注释掉。
接下来在工作空间目录下打开终端，输入：

catkin_make

一般编译不通过都是版本问题，需要仔细阅读Readme，或者Gazebo报错的话可以将其注释掉。

2.2 VScode安装以及ROS开发环境的配置

VScode可以直接到官网上下载安装包然后打开（或者直接下载NAS上的安装包），双击即开始安装。如果觉得太慢也可以在直接电脑上下载安装包，然后拷贝到Ubuntu里，注意系统架构，A1前面的板子是arm架构，后面的板子是amd不要下错了。
下载完成后，打开命令行，输入：

cd 你的工作空间路径
code .

即可打开Vscode。
要在VScode上编写ROS程序，还需要以下配置操作。
①打开Vscode,点击这里

③然后ctrl + shfit + b，点击第一行右边的小齿轮
将里面的内容替换如下：

{
// 有关 tasks.json 格式的文档，请参见
    // https://go.microsoft.com/fwlink/?LinkId=733558
    "version": "2.0.0",
    "tasks": [
        {
            "label": "catkin_make:debug", //代表提示的描述性信息
            "type": "shell",  //可以选择shell或者process,如果是shell代码是在shell里面运行一个命令，如果是process代表作为一个进程来运行
            "command": "catkin_make",//这个是我们需要运行的命令
            "args": [],//如果需要在命令后面加一些后缀，可以写在这里，比如-DCATKIN_WHITELIST_PACKAGES=“pac1;pac2”
            "group": {"kind":"build","isDefault":true},
            "presentation": {
                "reveal": "always"//可选always或者silence，代表是否输出信息
            },
            "problemMatcher": "$msCompile"
        }
    ]
}

④然后打开c_cpp_properties.json文件，将Cpp标准修改如下：

以上操作都要执行，否则将无法导入头文件。
以及编写程序时，如果里面出现了中文，记得加上：

setlocale(LC_CTYPE, "zh_CN.utf8");
setlocale(LC_ALL, "");

2.3 Python3.8下载

wget https://www.python.org/ftp/python/3.8.1/Python-3.8.1.tgz
tar -zxvf Python-3.8.1.tgz
cd Python-3.8.1
./configure
make
make test

如果make test命令出现ModuleNotFoundError: No module named‘_ctypes’
可以参考这篇博客解决
最后输入：

sudo make install

2.4 Python 虚拟环境配置

因为我们的视觉程序依赖Opencv和Mediapipe，而它们需要Python3.8来运行，但是ROS依赖的又是Python2,所以我们需要配置出一个虚拟环境，将两个环境隔离开。必须下载完Python3.8之后才可以执行此操作。这一步是为了把系统当前的Python环境换成Python3.8，因此没下载Python3.8执行这一步的话会直接报错。
原理：使用virtualenv创建一个Python3的环境。然后在这个环境中编译安装自己需要的软件包。在引用软件包的时候，如果没有对应的Python3的软件包，会自动的去Python2.7的环境里面找。这样很多软件包都是可以通用的。当然对于没有做好Python3支持的软件包也是没法用的。

mkdir catkin_ws # 创建工作空间
cd catkin_ws
mkdir src
cd src 
#创建虚拟环境
virtualenv -p /usr/local/python3.8.1/bin/python3.8.1 venv     #这一步是创建虚拟环境,路径不一定一样，仅供参考。
source venv/bin/activate                              #这一步是激活虚拟环境。
pip install catkin_pkg pyyaml empy rospkg numpy          #安装python3虚拟环境下所需的编译依赖之类的。
catkin_make
source devel/setup.bash

2.5 Mediapipe与Opencv的安装

回到上一步的工作空间目录下，激活虚拟环境，并下载安装Mediapipe与Opencv：

source venv/bin/activate  

此时命令行输入python,可以看到Python已经变成了3.8版本。
然后开始下载Opencv和Mediapipe包(如果报错就把pip3.8换为pip或pip3)

pip3.8 install opencv-python
pip3.8 install mediapipe

如果报错是pip版本不够，则执行

python3 -m pip install --upgrade pip

配置完成后视觉程序即可运行，注意，不要进行编译，直接右键选择在命令行中运行即可。

2.6 远程桌面链接

先到官网下载VncViewer,然后启动机器狗，连接机器狗WiFi,password是8个0；然后打开VNC_Viewer;
第一行选vnc-any; 地址为192.168.123.12；password八个0；
如果屏幕只有三分之一，则打开命令行，输入命令：

xrandr --fb 1920x1080

2.6报错解决：
如果在运行roscore时报错说python版本不对，则需要重置python软链接，将其指向python2(需要先通过whereis python命令查找python2所在路径,在命令行输入:

mv /usr/bin/python /usr/bin/python.bak
ln -s /usr/local/python2.7/bin/python2.7 /usr/bin/python #这里的路径是python2的路径
mv /usr/bin/pip /usr/bin/pip.bak
ln -s /usr/local/python3.7.1/bin/pip3 /usr/bin/pip

如果缺少某些包或依赖，直接用sudo apt-get 或pip install 命令安装即可，前提是得先激活虚拟环境，在虚拟环境中下载python包。

关键代码

视觉代码

```python
#! /usr/bin/env python3.8
import os
import sys
path = os.path.abspath(".")
sys.path.insert(0,path + "/src/scripts")
from numpy import rate
import rospy
from std_msgs.msg import Int32
import cv2
import mediapipe as mp
import numpy as np
import time#用于得知当前时间
import HandTrackingModule  as htm
"""1.导包
   2.初始化节点
   3.创建发布者对象
   4.编写对象并发布数据
"""
if __name__=="__main__":
#2.初始化节点
    rospy.init_node("vis")#传入节点名称
#3.创建发布者对象
    pub = rospy.Publisher("visual",Int32,queue_size=10)#话题名，消息类型，消息队列，
#4.编写对象并发布数据
    msg = Int32()
#指定发布频率
    rate = rospy.Rate(500)
#使用循环发布数据
    wCam,hCam = 640,480
    cTime = 0
    pTime = 0
    cap = cv2.VideoCapture(2)//如果摄像头获取画面报错，可以把这里换成0，1，或者3
    cap.set(3,wCam)
    cap.set(4,hCam)
    detector = htm.handDetector(detectionCon=0.7)
    while not rospy.is_shutdown():
        success,img = cap.read()
        img = detector.findHands(img)
        #Find Hand
        lmList = detector.findPosition(img,draw=False)
        if len(lmList) != 0:
            print(lmList[4],lmList[20])
            x1 = lmList[4][1]
            x2 = lmList[20][1]
            y1 = lmList[12][2]
            y2 = lmList[9][2]
            dx=x1-x2
            dy=y1-y2
            dh=lmList[4][2]-lmList[17][2]
            dw=lmList[12][1]-lmList[0][1]
            print(dx)
            print(dy)
 
            if dh<-20:
                print("ok")
                print(dw)
                if dw<-5:
                    print("left")
                    flag=3
                elif dw>5:
                    print("right")
                    flag=4
 
            elif dx>10 and dy<0:
                flag=2
                print("back")
            elif dx<-15 and dy<0:
                print("go ahead")
                flag=1
            if dy>5:
                if abs(dh)<40:
                    print("stop")
                    flag=0
            msg.data=flag
            rospy.loginfo("发布的数据是：%d",msg.data)
            pub.publish(msg)//发布数据
        cTime = time.time()
        fps = 1/(cTime-pTime)
        pTime = cTime
 
        cv2.putText(img,f"FPS:{int(fps)}",(10,70),cv2.FONT_HERSHEY_PLAIN,3,
        (255,0,0),3)#在图像上画出实时FPS
        cv2.imshow("IMG",img)
        if cv2.waitKey(1) == ord("q"):
            break  

视觉程序的详细注释请看上一篇博客，两者代码基本相同，只是多了一个判断模块和命令发布，这里都有注释。
运动控制源码：

/************************************************************************
Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved.
Use of this source code is governed by the MPL-2.0 license, see LICENSE.
************************************************************************/
 
#include <ros/ros.h>//头文件导入
#include <pthread.h>
#include <string>
#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <unitree_legged_msgs/HighCmd.h>
#include <unitree_legged_msgs/HighState.h>
#include "convert.h"
#include "std_msgs/Int32.h"
#include <sstream>
#ifdef SDK3_1
using namespace aliengo;
#endif
#ifdef SDK3_2
using namespace UNITREE_LEGGED_SDK;
#endif
int flag=0;//用于标志机器狗是否发布过停止命令
long motiontime=0;//用于标志当前机器狗接收到的命令，1是前进，2是后退，3是左转，四是停止
int flag_sp=0;
template<typename TLCM>
void* update_loop(void* param)
{
    TLCM *data = (TLCM *)param;
    while(ros::ok){
        data->Recv();
        usleep(2000);
    }
}
void doMsg(const std_msgs::Int32::ConstPtr &msg)
{
    printf("data:%d",msg->data);
    if(msg->data==0)//只要视觉模块发布过一次停止命令，机器狗就会停下，并且不再重新运动
    {
        motiontime=5;
        flag=1;
        printf("5");
    }
    else if(msg->data==1)//取出数据进行比较
    {
        motiontime=1;
        printf("1");
    }
    else if(msg->data==2)
    {
        motiontime=2;
        printf("2");
    }
    else if(msg->data==3)
    {
        motiontime=3;
        printf("3");
    }
    else if(msg->data==4)
    {
        motiontime=4;
        printf("4");
    }
 
    if (flag==1)
    {
        motiontime=5;
        printf("%d",motiontime);
    }
 
}
template<typename TCmd, typename TState, typename TLCM>
int mainHelper(int argc, char *argv[], TLCM &roslcm)
{
    std::cout << "WARNING: Control level is set to HIGH-level." << std::endl
              << "Make sure the robot is standing on the ground." << std::endl
              << "Press Enter to continue..." << std::endl;
    std::cin.ignore();
 
    ros::NodeHandle n;
    ros::Rate loop_rate(500);
 
    // SetLevel(HIGHLEVEL);
    TCmd SendHighLCM = {0};
    TState RecvHighLCM = {0};
    unitree_legged_msgs::HighCmd SendHighROS;
    unitree_legged_msgs::HighState RecvHighROS;
 
    roslcm.SubscribeState();
 
    pthread_t tid;
    pthread_create(&tid, NULL, update_loop<TLCM>, &roslcm);
    ros::Subscriber sub = n.subscribe("visual",15,doMsg);
    while (ros::ok()){
        ros::spinOnce();
        roslcm.Get(RecvHighLCM);
        RecvHighROS = ToRos(RecvHighLCM);
        // printf("%f\n",  RecvHighROS.forwardSpeed);
 
        SendHighROS.forwardSpeed = 0.0f;//前后速度
        SendHighROS.sideSpeed = 0.0f;//左右速度
        SendHighROS.rotateSpeed = 0.0f;//旋转速度
        SendHighROS.bodyHeight = 0.0f;//身体高度
 
        SendHighROS.mode = 0;//运动模式，0为静止模式，1为调整模型，机器狗可以调整自身高度。
        SendHighROS.roll  = 0;//2为运动模式，机器狗发布的速度命令方才有效
        SendHighROS.pitch = 0;//机器狗三个轴的角度
        SendHighROS.yaw = 0;
 
        //printf("%f\n",  RecvHighROS.forwardSpeed);
        //printf("3");
        if(motiontime==1){
            SendHighROS.mode = 2;
            SendHighROS.sideSpeed = 0.01f;
            SendHighROS.forwardSpeed = 0.2f; // -1  ~ +1
        }
 
        else if(motiontime==2){
            SendHighROS.mode = 2;
            SendHighROS.sideSpeed = 0.01f;
            SendHighROS.forwardSpeed = -0.2f; // -1  ~ +1
        }
        else if(motiontime==3){
            SendHighROS.mode = 2;
            SendHighROS.sideSpeed = 0.3f; // -1  ~ +1
        }
        else if(motiontime==4){
            SendHighROS.mode = 2;
            SendHighROS.sideSpeed = -0.3f;     
        }
        if(motiontime==5)
        {
            flag=1;
            SendHighROS.mode = 0;
            printf("ok");
            SendHighROS.forwardSpeed = 0.0f;
            SendHighROS.sideSpeed = 0.0f;
            // SendHighLCM = ToLcm(SendHighROS, SendHighLCM);
            // roslcm.Send(SendHighLCM);
        }
        SendHighLCM = ToLcm(SendHighROS, SendHighLCM); 
        roslcm.Send(SendHighLCM); //发布速度消息
        loop_rate.sleep(); //用于休眠函数，控制发布频率
    }
    return 0;
}
 
int main(int argc, char *argv[]){
    ros::init(argc, argv, "walk_ros_mode");
    std::string firmwork;
    ros::param::get("/firmwork", firmwork);
 
    #ifdef SDK3_1
        aliengo::Control control(aliengo::HIGHLEVEL);
        aliengo::LCM roslcm;
        mainHelper<aliengo::HighCmd, aliengo::HighState, aliengo::LCM>(argc, argv, roslcm);
    #endif
 
    #ifdef SDK3_2
        std::string robot_name;
        UNITREE_LEGGED_SDK::LeggedType rname;
        ros::param::get("/robot_name", robot_name);
        if(strcasecmp(robot_name.c_str(), "A1") == 0)
            rname = UNITREE_LEGGED_SDK::LeggedType::A1;
        else if(strcasecmp(robot_name.c_str(), "Aliengo") == 0)
            rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo;
 
        // UNITREE_LEGGED_SDK::InitEnvironment();
        UNITREE_LEGGED_SDK::LCM roslcm(UNITREE_LEGGED_SDK::HIGHLEVEL);
        mainHelper<UNITREE_LEGGED_SDK::HighCmd, UNITREE_LEGGED_SDK::HighState, UNITREE_LEGGED_SDK::LCM>(argc, argv, roslcm);
#endif

}
``
![GF1YA1IAM9O`Q4`E227J.png](https://dl-harmonyos.51cto.com/images/202209/759795e5957f16cb12e136a16c8e8fbefaa381.png?x-oss-process=image/resize,w_818,h_677)

总结：项目的最终能够实现对机器狗进行简单的前进后退，左右平移等运动控制，并且精度较高，误识率很低，在宇树A1的视觉板（性能相对较弱）上帧率能够达到12左右，刚好能够满足数据传输的需要，如果需要提高帧率的话可以将其部署在机器狗后面的运动控制板块上（性能较强），或者调节上篇文档提到的追踪置信度以及识别置信度函数（但不建议这么做），能够提高一定的帧率。
缺陷：由于受到机器狗计算平台性能以及相机精度的限制，在距离超过3.6米时手势识别便会较为困难，如果超过4米则无法识别到手势，以及暂时还无法控制进行机器狗转向等更复杂的运动。

项目进一步改进的思路：通过对模型进行裁剪或者蒸馏进一步降低模型所需计算资源（或者基于LCM的低延时通信，结合实验室的高性能服务器，将画面信息传输给服务器进行处理，再由服务器返回处理结果，可以极大提高帧率）。对于远距离识别问题，在不改变硬件的基础上，可以通过加入单目测距模块对手势距离进行测量并适当靠近手势，使机器狗保持一个较为良好的手势识别环境。而对于机器狗更复杂的运动控制，计划加入人体躯干模块（即识别全身节点），从而在视觉上判断更多具有较高辨识度的动作作为命令手势。