ros 坐标系间位姿变换节点pose2pose，线程处理数据python示例

ros 坐标系间位姿变换节点pose2pose，线程处理数据python示例

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

import rospy
import sys
#sys.path.append('.')
import cv2
import os
import math
import numpy as np
import tf
from geometry_msgs.msg import Pose
#from geometry_msgs.msg import PoseStamped
#from geometry_msgs.msg import PointStamped 
from geometry_msgs.msg import PoseWithCovarianceStamped 

from tf2_geometry_msgs import PoseStamped,PointStamped
import tf2_ros as tf2


import string
import json
import time

import threading
class PPDataTransPuber():
    def __init__(self,target_frame,pub,tfl,tbf,typ="pose"):
        self.buffchk={}
        self.tfl=tfl
        self.tbf=tbf
        self.Qpt=[]
        self.Qps=[]
        self.target_frame=target_frame
        self.typ= typ
        self.pub = pub
        
    def setPoint(self,pt):
        self.Qpt.append(pt)
        if len(self.Qpt)>500:
            self.Qpt.pop()

    def setPose(self,ps):
        self.Qps.append(ps)
        if len(self.Qps)>500:
            self.Qps.pop()


    def frameExists(self ,frame_name):
        if frame_name=="world":
            return True
        else:
            return self.tfl.frameExists(frame_name)


 
    def run(self):
        if self.typ== "point":
            while 1:
                if len(self.Qpt):
                    pt=self.Qpt.pop()

                    npt=self.transformPoint(self.target_frame, pt)
                    print npt
                else:
                    time.sleep(0.03)
                    pass
        elif self.typ== "pose":
            while 1:
                if len(self.Qps):
                    ps=self.Qps.pop()

                    nps=self.transformPose(self.target_frame, ps)
                    print nps
                    if nps:
                        self.pub.publish(nps)
                else:
                    time.sleep(0.03)
                    pass


    def transformPose(self, target_frame, ps):        
        try:
            #     5.调研订阅对象的 API 将 4 中的点坐标转换成相对于 world 的坐标
            pose_target = self.tbf.transform(ps,target_frame,rospy.Duration(1))
            #rospy.loginfo("转换结果:x = %.2f, y = %.2f, z = %.2f",
            #                pose_target.pose.position.x,
            #                pose_target.pose.position.y,
            #                pose_target.pose.position.z)
            return pose_target
        except Exception as e:
            #rospy.logerr("异常:%s",e)
            return None
            #res=tf_buffer.transform(p,"world")
    

    def transformPoint(self, target_frame, pt):
        
        #self.tfl.waitForTransform(target_frame, ps.header.frame_id, rospy.Time.now(), rospy.Duration(1.0))

        if self.frameExists(target_frame):# and self.frameExists(ps.header.frame_id)
            try:
                return self.tfl.transformPoint(target_frame,pt)
            except:
                return None
            pass

        else:
            #print (target_frame,self.tfl.frameExists(target_frame))
            #print (ps.header.frame_id,self.tfl.frameExists(ps.header.frame_id))
            return None




        #now = rospy.Time.now()
        #self.waitForTransform(target_frame, ps.header.frame_id, rospy.Time.now(), rospy.Duration(5.0))
        #ps.header.stamp = now
        #return TransformListener.transformPoint(self, target_frame, ps)


class Pose2Pose():


    def __init__(self):
        
        rospy.init_node('Pose2Pose',anonymous = True)
        self.rate_hz = rospy.get_param("~rate", 20)
        self.threshold_s = rospy.get_param("~threshold", 2)
        self.frames_target = rospy.get_param("~frames_target", "['uwb','map2d','map']")#"['map','map2d','map3d']"
        self.topic_name = rospy.get_param("~topic_name", "['/uwb_pose','/map2d_pose','/gnss_pose']")
        self.frames_target_list=eval(self.frames_target)
        self.topic_name_list=eval(self.topic_name)
        self.pubers=[]
        self.rate = rospy.Rate(self.rate_hz)
        self.th_objs=[]
        self.ths=[]
        #self.pose_msg=None
        self.pose_orientation=None
        self.pose_position=None


        #
        #self.rot_default=tf.transformations.quaternion_from_euler(0.0,0.0,0.0)#(0.0,0.0,83.0/180.0*3.14159)
        #print tf.transformations.quaternion_from_euler(0.0,0.0,83.0/180.0*3.14159)
        #self.pos_default=(0.0,0.0,0.0,0.0)#(0.5,0.5,0.0,0.0)
        #主变换
        self.tfl=tf.TransformListener()

        self.tbf = tf2.Buffer()
        self.tl = tf2.TransformListener(self.tbf)
        #self.tfl.waitForTransform( 'map','world', rospy.Time.now(), rospy.Duration(5.0))
        self.buffchk={}
        #兼容不同类型的话题poseWithCovarianceStamped
        rospy.Subscriber('/location_baselink_PoseWithCovarianceStamped', PoseWithCovarianceStamped, self.recv_pose_alltype) 
        rospy.Subscriber('/poseStamped', PoseStamped, self.recv_pose_alltype) 
        rospy.Subscriber('/pose', Pose, self.recv_pose_alltype) 
        
        print ("frames_target_list",self.frames_target_list)

        for i in range (len(self.frames_target_list)):
            frame_name=self.frames_target_list[i]
            topic_name=self.topic_name_list[i]

            puber = rospy.Publisher(topic_name, PoseStamped, queue_size=1)
            print puber
            self.pubers.append(puber)

            pdtp=PPDataTransPuber(frame_name,puber,self.tfl,self.tbf )      
            self.th_objs.append(pdtp)
            t = threading.Thread(target = pdtp.run)
            t.setDaemon(True)
            t.start()
            self.ths.append(t)
        pass

    #def get_pos(self,data):

    #    (roll, pitch, yaw) = euler_from_quaternion([data.orientation.x, data.orientation.y, data.orientation.z, data.orientation.w])

    #    rospy.loginfo("current position(x:%f,y:%f,z:%f),theta:%f", data.position.x, data.position.y, data.position.z, yaw)



    def recv_pose_alltype(self, msg):
        if type(msg) is Pose:
            pose_orientation = msg.orientation
            pose_position = msg.position
        elif type(msg) is PoseStamped:
            pose_orientation = msg.pose.orientation
            pose_position = msg.pose.position
        elif type(msg) is PoseWithCovarianceStamped:
            pose_orientation = msg.pose.pose.orientation
            pose_position = msg.pose.pose.position

        #(pose_roll, pose_pitch, pose_yaw) = tf.transformations.euler_from_quaternion([pose_orientation.x, pose_orientation.y, pose_orientation.z, pose_orientation.w])
        
        #(trans,rot) = self.tfl.lookupTransform("uwb", msg.header.frame_id, rospy.Time(0))
        #pose_x=pose_position.x+trans[0]
        #pose_y=pose_position.y+trans[1]
        #pose_z=pose_position.z+trans[2]

        #pose_roll=pose_roll+rot[0]
        #pose_pitch=pose_pitch+rot[0]
        #pose_yaw=pose_yaw+rot[0]

        #rot_default=tf.transformations.quaternion_from_euler(pose_roll,pose_pitch,pose_yaw)
        #print(pose_x,pose_y,pose_z)
        #print rot_default
        p=PoseStamped()
        p.header=msg.header
        p.pose.position=pose_position
        p.pose.orientation=pose_orientation
        #p.header.stamp=rospy.Time()
        #p.header.frame_id='map'
        #p.pose.position.x=1
        #p.pose.position.y=1
        #p.pose.position.z=2
        #p.pose.orientation.x=0
        #p.pose.orientation.y=0
        #p.pose.orientation.z=0
        #p.pose.orientation.w=1

        #trans 是两个帧之间的转换关系，此处不用，直接屏蔽就行  
        #trans=tf_buffer.lookup_transform('world','rh_wrist',time=rospy.Time(0),timeout=rospy.Duration(5))

        #----------------------------------------------
        #from tf2_geometry_msgs import PoseStamped
        #----------------------------------------------
        '''
        tf_buffer=self.tbf 
        try:
            point_target = tf_buffer.transform(p,"uwb",rospy.Duration(1))
            rospy.loginfo("转换结果:x = %.2f, y = %.2f, z = %.2f",
                            point_target.pose.position.x,
                            point_target.pose.position.y,
                            point_target.pose.position.z)
            rospy.loginfo("转换结果:x = %.2f, y = %.2f, z = %.2f, w = %.2f",
                            point_target.pose.orientation.x,
                            point_target.pose.orientation.y,
                            point_target.pose.orientation.z,
                            point_target.pose.orientation.w)
        except Exception as e:
            rospy.logerr("异常:%s",e)
            #res=tf_buffer.transform(p,"world")
        #'''
        #----------------------------------------------  


        #ps = PointStamped()
        #ps.header = msg.header
        #ps.point = pose_position
        #for o in self.th_objs:
        #    o.setPoint(ps)


        for o in self.th_objs:
            o.setPose(p)    
        '''
        ps_map = self.transformPoint("base_link",ps)
        ps_map2d = self.transformPoint("map2d",ps)
        ps_map3d = self.transformPoint("map3d",ps)
        print "---------------------------"
        print(ps_map)
        print(ps_map2d)
        print(ps_map3d)
        print "---------------------------"
        '''


        #now = rospy.Time.now()
        #self.waitForTransform(target_frame, ps.header.frame_id, rospy.Time.now(), rospy.Duration(5.0))
        #ps.header.stamp = now
        #return TransformListener.transformPoint(self, target_frame, ps)



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



if __name__ == '__main__':


    try:
        m=Pose2Pose()
        m.run()
    except rospy.ROSInterruptException:
        pass