读取雷达数据方案一

可爱橘猫咪

于 2022-08-15 16:16:06 发布

阅读量426

点赞数 1

文章标签：计算机视觉

本文链接：https://blog.csdn.net/weixin_43869428/article/details/126349080

版权

该博客展示了如何利用Python的socket库实现客户端与服务器的通信，接收服务端发送的数据，并将其保存到txt文件中。之后，博客详细介绍了如何从txt文件中读取数据，解析出位置坐标和旋转矩阵，涉及到了文件操作、数据转换和 scipy.spatial.transform 的 Rotation 模块。

摘要由CSDN通过智能技术生成

存入txt文件中，写得比较丑

#接受服务端的信息并保存到txt文件里面
import socket
#########################################
import os
if os.path.exists("pose.txt"):
  os.remove("pose.txt")
##########################################

#client = socket.socket()
client = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
client.connect(("192.168.43.209",6666)) # 建立连接：传入服务器端IP号和要连接的应用程序的端口号

client.send(b'Start')#客户端发送命令开始
#接受服务端的消息
while 1:
    data = client.recv(1024) #客户端可以接收服务器端的消息
    print(data.decode())
###########################################
    f=open("pose.txt","a+")
    f.writelines(data.decode())#将接受到的消息保存到一个txt文件里
    f.writelines('\n')
###########################################

client.close()

import os
from scipy.spatial.transform import Rotation as R
import time

def get_pose(filename):
    with open(filename, 'rb') as f:  # 打开文件
        # 在文本文件中，没有使用b模式选项打开的文件，只允许从文件头开始,只能seek(offset,0)
        offset = -200  # 设置偏移量
        while True:
            """
            file.seek(off, whence=0)：从文件中移动off个操作标记（文件指针），正往结束方向移动，负往开始方向移动。
            如果设定了whence参数，就以whence设定的起始位为准，0代表从头开始，1代表当前位置，2代表文件最末尾位置。
            """
            f.seek(offset, 2)  # seek(offset, 2)表示文件指针：从文件末尾(2)开始向前50个字符(-50)
            lines = f.readlines()  # 读取文件指针范围内所有行
            # print(lines)
            if len(lines) >= 2:  # 判断是否最后至少有两行，这样保证了最后一行是完整的
                p_x = float(lines[-8][5:])
                p_y = float(lines[-7][5:])
                p_z = float(lines[-6][5:])
                o_x = float(lines[-4][5:])
                o_y = float(lines[-3][5:])
                o_z = float(lines[-2][5:])
                o_w = float(lines[-1][5:]) # 取最后一行
                break
            # 如果off为50时得到的readlines只有一行内容，那么不能保证最后一行是完整的
            # 所以off翻倍重新运行，直到readlines不止一行
            # offset *= 2
        # print("position and oorientation:",p_x,p_y,p_z,o_x,o_y,o_z,o_w)
        #计算旋转矩阵
        Rq = [o_x,o_y,o_z,o_w]
        Rm = R.from_quat(Rq)
        rotation_matrix = Rm.as_matrix()
        # print('rotation_martrix:\n', rotation_matrix)

        return  p_x,p_y,p_z,rotation_matrix


if __name__ == '__main__':
    start = time.perf_counter()

    x,y,z,rotation_matrix = get_pose('pose1.txt')
    print('position:\n',x,y,z)
    print('rotation_martrix:\n',rotation_matrix)

    end = time.perf_counter()
    print(str(end - start))