标签：view node spin 命令 ndt err exception scribe rtl
tf, 很神奇的一个东西...想象一个使用场景, 一个小车, 下面有几个轮子, 中间是个底盘, 上面有若干激光传感器/雷达传感器/接近开关, 为了在运动的时候, 把这些底盘上的东西跟着底盘一起运动, 使用tf就能解决这样的问题.
ok, 如果是固定在小车的一般东西, 倒也没啥, 如果小车上, 还有一个机械臂, 小车在前进的同时, 机械臂做圆周运动, 那么机械臂在物理空间(所谓world或者map)的运动, 就必须通过tf来简化跟实现了.
首先, 看看一个tf监听者的代码:
#include <ros/ros.h> #include <tf/transform_listener.h> #include <turtlesim/Velocity.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"); ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("spawn"); //获得"spawn"服务客户端
turtlesim::Spawn srv; add_turtle.call(srv); //开启服务 ros::Publisher turtle_vel = node.advertise<turtlesim::Velocity>("turtle2/command_velocity", 10); //广告一个矢量消息, 名字是"turtle2/command_velocity" tf::TransformListener listener; //new一个监听者.
ros::Rate rate(10.0);
//一秒刷新10次
while (node.ok()){ tf::StampedTransform transform;
//new一下带时间戳的tf try{ listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
//获取父节点的/turtle2, 子节点/turtle1的tf } catch (tf::TransformException ex){ ROS_ERROR("%s",ex.what()); ros::Duration(1.0).sleep(); } turtlesim::Velocity vel_msg;
//new一个矢量信息, 通常包括xyz跟rpy vel_msg.angular = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x());
//将龟1的广播出来的角度信息, 做一个转换 vel_msg.linear = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2));
//将龟1广播出来的线向信息, 做一个转换 turtle_vel.publish(vel_msg); //把这个消息广播出去.
rate.sleep(); } return 0; };
这个监听者基本上就是获取广播者(下面的例子)广播出来的龟1的tf信息做了一个转换, 然后通过topic publish出来: "turtle2/command_velocity"
广播者broadcaster:
#include <ros/ros.h> #include <tf/transform_broadcaster.h> #include <turtlesim/Pose.h> std::string turtle_name; //全局变量: 龟名 void poseCallback(const turtlesim::PoseConstPtr& msg){
//一旦有人发送位置信息的回调 static tf::TransformBroadcaster br; tf::Transform transform; transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
//将消息里面的x,y组成矢量类型数据, 扔进tf tf::Quaternion q; q.setRPY(0, 0, msg->theta);
//写入tf的rpy部分 transform.setRotation(q); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
//将tf组成待时间戳的StampedTransform格式发布出去,父frame叫world, 自己就叫龟1/龟2 } 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); //订阅者龟/pose的topic
ros::spin(); return 0; };
将两个龟节点, 都启动, 再加入一个控制龟1的运动的节点, 做成一个launch:
<launch> <!-- Turtlesim Node--> <node pkg="turtlesim" type="turtlesim_node" name="sim"/> <!--龟龟的模拟器--> <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
<!--龟龟的控制器--> <!-- Axes --> <param name="scale_linear" value="2" type="double"/> <param name="scale_angular" value="2" type="double"/> <node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" /> <node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" /> <!-- 分别起两个广播者, 分别形参是龟1/龟2-->
<node pkg="learning_tf" type="turtle_tf_listener" name="listener" />
<!-- 起一个监听者-->
</launch>
rosrun tf tf_monitor
rosrun tf tf_echo /map /odom
这两个命令可以帮助查看tf当前的状态跟关系.
下面这个命令可以将tf关系输出pdf文件:
rosrun tf view_frames
标签：view node spin 命令 ndt err exception scribe rtl
原文地址：http://www.cnblogs.com/Montauk/p/6914058.html
