Unitree 1. 板间通信和数据获取

官方文档：

Go1 — Unitree_Docs 1.0rc documentation

GitHub - unitreerobotics/UnitreecameraSDK: Unitree GO1 camera SDK

CSDN：

宇树Unitree camera 双目鱼眼睛的使用_宇树a1 调用摄像头-CSDN博客

部署在Go1身上的手势识别_unitree go1-CSDN博客

1. 通信框架

1.1 IP地址

连接机器狗热点（出厂WiFi名称为Goxxxxxxxx，密码为00000000）

（1）进入Motion Control Board: RasPi 4B (192.168.123.161)

ssh pi@192.168.12.1
123

 进入此界面后，可以使用以下命令进入其他的板卡。注意不同板卡的切换需要使用exit退回pi的终端。

 （2）进入Main control board: MCU (192.168.123.10) Connection refused, 可能不被允许。

ssh unitree@192.168.123.10
123

（3）进入头部相机板卡: nano2gb (head, 192.168.123.13)

ssh unitree@192.168.123.13
123

（4）进入左右相机板卡:Nano (body, 192.168.123.14) unitree@unitree-desktop（背面接口的板卡）

ssh unitree@192.168.123.14
123

（5）进入下方相机板卡Sensing motherboard: Nano or NX (body, 192.168.123.15) 

ssh unitree@192.168.123.15
123

如果需要使用wifi连接go1，则必须连接树梅派。如果通过局域网连接go1，则需要设定用户ip地址为192.168.123.XXX。

2. 相机

进入各个板卡的主要目的是设定相关的相机设定。

2.1. 在unitree-desktop测试左右相机读取

首先下载GitHub - unitreerobotics/UnitreecameraSDK: Unitree GO1 camera SDK并发送到左右相机板卡:Nano (body, 192.168.123.14) unitree@unitree-desktop。

参考Go1 — Unitree_Docs 1.0rc documentation

在使用SDK之前需要先kill掉内置调用相机的程序才能成功获取图像。停止相关进程：

ps -aux | grep point_cloud_node | awk '{print $2}' | xargs kill -9
ps -aux | grep mqttControlNode | awk '{print $2}' | xargs kill -9
ps -aux | grep live_human_pose | awk '{print $2}' | xargs kill -9
ps -aux | grep rosnode | awk '{print $2}' | xargs kill -9

 测试左右双目鱼眼：

cd ..
./bins/example_getRectFrame

如果要修改观察的方向左//右，在 stereo_camera_config.yaml中修改data为0或1。

DeviceNode: !!opencv-matrix
   rows: 1
   cols: 1
   dt: d
   data: [ 0. ]

2.2. 头部板卡消息

需要先通过unitree-desktop或用户pc发送UnitreecameraSDK文件到head板卡：

scp -r UnitreecameraSDK unitree@192.168.123.13:/home/unitree/

进入头部相机板卡: nano2gb (head, 192.168.123.13)

ssh unitree@192.168.123.13
123

修改trans_rect_config.yaml，将相机发送数据的对象设定为unitree-desktop，192.168.123.15。或者设置为用户自己的ip地址。

#UDP address for image transfer   192.168.123.IpLastSegment
IpLastSegment: !!opencv-matrix
   rows: 1
   cols: 1
   dt: d
   data: [ 15. ]

编译后运行图片发送端，之后就可以在对应的IP（此处时192.168.123.15）运行接收代码。

cd UnitreecameraSDK/
./bins/example_putImagetrans

2.3. 配置发送

使用图像发送和接受，只需要配置在trans_rect_config.yaml中的参数。stereo_camera_config.yaml中主要用于在当前板卡验证相机的图像获取。

（1）配置IP地址

#UDP address for image transfer   192.168.123.IpLastSegment
IpLastSegment: !!opencv-matrix
   rows: 1
   cols: 1
   dt: d
   data: [ 15. ]

（2）配置需要传输的相机，使用0或1来切换身体的左右，头部的前和下视摄像头。

#DeviceNode
DeviceNode: !!opencv-matrix
   rows: 1
   cols: 1
   dt: d
   data: [ 0. ]

（3）配置传输图像的类型，据说双目的左右顺序反了。需要使用鱼眼消息可以看后文。

#0 右侧原始图像  1 双目原始图像  2 右侧矫正图像  3 双目矫正图像   -1 不传图
Transmode: !!opencv-matrix
   rows: 1
   cols: 1
   dt: d
   data: [ 0. ] 

#The transmode here, 0 and 1 cannot be used, you need to set 2 or 3

一些官方的代码使用解释：

/*
发送端：
方法概述：采用udp方法输出图片，传输方式已写入程序，由StereoCameraCommon::startCapture(true,false)中第一个参数控制定向udp传输，当其为true时打开udp264硬编码传输模式
参数：配置文件中IpLastSegment是接收端ip，接收端地址必须是192.168.123.IpLastSegment。
     配置文件中Transmode决定传输模式
     配置文件中Transrate决定传输速率，限制了传输速率小于图片速率
     端口9201~9205分别对应前方，下巴，左，右，腹部
     启动自定义图传时需使用kill.sh文件关闭Unitree/autostart/02camerarosnode程序来解除相机占用，Unitree/autostart/04imageai来释放算力。
     （也可使用以下几条指令
     ps -A | grep point | awk '{print $1}' | xargs kill -9
     ps -aux|grep mqttControlNode|grep -v grep|head -n 1|awk '{print $2}'|xargs kill -9
     ps -aux|grep live_human_pose|grep -v grep|head -n 1|awk '{print $2}'|xargs kill -9
     ）
传输图片模式：由配置文件中Transmode决定，该值只可通过读取配置文件方式写入
0：传输左目原始图片
1：传输双目原始图片
2：传输左目畸变校正后图片，视野范围角度可调60~140
3：传输双目畸变校正后图片
※※※注意，双目图的最后的左右目输出位置是反的。左目->右图，右目->左图
4：不建议使用！！，Depthmode = 2时 可输出深度图和对应左目图片，不建议使用。
*/

2.4. 发送原始鱼眼数据

修改example_putImagetrans代码如下，并使用后序代码接收即可。发送原始数据可以让帧率达到20+。

int main(int argc, char *argv[])
{
   
    UnitreeCamera cam("trans_rect_config.yaml"); ///< init camera by device node number
    if(!cam.isOpened())   ///< get camera open state
        exit(EXIT_FAILURE);   
    cam.startCapture(true,false); ///< disable share memory sharing and able image h264 encoding

    usleep(500000);
    while(cam.isOpened())
    {
        cv::Mat frame;
        std::chrono::microseconds t;
        if(!cam.getRawFrame(frame, t)){ ///< get camera raw image
            usleep(1000);
            continue;
        }
        char key = cv::waitKey(10);
        if(key == 27) // press ESC key
           break;
    }
    
    cam.stopCapture(); ///< stop camera capturing
    
    return 0;
}

2.4. 图像接收

需要在代码中设定本机IP和需要获取的相机ID：

前脸相机portID=9201（cam=1）,下巴相机portID=9202（cam=2）

左侧相机portID=9203（cam=3），右侧相机portID=9204（cam=4）

腹部相机portID=9205（cam=5）

在对应IP的机器上编译运行：

./bins/example_getimagetrans

也可以使用以下命令直接验证接受, 假设主机ip。

gst-launch-1.0 udpsrc address=192.168.123.102 port=9205 ! application/x-rtp,media=video,encoding-name=H264 ! rtph264depay ! h264parse ! avdec_h264 ! videoconvert ! autovideosink

示例代码：

import numpy as np
import sys
import cv2

# print(cv2.getBuildInformation())
# print(cv2.__version__)
# print(cv2.__file__)
# print(sys.path)


def main():
    IpLastSegment = "102"
    cam = 5
    if len(sys.argv) >= 2:
        cam = int(sys.argv[1])

    udpstrPrevData = "udpsrc address=192.168.123." + IpLastSegment + " port="
    # 端口：前方，下巴，左，右，腹部
    udpPORT = [9201, 9202, 9203, 9204, 9205]
    udpstrBehindData = " ! application/x-rtp,media=video,encoding-name=H264 ! rtph264depay ! h264parse ! avdec_h264 ! videoconvert ! appsink"

    udpSendIntegratedPipe = udpstrPrevData + str(udpPORT[cam - 1]) + udpstrBehindData
    print("udpSendIntegratedPipe:", udpSendIntegratedPipe)

    cap = cv2.VideoCapture(udpSendIntegratedPipe)#, cv2.CAP_GSTREAMER

    if not cap.isOpened():
        return
    while True:
        ret, frame = cap.read()
        frame = cv2.resize(frame, (640, 480))
        if frame is not None:
            cv2.imshow("video", frame)
        if cv2.waitKey(20) & 0xFF == ord('q'):
            break

    cap.release()
    cv2.destroyAllWindows()


if __name__ == "__main__":
    main()

3. 注意事项：OpenCV, GStreamer与Anaconda

使用conda安装的opencv虽然支持了GStreamer，但是运行代码无法启动。需要卸载opencv后源码编译安装opencv。参考以下link以在anaconda中安装cv2

python - Install Opencv from source to conda environment - Stack Overflow

部分报错：

Conda虚拟环境下libp11-kit.so.0: undefined symbol: ffi_type_pointer...问题解决-CSDN博客

# 设置环境变量
export CONDA_PREFIX=~/anaconda3/envs/YOUR_ENV_NAME
export PYTHON_VERSION=3.8
export CPLUS_INCLUDE_PATH=$CONDA_PREFIX/include/python$PYTHON_VERSION:$CONDA_PREFIX/lib/python$PYTHON_VERSION/site-packages/numpy/core/include

# 安装 NumPy
conda install numpy

# 创建构建目录并进入该目录
mkdir -p ~/Downloads/opencv_build
cd ~/Downloads/opencv_build

# 运行 CMake 配置
cmake -D CMAKE_BUILD_TYPE=RELEASE \
      -D CMAKE_INSTALL_PREFIX=$CONDA_PREFIX/ \
      -D OPENCV_EXTRA_MODULES_PATH=~/Downloads/opencv_contrib/modules \
      -D PYTHON3_LIBRARY=$CONDA_PREFIX/lib/libpython$PYTHON_VERSION.so \
      -D PYTHON3_INCLUDE_DIR=$CONDA_PREFIX/include/python$PYTHON_VERSION \
      -D PYTHON3_EXECUTABLE=$CONDA_PREFIX/bin/python \
      -D PYTHON3_PACKAGES_PATH=$CONDA_PREFIX/lib/python$PYTHON_VERSION/site-packages \
      -D PYTHON3_NUMPY_INCLUDE_DIRS=$CONDA_PREFIX/lib/python$PYTHON_VERSION/site-packages/numpy/core/include \
      -D BUILD_opencv_python2=OFF \
      -D BUILD_opencv_python3=ON \
      -D INSTALL_PYTHON_EXAMPLES=ON \
      -D INSTALL_C_EXAMPLES=OFF \
      -D OPENCV_ENABLE_NONFREE=ON \
      -D BUILD_EXAMPLES=ON \
      -D WITH_GSTREAMER=ON \
      -D WITH_FFMPEG=ON \
      ~/Downloads/opencv

# 编译和安装 OpenCV
make -j$(nproc)
sudo make install

更换python版本3.8.10

conda remove libffi
conda install python=3.8.10

运行成功：

4. 图像矫正

鱼眼摄像头的畸变矫正方法-python+opencv_鱼眼畸变矫正-CSDN博客

5. 常用指令记录

ssh unitree@192.168.123.15
123

ps -aux | grep point_cloud_node | awk '{print $2}' | xargs kill -9
ps -aux | grep mqttControlNode | awk '{print $2}' | xargs kill -9
ps -aux | grep live_human_pose | awk '{print $2}' | xargs kill -9
ps -aux | grep rosnode | awk '{print $2}' | xargs kill -9

scp -r UnitreecameraSDK unitree@192.168.123.13:/home/unitree/

cd UnitreecameraSDK/
./bins/putRawtrans

scp output_camCalibParams.yaml blamlight@192.168.123.102:/home/blamlight/Desktop