setting up your robot with tf（二）

1，using the robot state publisher on your own robot

上面一节是使用TF树结构来表示不同的框架之间的关系，且这棵TF转换树是一棵单遍历的树，表示了不同框架之间的关系，我们写了一个节点，用来定义两个框架的关系，这样下次使用就能自动调用这种关系，这样tf就帮我们做了很重要的一份工作。这一节我们学习robot state publisher。当我们的机器人有很多个相关连的框架时，这样把它们所有的相关框架信息发布给tf即形成一棵tf结构树就变得十分麻烦，而robot state publisher就为你做了这份工作。即TF树帮助你避免每次都要调用计算框架之间的关系，只要定义一次就能自动调用使用，而robot state publisher却能帮我们将复杂的框架信息发布给TF即做一棵TF树，这样就能自动调用了（太棒了）。

robot state publisher帮你发布状态消息给tf转换库。它内部有一个机器人运动模型，这样当给定机器人的节点位置时，robot state publisher能计算和发布这个机器人的每个连杆的3D位置。使用robot state publisher可以通过一个节点或者通过一个库。

使用robot state publisher最简单的一种方法是将它当作一个节点，对普通的用户来说，这是推荐的方法，运行它需要两样东西：第一个是一个urdf文件用来描述机器人，第二个是一个source文件用来发布节点位置信息作为一个sensor_msgs/JointState.

如何为robot_state_publisher来配置参数和主题呢？

订阅joint_states主题，设置tf前缀为命名空间用来发布转换消息，设置发布频率（默认为50Hz）

当建立好一个urdf机器人描述文件，且建立了一个节点位置信息的source文件，再创建一个简单的.launch文件即可。

这样就将位置信息通过robot state publisher（包含在source文件中）发布给了tf，tf形成了一棵tf结构树，如果用rviz就可调用tf来查看所生成的tf树，即robot state publisher通过计算节点关节信息来生成相应的tf结构转换树，解决了复杂的结构不用像上节tf_broadcaster一个个来发布框架相应关系，可以直接运行一个类似robot state publisher的节点来使用它，生成一棵tf转换树，对于以后的使用能自动调用转换。

2，using urdf with robot_state_publisher

2.1, create the URDF file即一个R2D2简单机器人的URDF描述文件

2.2Publishing the state为了指定机器人的状态，我们必须指定三个节点关节和总里程，先创建一个节点名称是state_publisher

注意创建source文件后要在CmakeLists.txt文件里添加可执行文件名称state_publisher，再用catkin_make进行编译，生成一个可执行文件。还要注意依赖包，应该包括tf,geometry_msgs ,roscpp,否则会出现错误，transformBroadcaster（）会找不到（）。

2.3创建一个launch文件，这个文件调用了两个节点robot_state_publisher和state_publisher(刚才我们所建立的节点)

2.4运行launch文件，然后打开rviz，调用机器人模型及TF后能看到结果：机器人绕着一个odom坐标运动，机器人上有TF框架显示。

3，分析source文件：

state_publisher.cpp:

   1 #include <string>
   2 #include <ros/ros.h>
   3 #include <sensor_msgs/JointState.h>
   4 #include <tf/transform_broadcaster.h>
//这里要用到转换发布对象TransformBroadcaster,所以要添加头文件
   5 
   6 int main(int argc, char** argv) {
   7     ros::init(argc, argv, "state_publisher");
//初始化ROS命名节点为state_publisher
 8     ros::NodeHandle n;
//NodeHandle是与ROS交流最主要的接入点
 9     ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);
//ROS发布消息给主题joint_states,消息类型为sensor_msgs::JointState
 10     tf::TransformBroadcaster broadcaster;
//tf的转换对象
 11     ros::Rate loop_rate(30);
  12 
  13     const double degree = M_PI/180;
  14 
  15     // robot state机器人状态
  16     double tilt = 0, tinc = degree, swivel=0, angle=0, height=0, hinc=0.005;
  17 
  18     // message declarations消息声明
  19     geometry_msgs::TransformStamped odom_trans;
  20     sensor_msgs::JointState joint_state;
//定义了两个消息odom_trans和joint_state
 21     odom_trans.header.frame_id = "odom";
  22     odom_trans.child_frame_id = "axis";
//消息odom_trans下的两个frame,父框架为odom，子框架为axis
 23 
  24     while (ros::ok()) {
  25         //update joint_state更新关节点状态
  26         joint_state.header.stamp = ros::Time::now();
  27         joint_state.name.resize(3);
  28         joint_state.position.resize(3);
  29         joint_state.name[0] ="swivel";
  30         joint_state.position[0] = swivel;
  31         joint_state.name[1] ="tilt";
  32         joint_state.position[1] = tilt;
  33         joint_state.name[2] ="periscope";
  34         joint_state.position[2] = height;
  35 
  36 
  37         // update transform更新转换，使绕一个半径为2的圆移动
  38         // (moving in a circle with radius=2)
  39         odom_trans.header.stamp = ros::Time::now();
  40         odom_trans.transform.translation.x = cos(angle)*2;
  41         odom_trans.transform.translation.y = sin(angle)*2;
  42         odom_trans.transform.translation.z = .7;
  43         odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle+M_PI/2);
  44 
  45         //send the joint state and transform发送关节点消息并且发布odom_trans转换消息，发送关节点的消息是state_publisher通过主题joint_state发送
             //给robot_state_publisher的，而odom与axis框架之间的关系是通过节点state_publisher发送的，即节点state_publisher完成了转换树的根部分，而robot_state_publisher
            //则完成了机器人各个框架之间的转换树。注意根树即框架odom与axis之间的关系是一个圆形的关系，故能看到机器人绕着odom坐标做圆形轨迹运动。
 46         joint_pub.publish(joint_state);
  47         broadcaster.sendTransform(odom_trans);
  48 
  49         // Create new robot state
  50         tilt += tinc;
  51         if (tilt<-.5 || tilt>0) tinc *= -1;
  52         height += hinc;
  53         if (height>.2 || height<0) hinc *= -1;
  54         swivel += degree;
  55         angle += degree/4;
  56 
  57         // This will adjust as needed per iteration
  58         loop_rate.sleep();
  59     }
  60 
  61 
  62     return 0;
  63 }

总结：用tf tool查看框架之间的关系，即用 $rosrun tf view_frames来查看， 可以看出新生成的框架odom与机器人的轴框架axis之间的关系是由节点state_publisher发布的，且它们的关系不是简单的线性关系，而是一个圆形的关系，而机器人本身的各个框架之间的对应关系是由robot_state_publisher生成的（是不是自动生成的?），这样就由节点state_publisher和节点robot_state_publisher生成了一棵包括odom框架的完整的tf转换树，注意state_publisher与robot_state_publisher之间的通信也是由一个主题joint_state发布消息形成的，这个可以使用rqt工具来查看。