ros中tf的新的学习感悟

tf是坐标系变换中常用到的工具，今天为几个关键的函数来解释一下用法：

一．广播tf

往tf中发布你建立的坐标系以及坐标系之间的变化。步骤如下： 1.定义一个广播，相当于发布话题时定义一个发布器，还是以官方的小乌龟例程为例：

static tf::TransformBroadcaster br；

2.定义存放转换信息（平动，转动）的变量

tf::Transform transform；

3.求得变换矩阵中的t

transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );

需要注意： （1）setOrigin()函数的参数类型需要为tf::Vector3类型 （2）假设是要发布一个子坐标系为”turtle1”父坐标系为“world”，那么其中(msg->x,msg->y,0.0)是指“turtle1”的坐标原点在“world”坐标系下的坐标。 4.求得旋转矩阵R

tf::Quaternion q; q.setRPY(0, 0, msg->theta); transform.setRotation(q)； 需要注意:

（1）setRPY()函数的参数为”turtle1”在“world”坐标系下的roll(绕X轴)，pitch(绕Y轴)，yaw(绕Z轴) （2）为了确保转换正确强烈建议，在转换完后，运行下程序，打开rviz下使用确认下转换是否正确，我在实验中有一次，明明依次填入了三个角度，但是在rviz下发现并不正确，pitch,yaw还存在一个负号的关系（楼主对旋转角的正负号向来头晕，所以都是直接打开rviz，直接试，直观而且准确）。 5.将变换广播出去

br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name))；

其中， （1）transform:存储变换关系的变量； （2）ros::Time::now():广播tf使的时间戳； （3）“world”:父坐标系的名字； （4）turtle_name:子坐标系的名字，这里因为子坐标系有两个，所以它定义了字符串用于存放名字，方便切换，通常我们把名字填到引号里就可以了。 总结一下，假设你在机器人上应用，如果你知道机器人的位置x,y,z,与三个旋转角roll,pitch,yaw就可以广播一个tf了，如果在是平面移动机器人，则只需要知道x,y与yaw即可。

#include <ros/ros.h> #include <tf/transform_broadcaster.h> //播放坐标系也就是播放机器人的位姿 #include <turtlesim/Pose.h>//tf中机器人的位姿 位置以及角度 这个属于消息类型 std::string turtle_name;//用于在命令行中输入zuo biao xi的名字 void poseCallback(const turtlesim::PoseConstPtr& msg){ static tf::TransformBroadcaster br;//播放者协助发布 tf::Transform transform;//变换对象,chu cun bianhuan de xinxi transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );//将位姿由二维变成3维 设置子坐标系在父坐标系中原点的坐标，即t tf::Quaternion q; q.setRPY(0, 0, msg->theta); transform.setRotation(q); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));//在tf树中广播变换信息 将transform中当前的信息发布出去 } int main(int argc, char** argv){ ros::init(argc, argv, "my_tf_broadcaster"); if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;}; turtle_name = argv[1]; ros::NodeHandle node; ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);//订阅的话题名， ros::spin(); return 0; };

二．监听tf

通过监听tf，我们可以避免繁琐的旋转矩阵的计算，而直接获取我们需要的相关信息。 在监听中我最常用两个函数： （1）lookupTransform（）； （2）transformPoint（）； （一）lookupTransform（）；函数的使用 1.函数功能：可以过得两个坐标系之间转换的关系，包括旋转与平移。 2.主要步骤： （1）定义监听器；

tf::TransformListener listener

（2）定义存放变换关系的变量

tf::StampedTransform transform;

（3）监听两个坐标系之间的变换

try{ listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform); } catch (tf::TransformException &ex) { ROS_ERROR("%s",ex.what()); ros::Duration(1.0).sleep(); continue; }

注意： A.由于tf的会把监听的内容存放到一个缓存中，然后再读取相关的内容，而这个过程可能会有几毫秒的延迟，也就是，tf的监听器并不能监听到“现在”的变换，所以如果不使用try,catch函数会导致报错：

“world” passed to lookupTransform argument target_frame does not exist. ”

并且会导致程序挂掉，使用try,catch之后就OK了。

B.lookupTransform()函数参数说明 a.官网的例程说的是实现从“/turtle2”到“/turtle1”的转换，但实际中使用时，我发现转换得出的坐标是在“/turtle2”坐标系下的 b.不可以把ros::Time(0)改成ros::time::now()，因为监听做不到实时，会有几毫秒的延迟。ros::Time(0)指最近时刻存储的数据，ros::time::now()则指当下，如果非要使用ros::time::now,则需要结合waitForTransform()使用，具体见: http://wiki.ros.org/tf/Tutorials/tf and Time (C++) 实际中ros::Time(0)大多数情况下可以满足要求。 c.最后转换关系存入transform中

（4）使用变换

transform.getOrigin().x() transform.getOrigin().y(),

可以得到在turtle1坐标系的原点，在turtle2坐标系下的位置。

transform.getRotation().getW(); transform.getRotation().getX(); transform.getRotation().getY(); transform.getRotation().getZ();

可以得到旋转的四元数，如何直接得到偏航角因为没有用到这个功能所以也就没有去找相关的函数。

#include <ros/ros.h> #include <tf/transform_listener.h>//接收者 #include <geometry_msgs/Twist.h>//消息类型 #include <turtlesim/Spawn.h>//服务类型 int main(int argc, char** argv){ ros::init(argc, argv, "my_tf_listener"); ros::NodeHandle node; ros::service::waitForService("spawn");//等待，直到服务“spawn”出现，只有这样才能请求此服务再产生一个turtle 此服务没有出现就一只等待 ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("spawn");//客户端来调用服务 turtlesim::Spawn srv;//这个服务对象包含请求和响应两部分。 add_turtle.call(srv);//调用此服务就是为了产生一个新的乌龟 ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);//这里来决定发布的速度信息是给谁的 tf::TransformListener listener;//tf订阅者 一旦这个对象创建了，就开始接收坐标系间的变化信息 ros::Rate rate(10.0); while (node.ok()){ tf::StampedTransform transform;//储存变换的信息 try{ listener.lookupTransform("/turtle2", "/carrot1", /*//targetframe sourceframe time transform 四个参数 transform是turtle1到turtle2的变换*/ ros::Time(0), transform); //Time(0)就是把最新的有效信息发布出去 在broadcaseter中是将当前信息发布出去 获得两个坐标系之间的变换关系R，t } catch (tf::TransformException &ex){ ROS_ERROR("%s",ex.what()); ros::Duration(1.0).sleep();//休眠一下，休眠时间为1s continue; } geometry_msgs::Twist vel_msg; //从transform中得到消息，然后发布给turtle2 vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),//实例化这个消息 transform.getOrigin().x()); vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2));//pow()是平方函数 turtle_vel.publish(vel_msg);//发布消息使turtle2运动 rate.sleep(); } return 0; }; /*调用服务spawn，然后再生一只乌龟*/ /*zhe li zui hou yong Rviz lai guankan*/

（二）transformPoint（）函数使用 在实际应用中我们肯定会需要把在一个坐标系下的点转换到另一个坐标系下，这就需要transformPoint（）函数。 1.函数功能：将一个坐标系下的点的坐标转换到另一个坐标系下。 2.函数参数：如

listener_.transformPoint("PTAM_world",m_normal_pose,pose_PTAM_world)；

（1）其中m_normal_pose数据类型为 geometry_msgs::PointStamped， 其中需要定义m_normal_pose.header.frame_id即该点所属的坐标系 （2）而“PTAM_world”则指，我要将m_normal_pose转换到“PTAM_world”坐标系下。 pose_PTAM_world是转换的结果，数据类型同样为geometry_msgs::PointStamped。 3.使用例程：

geometry_msgs::PointStamped turtle1; turtle1.header.stamp=ros::Time(); turtle1.header.frame_id="turtle1"; turtle1.point.x=1; turtle1.point.y=2; turtle1.point.z=3; geometry_msgs::PointStamped turtle1_world; try{ listener_.transformPoint("PTAM_world",turtle1,turtle1_world); } catch (tf::TransformException &ex) { ROS_ERROR("%s",ex.what()); ros::Duration(1.0).sleep(); }

说明：同样因为延时，turtle1.header.stamp不能为ros::Time::now();否则会出类似错误

[ERROR] [1456669076.279804500]: Lookup would require extrapolation into the future. Requested time 1456669076.279616253 but the latest data is at time 1456669076.159341977, when looking up transform from frame

/* 引入一个新的消息类型 std_msgs/Header header uint32 seq //频率 这个每被引用一次就加1 一次 time stamp //时间 string frame_id //坐标系id geometry_msgs/Point point float64 x float64 y float64 z */ #include <ros/ros.h> #include <geometry_msgs/PointStamped.h> #include <tf/transform_listener.h> void transformPoint(const tf::TransformListener& listener) { geometry_msgs::PointStamped laser_point; laser_point.header.frame_id = "base_laser"; laser_point.header.stamp=ros::Time();//获得的最新的可用的变换 laser_point.point.x = rand()%2; laser_point.point.y = rand()%2; laser_point.point.z = rand()%2; try{ geometry_msgs::PointStamped base_point; //实例化一个消息对象 Listener.transformPoint("base_link",laser_point,base_point);//第一个参数表示目标坐标系 ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f", laser_point.point.x, laser_point.point.y, laser_point.point.z, base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec()); } catch(tf::TransformException &ex) { ROS_ERROE("%s",ex.what()); ros::Duration(1.0).sleep(); } } int main(int argc,char** argv) { ros::init(argc,argv,"tf_listener"); ros::Nodehandle nh; tf::TransformListener listener(ros::Duration(10));//让订阅者订阅的时间，等待10s，等待超过10s还没接收到消息就放弃之前接受到的消息 tf::Timer timer=n.createTimer(ros::Duration(1),boost::bind(&transformPoint,boost::ret(listener))); //1s调用boost::bind(&transformPoint,boost::ret(listener)一次 绑定回调函数的地址 } 总结，只有开始使用tf了才能逐渐感受到它的方便，另外再建议tf一定要与rviz结合使用，真的直观又方便。