在命令行工具中也有一个与transformcaster相类似的工具叫做static_transform_publisher,它能够接受命令行参数来接受位置信息、旋转信息、父框架、子框架以及周期信息,通常在launch文件中来指定。static_transform_publisher主要是变换两个固定坐标系之间的转换,eg:base_link和base_laser是底座与激光雷达之间的变换。
eg:
<node pkg="tf" type="static_transform_publisher" name="broadcaster" args="1 0 0 0 0 0 1 parent child 100">
下面来实现tf的订阅者
新建一个源文件turtle_tf_listener.cpp
在这个订阅者的任务里面主要是调用一个服务再生一个turtle,然后再发布cmd_vel这个主题在turtle2上面用于控制turtle2的轨迹,同时turtle2的轨迹是由transform_listener中得到的transform是由turtle1到turtle2的变换。
#include<ros/ros.h>
#include<tf/transform_listener.h>
#include<geometry_msg/Twist.h>
#include<turtlesim/Spawn.h>
int main(int argc,char **argv)
{
ros::init(argc,argv,"turtle_tf_listener");//节点名字turtle_tf_listener
ros::NodeHandle node;
//下面写服务,等待spawn服务,如果服务没有到达就一直等待
ros::service::waitForService("spawn");
//创建一个服务的客户端,其类型为turtlesim::Spawn,服务类型为spawn
ros::ServiceClient add_turtle=node.service<turtlesim::Spawn>("spawn");
//创建一个turtlesim::Spawn变量
turtlesim::Spawn srv;
//调用该服务
add_turtle.call(srv);
//下面来写一个publisher,其类型为geometry_msgs::Twist,发布到turtle2/cmd_vel上,消息队列为10
ros::Pubisher turtle_vel=node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel",10);
//下面创建一个transform_listener对象
tf::transformListener listener;
//指定一个频率
ros::Rate rate(10.0);
//判断节点是否被关闭
while(ros::ok())
{
//创建一个stampedtransform对象来储存变换的信息
tf::StampedTransform transform;
try //在try里面使用lookuptransform
{
//其参数使targetframe,sourceframe,time,以及transform它用来储存变换的信息。父框架是turtle2,子框架是turtle1,时间为ros::Time(0)是指把最新的有效信息发布出去,在之前的broadcater中用的是time.now()是把现在的信息发送出去。
listener.lookupTransform("/turtle2","/turtle1",ros::Time(0),)
}
catch(tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());//把异常显示在终端上
ros::Duration(1.0).sleep();//显示的时间为1s。
continue;
}
//从transform中得到一个Twist的消息,然后发布给turtle2,下面实例化一个geometry_msgs
geometry_msgs::Twist vel_msg;
//角速度z的值,先指定一个系数4.0,然后其计算方法为atan2。
vel_msg.angular.z = 4.0* atan2(transform.getOrigin().y(), transform.getOrigin().x());
//线速度只指定x,系数设为0.5.其计算方法是使用平方函数。
vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2));
//然后发布这个消息。
turtle_vel.publish(vel_msg);
rate.sleep();
}
return 1;
}
回顾一下上面程序包含了哪些内容:
首先,初始化节点,然后调用Spawn服务再生一只乌龟,接着再定义一个publisher发布消息在turtle2/cmd_vel上面,发布的消息为geometry_msg:Twist类型的vel_msg,其内容为transform储存的turtle2到turtle1的变换。
下面到CMakeList.txt里面添加内容
add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})
下面再来写一个launch文件。
demo.launch
<launch>
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
<node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf">
<node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf">
<node pkg="learning_tf" type="turtle_tf_listener" name="listener">
</launch>
launch文件的逻辑为首先启动turtlesim_node,然后再启动turtle_teleop_key通过按键来控制它的运动,然后再learning_tf中再生一只乌龟,有两只乌龟,在tf_listener中会发布turtle2/cmd_vel这个主题来控制turtle2的运动,在turtle_tf_broadcaster中同时启动两个节点,一个节点订阅的是turtle1的pose,另一个节点订阅的是turtle2的pose,然后在tf树中广播turtle1和turtle2的位置;然后listener订阅在世界坐标系中的turtle1个turtle2的相对位置,储存在transform中,通过transform中存储的位置信息转化为控制信息。
下面去编译,别忘了增加tf的依赖项。!!!!!!!!
运行roslaunch learning_tf demo.launch
运行仿真工具rosrun rviz rviz
运行之后把tf添加进去。记得把Fixed frame选择为world。