第七课 ROS的空间描述和变换

Posted 2020-09-12 郭润

在命令行工具中也有一个与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。