ros uwb2world坐标转换python示例
import rospy
import sys
sys.path.append('.')
import cv2
import os
import math
import numpy as np
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
self.f2 = f2
self.theta = theta
self.offsetx = offsetx
self.offsety = offsety* f
self.zoomx = zoomx
self.zoomy = zoomy
self.f_mat = np.array([[1, 0], [0, 1 * f]])
self.f2_mat = np.array([[1, 0], [0, 1 * f2]])
self.offset = np.array([[self.offsetx], [self.offsety]])
self.zoom = np.array([[self.zoomx,0], [0,self.zoomy]])
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):
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):
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)
result = np.add(self._offset, result)
result = np.dot(self._rota_mat, 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):
if self.uwb_world_c2dt is None:
print("trans point1: gen_transform_from_params ...")
return
x,y=self.uwb2world(data.point.x, data.point.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)
def pub_position(self,x,y, data,pub):
initpose = PointStamped()
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)
return x1, y1
def gen_transform_from_params(self,params="1, 1, 0, 1.5, 1.2"):
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
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
