#include "ros/ros.h"
#include <tf/transform_listener.h>
#include <tf/tf.h>

int main( int argc, char* argv[] )
{
    ros::init(argc, argv, "test_tf");
    ros::NodeHandle nh;

    tf::TransformListener listener;
    geometry_msgs::PoseStamped in_pose;
    in_pose.header.frame_id = "parent";
    in_pose.header.stamp = ros::Time(0);
    in_pose.pose.position.x = 1;
    in_pose.pose.position.y = 2;
    in_pose.pose.position.z = 0;

    in_pose.pose.orientation.x = 0;
    in_pose.pose.orientation.y = 0;
    in_pose.pose.orientation.z = 0.344215153;
    in_pose.pose.orientation.w = 0.93889079678;
    ros::Rate r(10);
    while(nh.ok() )
    {
        out_pose.header.frame_id = "child";
        // listener->waitForTransform("child", "parent", ros::Time(0),  ros::Duration(1) );
        listener->transformPose("child", in_pose, out_pose);
        r.sleep();
    }
}

如果没有while循环，只是一个transformPose，运行就会报错，找不到child，即使tf关系是正常的。如果还是报错，需要加上waitForTransform一行

问题

tf使用transformPose报错: Exception in transformPose: Quaternion malformed, magnitude: 0 should be 1.0

源码在此，原因是四元数平方和偏离1太多

inline void assertQuaternionValid(const geometry_msgs::Quaternion & q)
{
   if(std::fabs(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w - 1) > 0.01)
   {
     std::stringstream ss;
     ss << "Quaternion malformed, magnitude: " << q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w << " should be 1.0" <<std::endl;
     throw tf::InvalidArgument(ss.str());
   }
};

python中使用transformPose

import rospy
import tf
from tf import TransformListener

quaternion = tf.transformations.quaternion_from_euler(0, 0, self.ang, axes='sxyz')
self.temp_pose.pose = Pose(Point(self.goal_x, self.goal_y, 0.000), Quaternion(quaternion[0],quaternion[1],quaternion[2],quaternion[3]))
self.temp_pose.header.frame_id = 'base_link'
# 解决时间戳的问题
self.temp_pose.header.stamp = rospy.Time(0)
try:
    self.tf_listener_.waitForTransform("odom", "base_link", rospy.Time(0), rospy.Duration(2.0) )
    self.goal = self.tf_listener_.transformPose('odom', self.temp_pose)
except (tf.LookupException, tf.ConnectivityException):
    rospywarn('tranform frame failed !')

开始总是出现target_frame does not exist的错误，但是用tf_echo明明可以看到两个坐标系，于是加上了waitForTransform，没有问题了。但是又出现了那个时间戳范围不足的问题，还是Time(0)解决

参考：
tf listener cant look up transform frame