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