ROS TF2

官网教程链接

介绍tf2

turtlesim的多机器人示例中，展示了tf2的一些功能。 也介绍了使用tf2_echo，view_frames和rviz。
使对tf2可以做的事情有个很好的了解。

1、安装演示示例

首先，获取所有依赖项并编译演示包。

$ sudo apt-get install ros-$ROS_DISTRO-turtle-tf2 ros-$ROS_DISTRO-tf2-tools ros-$ROS_DISTRO-tf

完毕后如上图

2、运行演示示例

现在，已经编译了turtle_tf2教程包，运行演示。

$ roslaunch turtle_tf2 turtle_tf2_demo.launch

有两只小乌龟，一只爬向了另一只

运行键盘控制乌龟移动的节点

rosrun turtlesim turtle_teleop_key

通过箭头来移动一只乌龟，可以看到另一只乌龟一直追着。

3、如何实现的

使用tf2库创建三个坐标系：世界坐标系、turtle1坐标系和turtle2坐标系。 使用tf2广播器发布turtle标系，并使用tf2侦听器计算turtle标系中的差异并移动一只乌龟跟随另一只乌龟。

4、tf2 工具

4.1 view_frames

view_frames 创建 tf2 通过ROS广播的所有坐标系的示意图，他们的相互关系。

$ rosrun tf2_tools view_frames.py

tf2侦听器正在侦听通过ROS广播的坐标系，并绘制坐标系连接方式的树。 要查看树：
可以通过指令

$ evince frames.pdf

这个指令会运行然后自动结束的。直接给你保存到home路径下，也不问一下。。。

在这个图上，可以看到tf2广播的三个坐标系：世界坐标系，turtle1坐标系和turtle2坐标系，并且世界坐标系是turtle1和turtle2坐标系的父级。 view_frames还报告一些诊断信息，这些信息有关何时接收到最旧和最新的坐标系转换，以及将tf2帧发布到tf2进行调试的速度。

4.2 tf_echo

tf_echo报告 通过ROS广播的任何两个坐标系之间的转换关系

指令格式

$ rosrun tf tf_echo [reference_frame] [target_frame]

看一下turtle2坐标系相对于turtle1坐标系的变换

$ rosrun tf tf_echo turtle1 turtle2

未运动时
移动时

当移动乌龟时，会看到随着两只乌龟相对移动，坐标变换发生了变化。

4.3 rviz

rviz是一种可视化工具，可用于检查tf2坐标系。 看看通过rviz的turtle坐标系。 让我们使用turtle_tf2配置文件，使用rviz的-d选项启动rviz：

$ rosrun rviz rviz -d `rospack find turtle_tf2`/rviz/turtle_rviz.rviz

控制乌龟移动，会看到 网格中的两个坐标系也动了，并且一个跟随一个。

编写tf2静态广播器（C ++）

下面 记录 如何将静态坐标系广播到tf2
之后编写 代码 实现 上面的演示 示例

1、创建 learning_tf2 功能包

首先，将创建一个catkin功能包，该功能包将用于学习tf2。 这个名为learning_tf2的软件包依赖tf2，tf2_ros，roscpp，rospy和turtlesim。

指令：

$ catkin_create_pkg learning_tf2 tf2 tf2_ros roscpp rospy turtlesim

2、How to broadcast transforms

2.1 代码

创建一个 cpp src/static_turtle_tf2_broadcaster.cpp

#include 
#include 
#include 
#include 
#include 


std::string static_turtle_name;

int main(int argc, char **argv)
{
  ros::init(argc,argv, "my_static_tf2_broadcaster"); //初始化ros节点
  
// 判断参数 对不对   参数应该有8个 
  if(argc != 8)
  {
    ROS_ERROR("Invalid number of parameters\nusage: static_turtle_tf2_broadcaster child_frame_name x y z roll pitch yaw");
    return -1;
  }
  if(strcmp(argv[1],"world")==0)  // 子坐标系 不能 是 world
  {
    ROS_ERROR("Your static turtle name cannot be 'world'");
    return -1;
  }



  static_turtle_name = argv[1];  // 取得静态 乌龟坐标系的名字
  
  static tf2_ros::StaticTransformBroadcaster static_broadcaster;//创建了一个 StaticTransformBroadcaster 对象  之后通过 这个来 发布  转换信息
  
  geometry_msgs::TransformStamped static_transformStamped; // 声明转换信息

  // 赋值 转换信息
  static_transformStamped.header.stamp = ros::Time::now();
  static_transformStamped.header.frame_id = "world";
  static_transformStamped.child_frame_id = static_turtle_name;
  static_transformStamped.transform.translation.x = atof(argv[2]);
  static_transformStamped.transform.translation.y = atof(argv[3]);
  static_transformStamped.transform.translation.z = atof(argv[4]);
  tf2::Quaternion quat;
  quat.setRPY(atof(argv[5]), atof(argv[6]), atof(argv[7]));
  static_transformStamped.transform.rotation.x = quat.x();
  static_transformStamped.transform.rotation.y = quat.y();
  static_transformStamped.transform.rotation.z = quat.z();
  static_transformStamped.transform.rotation.w = quat.w();

  
  // 通过  StaticTransformBroadcaster  把 转换信息发送出去
  static_broadcaster.sendTransform(static_transformStamped);
  
  // 终端 显示
  ROS_INFO("Spinning until killed publishing %s to world", static_turtle_name.c_str());
  ros::spin();
  return 0;
};

2.2 代码解释

包含 发布静态 坐标 转换 需要的文件

#include 
#include   // 这个是 核心
#include 
#include 
#include 

判断参数 对不对 参数应该有8个

// 判断参数 对不对   参数应该有8个 
  if(argc != 8)
  {
    ROS_ERROR("Invalid number of parameters\nusage: static_turtle_tf2_broadcaster child_frame_name x y z roll pitch yaw");
    return -1;
  }
  if(strcmp(argv[1],"world")==0)  // 子坐标系 不能 是 world
  {
    ROS_ERROR("Your static turtle name cannot be 'world'");
    return -1;
  }

声明一些 内容

static_turtle_name = argv[1];  // 取得静态 乌龟坐标系的名字
  
  static tf2_ros::StaticTransformBroadcaster static_broadcaster;//创建了一个 StaticTransformBroadcaster 对象  之后通过 这个来 发布  转换信息
  
  geometry_msgs::TransformStamped static_transformStamped; // 声明转换信息

赋值 转换信息

  // 赋值 转换信息
  static_transformStamped.header.stamp = ros::Time::now();
  static_transformStamped.header.frame_id = "world";
  static_transformStamped.child_frame_id = static_turtle_name;
  static_transformStamped.transform.translation.x = atof(argv[2]);
  static_transformStamped.transform.translation.y = atof(argv[3]);
  static_transformStamped.transform.translation.z = atof(argv[4]);
  tf2::Quaternion quat;
  quat.setRPY(atof(argv[5]), atof(argv[6]), atof(argv[7]));
  static_transformStamped.transform.rotation.x = quat.x();
  static_transformStamped.transform.rotation.y = quat.y();
  static_transformStamped.transform.rotation.z = quat.z();
  static_transformStamped.transform.rotation.w = quat.w();

通过 StaticTransformBroadcaster 把 转换信息发送出去

  // 通过  StaticTransformBroadcaster  把 转换信息发送出去
  static_broadcaster.sendTransform(static_transformStamped);

可以看到 main函数里没有 while 循环 ，所以只执行一次。
相当于 设定的 一个 静态（不变）的坐标系 和 世界坐标系的 转换（TF）

3、编译 运行

在 CMakeLists.txt 添加

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

编译

$ catkin_make

运行

$ roscore

rosrun learning_tf2 static_turtle_tf2_broadcaster mystaticturtle 0 0 1 0 0 0

4、检查结果

 $ rostopic echo /tf_static

结果
/tf_static 这个topic 可能会 存放 所有 通过 StaticTransformBroadcaster 静态坐标转换

5、发布静态转换更合适的方法

上面的代码 旨在说明如何使用StaticTransformBroadcaster发布静态转换。 在实际的开发过程中，不必自己编写此代码，而应优先使用专用的tf2_ros工具来编写代码。 tf2_ros提供了一个名为static_transform_publisher的可执行文件，可以用作命令行工具或可以添加到启动文件的节点。

有两种方式 一种 角度 的一种四元数的 用有几个参数来区分

static_transform_publisher x y z yaw pitch roll frame_id child_frame_id

Publish a static coordinate transform to tf2 using an x/y/z offset in meters and quaternion. 

通过 这中 launch 文件的 方式 ，直接在launch 中发布静态 坐标变化

<launch>
<node pkg="tf2_ros" type="static_transform_publisher" name="link1_broadcaster" args="1 0 0 0 0 0 1 link1_parent link1" />
</launch>

编写 tf2 坐标变换 （上面为静态，此为动态）

上面为 静态：两坐标变换关系 不变
此为 动态 ：两坐标变换关系 改变

下面记录 如何将坐标系广播到tf2。 在这种情况下，广播海龟移动时不断变化的坐标系。

1、创建 learning_tf2 功能包

首先，将创建一个catkin功能包，该功能包将用于学习tf2。 这个名为learning_tf2的软件包依赖tf2，tf2_ros，roscpp，rospy和turtlesim。

指令：

$ catkin_create_pkg learning_tf2 tf2 tf2_ros roscpp rospy turtlesim

2、代码

#include 
#include 
#include 
#include 
#include 

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg){
  static tf2_ros::TransformBroadcaster br;
  geometry_msgs::TransformStamped transformStamped;
  
  transformStamped.header.stamp = ros::Time::now();
  transformStamped.header.frame_id = "world";
  transformStamped.child_frame_id = turtle_name;
  transformStamped.transform.translation.x = msg->x;
  transformStamped.transform.translation.y = msg->y;
  transformStamped.transform.translation.z = 0.0;
  tf2::Quaternion q;
  q.setRPY(0, 0, msg->theta);
  transformStamped.transform.rotation.x = q.x();
  transformStamped.transform.rotation.y = q.y();
  transformStamped.transform.rotation.z = q.z();
  transformStamped.transform.rotation.w = q.w();

  br.sendTransform(transformStamped);
}

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf2_broadcaster");

  ros::NodeHandle private_node("~");
  if (! private_node.hasParam("turtle"))
  {
    if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};
    turtle_name = argv[1];
  }
  else
  {
    private_node.getParam("turtle", turtle_name);
  }
    
  ros::NodeHandle node;
  ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

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

3、代码解释

1）

#include 
#include 
#include 
#include 
#include 

tf2 软件包提供了 TransformBroadcaster 的实现，以帮助简化发布转换的任务。 要使用TransformBroadcaster，需要包含tf2_ros / transform_broadcaster.h头文件。

2）

  static tf2_ros::TransformBroadcaster br;

创建一个TransformBroadcaster对象，稍后将使用它发送tf。

3）

  geometry_msgs::TransformStamped transformStamped;
  
  transformStamped.header.stamp = ros::Time::now();
  transformStamped.header.frame_id = "world";
  transformStamped.child_frame_id = turtle_name;

创建一个Transform对象，并为其提供适当的数据。

给要发布的tf加一个时间戳，用当前时间ros :: Time :: now（）标记它。

设置要创建的link的父框架的名称，在本例中为“ world”

设置要创建的link的子节点的名称，这就是乌龟本身的名称。

4）

  transformStamped.transform.translation.x = msg->x;
  transformStamped.transform.translation.y = msg->y;
  transformStamped.transform.translation.z = 0.0;
  tf2::Quaternion q;
  q.setRPY(0, 0, msg->theta);
  transformStamped.transform.rotation.x = q.x();
  transformStamped.transform.rotation.y = q.y();
  transformStamped.transform.rotation.z = q.z();
  transformStamped.transform.rotation.w = q.w();

将3D乌龟位姿信息复制到3D变换（TF）中

5）

  br.sendTransform(transformStamped);

完成实际工作的地方。 使用TransformBroadcaster发送转换仅需要传递转换本身。

4、运行例程

1）编译

CMakeLists.txt 文件中加入

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

编译

 $ catkin_make

bin文件夹中应该有一个名为turtle_tf2_broadcaster的二进制文件。

2）为此bin文件 创建启动文件（launch）。 命名为start_demo.launch

新建文件夹 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="learning_tf2" type="turtle_tf2_broadcaster"
          args="/turtle1" name="turtle1_tf2_broadcaster" />
    <node pkg="learning_tf2" type="turtle_tf2_broadcaster"
          args="/turtle2" name="turtle2_tf2_broadcaster" />

  </launch>

3）运行

 $ roslaunch learning_tf2 start_demo.launch

4）查看结果

 $ rosrun tf tf_echo /world /turtle1