Python读取雷达位姿数据方案二

使用socket\UDP进行通信，用socket嵌套了ros实时读取ros话题中发布的信息，使用了线程和toml配置文件，相比上一版写的很漂亮

服务端：

import socket
from time import ctime
import rospy
from nav_msgs.msg import Odometry
import json
import time
HOST = '192.168.7.102'
PORT = 9001
ADDR = (HOST, PORT)
BUFFSIZE = 512

def pprint(msg):
    asd= msg.pose.pose
    print(asd)
    time.sleep(0.5)

def poseCallback(msg):
    asd= msg.pose.pose
    print(asd)
    data, address = s.recvfrom(BUFFSIZE)
    print('recv!!!!!!')
    s.sendto(str(asd), address)
    time.sleep(0.5)

def udpServer():
    print('UDP start....')
    
    rospy.init_node('pose_subscriber')
    
    while not rospy.is_shutdown():
        print('spin...')
        rospy.Subscriber("/lidar_pose", Odometry, poseCallback, queue_size = 1, buff_size=52428800)
        # rospy.Subscriber("/lidar_pose", Odometry, pprint, queue_size = 1, buff_size=52428800)
        rospy.spin()

    s.close()

if __name__ == '__main__':
    s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    s.bind(ADDR)
    udpServer()

客户端：

import os
import os.path as osp
import sys

import numpy as np
import numpy.linalg as npl

from scipy.spatial.transform import Rotation as R
import socket
import time
import toml


class Leishen:
    def __init__(self):
        self.conf = toml.load("configs/leishen.toml")
        self.trans_matrix = None

        self.client = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.addr = (self.conf['ip'], self.conf['port'])

        self.pre_loc = None
    
    def sendandreceive(self):
        print('send')
        self.client.sendto('send'.encode('utf-8'), self.addr)
        data, addrs = self.client.recvfrom(512)
        return data

    def __call__(self):
        data = self.sendandreceive()

        pose = data.decode()
        Pose = pose.splitlines()
        
        p_x = float(Pose[1][5:12])
        p_y = float(Pose[2][5:12])
        p_z = float(Pose[3][5:12])
        o_x = float(Pose[5][5:12])
        o_y = float(Pose[6][5:12])
        o_z = float(Pose[7][5:12])
        o_w = float(Pose[8][5:12])

        Rq = [o_x, o_y, o_z, o_w]
        Rm = R.from_quat(Rq)
        rotation_matrix = Rm.as_matrix()

        T = np.eye(4)
        T[:3, :3] = rotation_matrix


        c = np.array([[0, -1, 0, 1], [-1, 0, 0, 1], [0, 0, 1, 1], [0, 0, 0, 1]])
        T = c.dot(T)

        T[:3, [3]] = np.array([[0.32+p_y], [2.43-p_x], [p_z]])

        # door
        # sind = 0.019254740597129394
        # cosd = 0.9998146102975978
        
        # sind = -0.011885769348382657
        # cosd = 0.9999293617486172
        
        # 311 312
        # sind = 0.029668866667721464
        # cosd = 0.9995597822795058
        # r = npl.inv(np.array([
        #     [cosd, -sind, 0, 0],
        #     [sind, cosd, 0, 0],
        #     [0, 0, 1, 0],
        #     [0, 0, 0, 1]]))
        
        if self.pre_loc is None:
            self.pre_loc = np.array([p_x, p_y])
        else:
            curr_pos = np.array([p_x, p_y])
            if npl.norm(self.pre_loc - curr_pos) > 4:
                print('lost!\n'*5)
                return
            else:
                self.pre_loc = curr_pos
        
        return T
        # return r @ T

if __name__ == '__main__':
    leishen =  Leishen()
    while 1:
        print("trans_matrix:\n", leishen())
        time.sleep(1)