Jetson nano 获取T265数据并使用串口发送

Created: July 14, 2023 1:05 PM
Created by: Luser
Type: AI、Jetson nano

串口使用

查看串口设备：

ls /dev/tty*

/dev/tty    /dev/tty18  /dev/tty28  /dev/tty38  /dev/tty48  /dev/tty58   /dev/ttyp0  /dev/ttypb
/dev/tty0   /dev/tty19  /dev/tty29  /dev/tty39  /dev/tty49  /dev/tty59   /dev/ttyp1  /dev/ttypc
/dev/tty1   /dev/tty2   /dev/tty3   /dev/tty4   /dev/tty5   /dev/tty6    /dev/ttyp2  /dev/ttypd
/dev/tty10  /dev/tty20  /dev/tty30  /dev/tty40  /dev/tty50  /dev/tty60   /dev/ttyp3  /dev/ttype
/dev/tty11  /dev/tty21  /dev/tty31  /dev/tty41  /dev/tty51  /dev/tty61   /dev/ttyp4  /dev/ttypf
/dev/tty12  /dev/tty22  /dev/tty32  /dev/tty42  /dev/tty52  /dev/tty62   /dev/ttyp5  /dev/ttyS0
/dev/tty13  /dev/tty23  /dev/tty33  /dev/tty43  /dev/tty53  /dev/tty63   /dev/ttyp6  /dev/ttyS1
/dev/tty14  /dev/tty24  /dev/tty34  /dev/tty44  /dev/tty54  /dev/tty7    /dev/ttyp7  /dev/ttyS2
/dev/tty15  /dev/tty25  /dev/tty35  /dev/tty45  /dev/tty55  /dev/tty8    /dev/ttyp8  /dev/ttyS3
/dev/tty16  /dev/tty26  /dev/tty36  /dev/tty46  /dev/tty56  /dev/tty9    /dev/ttyp9  /dev/ttyTHS1
/dev/tty17  /dev/tty27  /dev/tty37  /dev/tty47  /dev/tty57  /dev/ttyGS0  /dev/ttypa  /dev/ttyTHS2

https://cdn.jsdelivr.net/gh/luserli/figure/photos/202307152116041.png

Jetson nano的GPIO口与树莓派的GPIO口相兼容，可在下图查看Jetsonnano的GPIO引脚定义：

https://cdn.jsdelivr.net/gh/luserli/figure/photos/Jetson%20nano%20GPIO%E5%BC%95%E8%84%9A%E5%8F%8A%E5%8A%9F%E8%83%BD%E5%9B%BE.png

所以可以使用串口设备/dev/ttyTHS1进行GPIO的串口数据传输。

不同于树莓派中使用的串口库serial，Jetson nano 中的串口使用需要下载串口库pyserial:

pip3 install pyserial

使用：

import serial

ser = serial.Serial("/dev/ttyTHS1", 115200, timeout=1)  # 串口设置

T265初始化

# 声明 RealSense 管道，封装实际设备和传感器
pipe = rs.pipeline()

# 构建配置对象并请求姿势数据
cfg = rs.config()
cfg.enable_stream(rs.stream.pose)

# 使用请求的配置开始流式传输
pipe.start(cfg)

T265 pose数据获取并串口输出

俯仰角(pitch)、横滚角(roll)和偏航角(yaw)概念：

简单理解3D系统中pitch/yaw/roll 的基本概念

T265可直接获取设备四元数信息，官方例程使用四元数的方式进行姿态角度计算

四元数的基本概念：
四元数是一种数学工具，用于表示旋转的方向和角度。与欧拉角不同，四元数可以避免万向锁问题，并且计算效率更高。

一个四元数由实部（也称为标量部分）和虚部（也称为矢量部分）组成。标量部分是一个实数，而矢量部分是一个三维向量。四元数可以表示为：

$q = w + xi + yj + zk$

其中，w是实部，x、y和z是矢量部分的三个分量，i、j和k是虚部的三个基本单位向量。四元数的长度可以使用欧几里得范数计算：

$|q| = sqrt(w^2 + x^2 + y^2 + z^2)$

四元数可以用来表示旋转的方向和角度，其中旋转角度θ和旋转轴(x, y, z)可以由四元数的虚部推导出来。具体地说，旋转轴和旋转角度可以使用以下公式计算：

$θ = 2 * acos(w)$

$(x, y, z) = (x, y, z) / sqrt(x^2 + y^2 + z^2)$

其中acos是反余弦函数。通过这些公式，可以从四元数中提取旋转的方向和角度，并将其转换为欧拉角或其他旋转表示形式。

俯仰角(pitch)、横滚角(roll)和偏航角(yaw)计算公式：

可以使用四元数来计算旋转的俯仰角、横滚角和偏航角。使用四元数表示旋转时，可以使用以下公式将四元数转换为欧拉角形式：

$pitch = asin(2 * (qwqy - qxqz))$

$roll = atan2(2 * (qwqx + qyqz), 1 - 2 * (qxqx + qyqy))$

$yaw = atan2(2 * (qwqz + qxqy), 1 - 2 * (qyqy + qzqz))$

其中，qx、qy、qz和qw分别是四元数的虚部和实部，atan2是反正切函数，asin是反正弦函数。这些公式可以将四元数表示的旋转转换为欧拉角形式，其中pitch表示绕y轴旋转的俯仰角，roll表示绕x轴旋转的横滚角，yaw表示绕z轴旋转的偏航角。

将T265获取到的四元数数据带入计算公式中即可获得俯仰角、横滚角和偏航角

参考官方例程的代码：

data = pose.get_pose_data()

w = data.rotation.w
x = -data.rotation.z
y = data.rotation.x
z = -data.rotation.y

pitch =  -m.asin(2.0 * (x*z - w*y)) * 180.0 / m.pi;
roll  =  m.atan2(2.0 * (w*x + y*z), w*w - x*x - y*y + z*z) * 180.0 / m.pi;
yaw   =  m.atan2(2.0 * (w*z + x*y), w*w + x*x - y*y - z*z) * 180.0 / m.pi;

添加串口传输与位置信息后的完整代码：

import pyrealsense2 as rs
import math as m
import serial
import time

#取整数的补码
def int2hex(data):
    if(data>0):
        return data
    else :
        data=-data
        data=data|0x8000
        return data
    

while (True):
    ser = serial.Serial("/dev/ttyTHS1", 115200, timeout=1)  # 串口设置
    # 声明 RealSense 管道，封装实际设备和传感器
    pipe = rs.pipeline()

    # 构建配置对象并请求姿势数据
    cfg = rs.config()
    cfg.enable_stream(rs.stream.pose)

    # 使用请求的配置开始流式传输
    pipe.start(cfg)

    try:
        while (True):
            # 等待相机发出的下一组帧
            frames = pipe.wait_for_frames()

            # 获取姿势框架
            pose = frames.get_pose_frame()
            if pose:
                # jetson nano t265角度传输
                data = pose.get_pose_data()
                # 位姿四元数的欧拉角
                ## 通过T265直接获取四元数
                w = data.rotation.w
                x = -data.rotation.z
                y = data.rotation.x
                z = -data.rotation.y
                ## 使用四元数公式计算三种欧拉角
                pitch =  -m.asin(2.0 * (x*z - w*y)) * 180.0 / m.pi
                roll  =  m.atan2(2.0 * (w*x + y*z), w*w - x*x - y*y + z*z) * 180.0 / m.pi
                yaw   =  m.atan2(2.0 * (w*z + x*y), w*w + x*x - y*y - z*z) * 180.0 / m.pi
                
                # 转换为十六进制数并使用 bytearray 将数据打包成需要传输的格式。
                pi = int2hex(int(pitch))
                ro = int2hex(int(roll))
                ya = int2hex(int(yaw))
                my_data1 = bytearray([0xaa,0x29,0x05,0x35,0x06,pi>>8,pi&0xff,ro>>8,ro&0xff,ya>>8,ya&0xff,0x00])
                ser.write(my_data1) # 串口传输 
                
                hh = int2hex(-10)
                print("------------hh:",hh)
                print("Frame #{}".format(pose.frame_number))
                print("RPY [deg]: Roll: {0:.7f}, Pitch: {1:.7f}, Yaw: {2:.7f}".format(roll, pitch, yaw))
                
                # 使用相机的位置信息，将位置信息转化为十六进制数据，并将数据打包成需要传输的格式。
                #xyz
                my_x = data.translation.x*10000
                my_y = data.translation.y*10000
                my_z = data.translation.z*10000
                
                my_x = int2hex(int(my_x))
                my_y = int2hex(int(my_y))
                my_z = int2hex(int(my_z))
                my_data2 = bytearray([0xaa,0x29,0x05,0x34,0x06,my_x>>8,my_x&0xff,my_y>>8,my_y&0xff,my_z>>8,my_z&0xff,0x00])
                ser.write(my_data2) # 串口传输
                print("pos:{}".format(data.translation))
                time.sleep(0.1)  #调试时使用，方便观察
                
    finally:
        pipe.stop()

Jetson nano 获取T265数据并使用串口发送

串口使用

T265初始化

T265 pose数据获取并串口输出

感谢您的支持，我会继续努力的!