setting up your robot with tf（2）

2 writing code（C++）

在上一节的基础上，我们来写一个小例子。为了实现相对坐标系之间的转换，我们要做的第一件事情是创建一个node去发布变换，其次，需要创建一个node去接收ROS发布的数据，并且利用tf去转换这个数据。

我们从创建这个package开始。这个package的名称为robot_setup_tf，它依赖于roscpp ,tf , geometry_msgs.

$ cd %TOP_DIR_YOUR_CATKIN_WS%/src$ catkin_create_pkg robot_setup_tf roscpp tf geometry_msgs

注意：要在catkin_ws中执行以上代码。

Alternative in fuerte, groovy and hydro: there is a standard robot_setup_tf_tutorial package in the navigation_tutorialsstack. You may want to install by following (%YOUR_ROS_DISTRO% can be { fuerte, groovy } etc.):

$ sudo apt-get install ros-%YOUR_ROS_DISTRO%-navigation-tutorials 

3 发布一个变换（broadcasting a transform）

在完成package以后，这里创建一个node去完成消息发布 base_laser------>  base_link 

创建如下代码：robot_setup_tf/src/tf_broadcaster.cpp

 #include <ros/ros.h> #include <tf/transform_broadcaster.h>//要使用TransformBroadcaster来发布转换消息，这样就必须包括tf包里面的tranform_broadcaster.h头文件    int main(int argc, char** argv){      ros::init(argc, argv, "robot_tf_publisher");//初始化ros      ros::NodeHandle n;      ros::Rate r(100);     tf::TransformBroadcaster broadcaster;//创建一个TransformBroadcaster对象来发送base_link转换消息给base_laser     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"));       //用TransformBroadcaster来发布一个转换，需要五个参数：       //第一个为旋转变换，由btQuaternion指定，其中btQuaternion包括pitch,roll,yaw       //第二个为一个平移转换，由tVector3指定，对应robot base的laser的偏差为x轴偏差为10cm，z轴偏差为20cm;       //第三个为发布一个时间戳，这里设置为ros::Time::now(),       //第四个为父节点的名称,这里为base_link；       //第五个为子节点的名称，这里为base_laser       r.sleep();       }   }

4 使用变换（using a transform）

这里，我们将创建一个node利用上节发布的信息，把一个base_laser坐标系下的点转换到base_link坐标系下。

创建如下代码：robot_setup_tf/src/tf_listener.cpp

  #include <ros/ros.h>   #include <geometry_msgs/PointStamped.h>   #include <tf/transform_listener.h>//获得TransformListener来自动订阅转换消息，也即订阅到TransformBroadcaster发布的消息   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";     //创建一个geometry_msgs::PointStamped的点，其中Stamped意思是包含一个头文件，允许我们连接一个timestamp时间戳和一个frame_id框架id,其中timestamp设为     //ros::Time(),而frame_id设为base_laser,并将它的点值设为以下那些值x:1.0,y:0.2,z:0.0;    //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());//输出，类似与cout  及  printf    }    //为了将base_laser转换为base_link框架，需要使用TransformListener对象，并调用它的转换点函数transformPoint(),里面含有三个参数：    //想转换点的框架的名称这里为base_link，我们从哪里转换的点这里为laser_point,转换后存储点这里为base_point    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();  }

5 运行（Building the Code）

打开CMakeList.txt，添加以下代码到文件底部：

add_executable(tf_broadcaster src/tf_broadcaster.cpp)add_executable(tf_listener src/tf_listener.cpp)target_link_libraries(tf_broadcaster ${catkin_LIBRARIES})target_link_libraries(tf_listener ${catkin_LIBRARIES})

然后，确保上述文件都保存完毕。

cd ~/catkin_ws/catkin_make

6 测试代码（Runing the code）

在第一个终端，运行如下指令：

$ source ./devel/setup.bash

<pre name="code" class="cpp" style="color: rgb(51, 51, 51); font-size: 14px; font-weight: bold; line-height: 20px; font-family: 'Helvetica Neue', Helvetica, Arial, sans-serif;">$ roscore

在第二个终端，运行如下指令：

$ source ./devel/setup.bash$ rosrun robot_setup_tf tf_broadcaster

在第三个终端，运行如下指令：

$ source ./devel/setup.bash$ rosrun robot_setup_tf tf_listener

如果成功运行，你将会看到如下画面：