Gmapping从开始到放弃—写一个TF 广播

这是一个关于实现把机器人的位姿广播到TF中，这是对ROS 有一定的熟悉之后教程
（1）cd catkin_ws/src 进入我们的ROS 的工作空间
（2）catkin_create_pkg my_tf tf roscpp rospy turtlesim 这一句是新建一个ROS 的包，也就是一个ROS的工程，并添加他的依赖项，主要依赖tf和C++以及你可以使用python开发

(3) cd %YOUR_CATKIN_WORKSPACE_HOME%/
(4)catkin_make 编译一次
(5)source ./devel/setup.bash
然后在my_tf的src文件下新建一个文件，可以自由命名 但是后面也是要用的，这里就命名是turtle_tf_broadcaster.cpp
添加以下程序

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

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg){
  static tf::TransformBroadcaster br;//定义一个广播，相当于发布话题时定义一个发布器broadcaster
  tf::Transform transform;//定义存放转换信息（平动，转动）的变量
  transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) ); //设置坐标原点 2D位置并转换为3D位置，添加了一个z系，但是设为0.0
  tf::Quaternion q;  //声明一个四元数
  q.setRPY(0, 0, msg->theta); //这里因为turtlesim在二维平面所以只有RPY的Yaw的信息，其他两个都设置为0
  transform.setRotation(q); //设置旋转也就是把RPY信息转为四元数
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));//将变换广播出去 发布到tf广播中 StampedTransform四个参数我们可以知道 transform的setOrigin 和 setRotation就是设置小乌龟的位姿，Time时间戳也就是现在的时间， “world”我们需要将创建的link的父框架的名字传输过去，在这个例子中是world,最后，我们需要将创建的link的子框架的名字传输过去，在这里就是turtle本身

}

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_broadcaster"); ////初始化ros,命名节点为my_tf_broadcaster
  if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};
  turtle_name = argv[1];  //乌龟的名字可以输入

  ros::NodeHandle node; ////NodeHandle是与ROS系统交流的最主要的接入点，是一个句柄
  ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);////从master订阅某乌龟的“/pose”话题，当消息到来时，即当乌龟位置改变时产生新的消息时，ROS将会调用poseCallback 

  ros::spin();
  return 0;
};

总结: 假设你在机器人上应用，如果你知道机器人的位置x,y,z,与三个旋转角roll,pitch,yaw就可以广播一个tf了，如果在是平面移动机器人，则只需要知道x,y与yaw即可
（6）在CMakeLists.txt添加如下的两行

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

（7）catkin_make 编译
编译完成之后，会发现会生成一个二进制文件，这个文件就是生成的可执行文件
（8）在my_tf文件下新建一个文件命名为launch之后新建一个命名为
start_demo.launch，并把下面的这段代码复制进去

<launch>
    <!-- Turtlesim Node-->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
    <!-- 解释一下，这个是启动小乌龟的-->

    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
    <!-- 这是启动移动小乌龟需要控制的键盘-->
    <!-- Axes -->
    <param name="scale_linear" value="2" type="double"/>
    <param name="scale_angular" value="2" type="double"/>
<!-- 以上是对小乌龟的线速度和角速度的一个尺度的参数 -->
    <node pkg="my_tf" type="turtle_tf_broadcaster"
          args="/turtle1" name="turtle1_tf_broadcaster" />
    <node pkg="my_tf" type="turtle_tf_broadcaster"
          args="/turtle2" name="turtle2_tf_broadcaster" />
<!-- 是程序中我们需要输入的需要创建的link的子框架的名字传输过去-->
  </launch>

（9）运行此程序
roslaunch learning_tf start_demo.launch

（10 ）箭头按键就可以移动小乌龟，我们可以监听TF广播
rosrun tf tf_echo /world /turtle1


还有一篇在博客园的TF总结：
http://www.cnblogs.com/li-yao7758258/p/7672521.html