ros uwb2world坐标转换python示例

ros uwb2world坐标转换python示例

# coding=utf-8
#!/usr/bin/env python
#coding:utf-8

import rospy
import sys
sys.path.append('.')
import cv2
import os
import math
import numpy as np
#from test_py.msg import Image
from sensor_msgs.msg import Image
from sensor_msgs.msg import CameraInfo
from cv_bridge import CvBridge, CvBridgeError
from geometry_msgs.msg import PointStamped 
from geometry_msgs.msg import PoseWithCovarianceStamped 

from autoware_msgs.msg import DetectedObjectArray
import json



# 变换数据的类，将参数变换为矩阵
class Coordinate2DTransData():

    def __init__(self, f=1, f2=1,theta=0, offsetx=0, offsety=0, zoomx=1, zoomy=1):
        self.set_transform_info(f, f2,theta, offsetx, offsety, zoomx, zoomy)
    #以变换方式设置数据，直接设置让点怎么移动。
    def set_transform_info(self,f=1, f2=1,theta=0, offsetx=0, offsety=0, zoomx=1, zoomy=1):
        self.f = f #A坐标系y轴需要反向
        self.f2 = f2 #B坐标系y轴需要反向
        self.theta = theta #B坐标系转theta度x轴与A重合
        self.offsetx = offsetx #B与A原点重合 x方向的位移
        self.offsety = offsety* f #B与A原点重合 y方向的位移
        self.zoomx = zoomx #x缩放
        self.zoomy = zoomy #y缩放
        # trans A坐标系y轴反向变换矩阵
        self.f_mat = np.array([[1, 0], [0, 1 * f]])
        # trans B坐标系y轴反向变换矩阵
        self.f2_mat = np.array([[1, 0], [0, 1 * f2]])
        # offset #B与A原点重合 xy方向的位移
        self.offset = np.array([[self.offsetx], [self.offsety]])
        self.zoom = np.array([[self.zoomx,0], [0,self.zoomy]])
        # rota #角度转弧度
        thetapi = math.pi * theta / 180.0
        co = np.cos(thetapi)
        si = np.sin(thetapi)
        #旋转矩阵
        self.rota_mat = np.array([[co, -si], [si, co]])

    #以坐标系信息方式设置数据，用坐标角位信息变成让点怎么移动。
    def set_coordinate_info(self, Af=1, Bf2=1,Btheta=0, Bposix=0, Bposiy=0, Bzoomx=1, Bzoomy=1):
        #B在A中的角度 Btheta，旋转变化为 -Btheta,
        #B在A中的x Bposix,位移变化为 -Bposix,
        #B在A中的y Bposiy,位移变化为 -Bposiy,
        self.set_transform_info(Af, Bf2,-Btheta, -Bposix, -Bposiy, Bzoomx, Bzoomy)


# 变换类，输入变换数据后可以做变换
class Coordinate2DTrans():

    def __init__(self, td):
        self._f = td.f
        self._f_mat = td.f_mat
        self._offset = td.offset
        self._rota_mat = td.rota_mat
        self._f2_mat = td.f2_mat
        self._zoom =td.zoom

    def p2mat(self, x, y):
        return np.array([[x], [y]])#

    def mat2p(self, mat_p):
        # lst=mat_p.tolist()
        # return lst[0][0],lst[1][0]
        # print type(mat_p.tolist())
        return mat_p[0][0], mat_p[1][0]

    def trans_f(self, mat_p):
        result = np.dot(self._f_mat, mat_p)
        return result

    def trans_rota(self, mat_p):
        result = np.dot(self._rota_mat, mat_p)
        return result

    def trans(self, mat_p):
        result = np.dot(self._f_mat, mat_p)
        print("t")
        print(result)
        result = np.dot(self._rota_mat, result)
        print("r")
        print(result)
        result = np.add(self._offset, result)
        print("o")
        print(result)
        return result

    def trans2(self, mat_p):
        #输入反转变换
        result = np.dot(self._f_mat, mat_p)
        #print("t")
        #print(result)
        # 位移变换
        result = np.add(self._offset, result)
        #print(self._offset)
        #print("o")
        #print(result)
        # 旋转变换
        result = np.dot(self._rota_mat, result)
        #print("r")
        #print(result)
        # 输出放缩变换
        result = np.dot(self._zoom, result)
        # 输出反转变换
        result = np.dot(self._f2_mat, result)

        return result

    def trans_xy2xy(self, x, y):
        return self.mat2p(self.trans(self.p2mat(x, y)))


class MarvelmindTag2():


    def __init__(self):
        self.p1_x = self.p1_y = self.p2_x = self.p2_y = 0.0
        self.uwb_world_c2dt=None
        rospy.init_node('MarvelmindTag2',anonymous = True)


        rospy.Subscriber('/location_pos2', PointStamped, self.recv_point2)
        rospy.Subscriber('/location_pos', PointStamped, self.recv_point1)
        self.posepub2 = rospy.Publisher('/location_pos2_at_world', PointStamped, queue_size=10)
        self.posepub1 = rospy.Publisher('/location_pos_at_world', PointStamped, queue_size=10)
        self.rate = rospy.Rate(1)
        pass

    def recv_point1(self,data):
        #print "p1"
        #print data.point
        if  self.uwb_world_c2dt is None:
            print("trans point1: gen_transform_from_params ...")
            return
        #print "p1"
        #print (data.point.x, data.point.y)
        x,y=self.uwb2world(data.point.x, data.point.y)
        #print(x, y)
        #data.point.x=x
        #data.point.y=y
        self.pub_position( x, y,data,self.posepub1)


    def recv_point2(self,data):
        if self.uwb_world_c2dt is None:
            print("trans point2: gen_transform_from_params ...")
            return
        x, y = self.uwb2world( data.point.x, data.point.y)
        print(x,y)
        self.pub_position(x, y, data, self.posepub2)
        #self.count_heading()


    def pub_position(self,x,y, data,pub):

        initpose = PointStamped()
        #initpose.header.stamp = rospy.get_rostime()
        initpose.header = data.header
        initpose.point.x = x
        initpose.point.y = y
        initpose.point.z = data.point.z

        pub.publish(initpose)

    def uwb2world(self,x, y):
        p_uwb = self.uwb_world_c2dt.p2mat(x, y)
        p_world = self.uwb_world_c2dt.trans2(p_uwb)  # 变换
        x1, y1 = self.uwb_world_c2dt.mat2p(p_world)

        #print(x1, y1)
        return x1, y1
    def gen_transform_from_params(self,params="1, 1, 0, 1.5, 1.2"):
        # =======================================================================
        #                            系统的应用
        # =======================================================================
        #              . uwb
        #            .   .       N
        #          .       .     |
        #        .           .   | world
        #      .               . .---------E
        # field
        #   A-----width-----B
        #   |               |
        # height            |
        #   |               |
        #   C---------------D

        # uwb转world
        uwb_world_td = Coordinate2DTransData()  # 创建转换数据
        uwb_world_td.set_coordinate_info(1, 1, 90, 1.5, -1.2)  # 坐标系关系
        self.uwb_world_c2dt = Coordinate2DTrans(uwb_world_td)  # 创建转换实例

    def run(self):

        while not rospy.is_shutdown():
                self.rate.sleep()
                if rospy.is_shutdown():
                    break



if __name__ == '__main__':


    try:
        m=MarvelmindTag2()
        m.gen_transform_from_params("-1, 1, 90, 1.5, 1.2")
        m.run()
    except rospy.ROSInterruptException:
        pass