ros 将位姿转换为父子坐标系间的tf变换节点，pose2tf，附加调整功能python示例

ros 将位姿转换为父子坐标系间的tf变换节点，pose2tf，附加调整功能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 


import string
import json



class TFParentChild():


    def __init__(self):
        
        rospy.init_node('TFParentChild',anonymous = True)
        self.rate_hz = rospy.get_param("~rate", 20)
        self.threshold_s = rospy.get_param("~threshold", 2)
        self.frame_parent = rospy.get_param("~frame_parent", "parent")
        self.frame_child = rospy.get_param("~frame_child", "child")
        #兼容不同类型的话题
        rospy.Subscriber('/poseWithCovarianceStamped', PoseWithCovarianceStamped, self.recv_pose_alltype) 
        rospy.Subscriber('/poseStamped', PoseStamped, self.recv_pose_alltype) 
        rospy.Subscriber('/pose', Pose, self.recv_pose_alltype) 
        self.rate = rospy.Rate(self.rate_hz)

        rospy.Subscriber('/adjust_pose', Pose, self.adjust_slightly_pose) 
        #self.pose_msg=None
        self.pose_orientation=None
        self.pose_position=None

        self.threhold_recv_pose=0
        self.threhold_recv_pose_mx=self.rate_hz* self.threshold_s  # 10秒阈值
        self.threhold_recv_pose_adjust=0
        self.threhold_recv_pose_adjust_mx=self.rate_hz* self.threshold_s  # 10秒阈值
        #
        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.pose_roll=0.0
        self.pose_pitch=0.0
        self.pose_yaw=0.0
        self.pose_x=0.0
        self.pose_y=0.0
        self.pose_z=0.0
        #微调变换
        self.adjust_roll=0.0
        self.adjust_pitch=0.0
        self.adjust_yaw=0.0
        self.adjust_x=0.0
        self.adjust_y=0.0
        self.adjust_z=0.0

        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 adjust_slightly_pose(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

        (self.adjust_roll, self.adjust_pitch, self.adjust_yaw) = tf.transformations.euler_from_quaternion([pose_orientation.x, pose_orientation.y, pose_orientation.z, pose_orientation.w])
        self.adjust_x=pose_position.x
        self.adjust_y=pose_position.y
        self.adjust_z=pose_position.z

        self.threhold_recv_pose_adjust=self.threhold_recv_pose_adjust_mx

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

        (self.pose_roll, self.pose_pitch, self.pose_yaw) = tf.transformations.euler_from_quaternion([self.pose_orientation.x, self.pose_orientation.y, self.pose_orientation.z, self.pose_orientation.w])
        self.pose_x=self.pose_position.x
        self.pose_y=self.pose_position.y
        self.pose_z=self.pose_position.z
        self.threhold_recv_pose = self.threhold_recv_pose_mx




    def pub_tf(self):
        if self.threhold_recv_pose>0:
            self.threhold_recv_pose=self.threhold_recv_pose-1
            #有地图加载发布了tf变换
            rot = (self.pose_orientation.x, self.pose_orientation.y, self.pose_orientation.z, self.pose_orientation.w)
            pos = (self.pose_position.x,self.pose_position.y,0.0)
        else :
            #没有发布，默认地图和world变换
            rot=self.rot_default
            pos=self.pos_default

            self.pose_roll=0.0
            self.pose_pitch=0.0
            self.pose_yaw=0.0
            self.pose_x=0.0
            self.pose_y=0.0
            self.pose_z=0.0

        if self.threhold_recv_pose_adjust>0:
            self.threhold_recv_pose_adjust=self.threhold_recv_pose_adjust-1
        else :
            self.adjust_roll=0.0
            self.adjust_pitch=0.0
            self.adjust_yaw=0.0
            self.adjust_x=0.0
            self.adjust_y=0.0
            self.adjust_z=0.0

        
        pos =(self.pose_x+self.adjust_x,self.pose_y+self.adjust_y,self.pose_z+self.adjust_z)
        rot=tf.transformations.quaternion_from_euler(self.pose_roll+self.adjust_roll,self.pose_pitch+self.adjust_pitch,self.pose_yaw+self.adjust_yaw)

        br = tf.TransformBroadcaster()
        br.sendTransform(pos,
                         rot,
                         rospy.get_rostime(),
                         self.frame_child,
                         self.frame_parent) 


    def run(self):

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



if __name__ == '__main__':


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