ROS中创建tf的两种方法对比

第一种方法：来源于 http://wiki.ros.org/urdf/Tutorials/Using urdf with robot_state_publisher
cpp源码如下：（相比较于官方的源码，删去了sensor_msgs::JointState部分，只考虑tf的创建与发布）

int main(int argc, char** argv) {
    ros::init(argc, argv, "state_publisher");
    ros::NodeHandle n;
    tf::TransformBroadcaster broadcaster;

    const double degree = M_PI/120;
    double angle = 0;

    // message declarations
/*    This expresses a transform from coordinate frame header.frame_id 
                               to the coordinate frame child_frame_id*/
    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id  = "box";

    ros::Rate loop_rate(30);
    while (ros::ok()) {
        // update transform
        // (moving in a circle with radius=2)
        odom_trans.header.stamp = ros::Time::now();
        odom_trans.transform.translation.x = cos(angle)*2;  //公转半径
        odom_trans.transform.translation.y = sin(angle)*2;  //公转半径
        odom_trans.transform.translation.z = 0;
        odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle+M_PI/2);

        //send the joint state and transform
        broadcaster.sendTransform(odom_trans);

        angle += degree;

        // This will adjust as needed per iteration
        loop_rate.sleep();
    }
    return 0;
}

这段代码的中心在于：

• 创建tf的发布者
• tf::TransformBroadcaster broadcaster;
• 创建tf数据
• geometry_msgs::TransformStamped odom_trans;
• 设置odom_trans的内容
• rosmsg show geometry_msgs/TransformStamped
• 发布tf变换
• broadcaster.sendTransform(odom_trans);

这里需要注意的是，这个包的src里只有这一个cpp文件，那么发布的tf变换由谁接受呢？
launch源码如下：

<launch>
        <param name="robot_description" command="cat $(find r2d2)/model.xml" />
        <node name="robot_state_publisher"  pkg="robot_state_publisher" type="state_publisher" />
        <node name="state_publisher"        pkg="r2d2"                  type="state_publisher" />

        <node name = "rviz" pkg = "rviz" type = "rviz" />

</launch>

可以看到，这个包一共开启了三个node。除去刚刚cpp文件创建的state_publisher的node之外。rviz的node用来显示现象。并无太多可说。
robot_state_publisher包用来发布机器人的状态。它一共有两个输入。一个是urdf文件表示机器人。这里从param_server中的robot_description读取。以下是ros.wiki上对于这个包的解释。

This package allows you to publish the state of a robot to tf. Once the state gets published, it is available to all components in the system that also use tf. The package takes the joint angles of the robot as input and publishes the 3D poses of the robot links, using a kinematic tree model of the robot. The package can both be used as a library and as a ROS node.

robot_state_publisher uses the URDF specified by the parameter robot_description and the joint positions from the topic joint_states to calculate the forward kinematics of the robot and publish the results via tf.

需要注意的一点是，ros提供的另外一个包joint_state_publisher可以发布joint_states主题下的消息，所以两个包往往一起使用。
第二种方法：来源于 http://www.guyuehome.com/355
源码如下：
tf_broadcaster.cpp

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv)
{
    ros::init(argc, argv, "robot_tf_publisher");
    ros::NodeHandle n;
    tf::TransformBroadcaster broadcaster;

    ros::Rate r(100);
    while(n.ok()){
        broadcaster.sendTransform
        (
            tf::StampedTransform
            (
                tf::Transform( tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2) ),
                ros::Time::now(),
                "base_link",
                "base_laser"
            )
        );
        r.sleep();
    }
}

tf_listener.cpp

#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_listener.h>

void transformPoint(const tf::TransformListener& listener){
    //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame
    geometry_msgs::PointStamped laser_point;
    laser_point.header.frame_id = "base_laser";
    //we'll just use the most recent transform available for our simple example
    laser_point.header.stamp = ros::Time();
    //just an arbitrary point in space
    laser_point.point.x = 1.0;
    laser_point.point.y = 0.2;
    laser_point.point.z = 0.0;

    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_ERROR(
            "Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what()
            );
    }
}

int main(int argc, char** argv){
    ros::init(argc, argv, "robot_tf_listener");
    ros::NodeHandle n;
    tf::TransformListener listener(ros::Duration(10));

    //we'll transform a point once every second
    ros::Timer timer = n.createTimer(
        ros::Duration(1.0),
        boost::bind( &transformPoint, boost::ref(listener) )
        );

    ros::spin();
}

代码的中心在于：tf_broadcaster.cpp中的

broadcaster.sendTransform
        (
            tf::StampedTransform
            (
                tf::Transform( tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2) ),
                ros::Time::now(),
                "base_link",
                "base_laser"
            )
        );

用来发布一个tf变换的位姿矩阵
以及tf_listener.cpp中的

listener.transformPoint("base_link", laser_point, base_point);

进行tf变换。
需要注意的是，这里的

void transformPoint(const tf::TransformListener& listener)

是一个callback函数。它的调用是由

n.createTimer

函数执行的。
这里的tf变换一共有两个输入。一个是变换需要的位姿矩阵，一个是需要变换的点。位姿矩阵是在tf_broadcaster.cpp中的

 tf::Transform( tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2) ),

设置完成。
第二个要变换的点，是在tf_listener.cpp中

    geometry_msgs::PointStamped laser_point;

中设置的。