ROS TF2

  • 介绍tf2

 

  • 1、安装演示示例
  • 2、运行演示示例
  • 3、如何实现的
  • 4、tf2 工具
    • 4.1 view_frames
    • 4.2 tf_echo
    • 4.3 rviz
  • 编写tf2静态广播器(C ++)
    • 1、创建 learning_tf2 功能包
    • 2、How to broadcast transforms
      • 2.1 代码
      • 2.2 代码解释
    • 3、编译 运行
    • 4、检查结果
      • 5、发布静态转换更合适的方法

 

  • 编写 tf2 坐标变换 (上面为静态,此为动态)
    • 1、创建 learning_tf2 功能包
    • 2、代码
    • 3、代码解释
    • 4、运行例程
      •  1)编译
      •  2)为此bin文件 创建启动文件(launch)。 命名为start_demo.launch
      •  3)运行
      •  4)查看结果

官网教程链接

介绍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 代码

#include <ros/ros.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include 
#include <tf2/LinearMath/Quaternion.h>


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 <ros/ros.h>
#include <tf2_ros/static_transform_broadcaster.h>  // 这个是 核心
#include <geometry_msgs/TransformStamped.h>
#include
#include <tf2/LinearMath/Quaternion.h>


判断参数 对不对 参数应该有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 中发布静态 坐标变化

编写 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 <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <turtlesim/Pose.h>

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 <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <turtlesim/Pose.h>


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

写如下代码

  
     
    

    
    
    
    

    
    

  


3)运行

 $ roslaunch learning_tf2 start_demo.launch


在这里插入图片描述


4)查看结果

 $ rosrun tf tf_echo /world /turtle1


在这里插入图片描述