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)查看结果
- 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