文章目录
TF功能包的作用
tf工具包,底层实现采用的是一种树状数据结构,根据时间缓冲并维护多个参考系之间的坐标变换关系,可以帮助程序员在任意时间,将点、向量等数据的坐标,在两个参考系中完成坐标变换,默认记录10S内的坐标。
TF坐标变换的实现方式
- 广播TF变换
- 监听TF变换
查看TF树
rosrun rqt_tf_tree rqt_tf_tree
TF坐标系广播和监听的编程实现
实现一个海龟跟随另一个海龟
- 创建名为learning_tf的功能包
C++
1.代码
广播者
/*
该例程产生tf数据,并计算,发布turtle的速度指令
*/
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& msg)
{
//创建tf的广播器
static tf::TransformBroadcaster br;
//初始化tf数据
tf::Transform transform;
transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);
transform.setRotation(q);
//广播world与海龟坐标之间的tf数据
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
int main(int argc, char **argv)
{
//初始化ros节点
ros::init(argc, argv, "my_tf_broadcaster");
//输入参数作为海龟的名字
if(argc != 2)
{
ROS_ERROR("need turtle name as argument");
return -1;
}
turtle_name = argv[1];
//订阅海龟的位置话题
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
//循环等待回调函数
ros::spin();
return 0;
}
监听者
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
#include <cmath>
int main(int argc, char**argv)
{
//初始化ros节点
ros::init(argc, argv, "my_tf_listener");
//创建节点句柄
ros::NodeHandle n;
//请求产生turtle2产生
ros::service::waitForService("/spawn");
ros::ServiceClient add_turtle = n.serviceClient<turtlesim::Spawn>("/spawn");
turtlesim::Spawn srv;
add_turtle.call(srv);
//创建发布turtle2速度控制指令的发布者
ros::Publisher turtle_vel = n.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);
//创建tf的监听者
tf::TransformListener listener;
ros::Rate rate(10.0);
while(n.ok())
{
//获取turtle1 and turtle2 坐标系之间的tf数据
tf::StampedTransform transform;
try
{
listener.waitForTransform("/turtle2", "turtle1", ros::Time(0), ros::Duration(3.0));
//这个函数的意思是从当前时刻开始查看2个海龟的tf坐标,持续3秒,如果存在就直接跳过,不存在则退出
listener.lookupTransform("/turtle2", "turtle1", ros::Time(0), transform);
//这个函数的意思是把当前2海龟的tf坐标保存到transfrom中
}
catch(tf::TransformException &ex)
{
ROS_ERROR("%s", ex.what());
ros::Duration(1.0).sleep();
continue;
}
//根据turtle1与turtle2坐标之间的位置关系,发布turtle2的速度控制指令
geometry_msgs::Twist vel_msg;
vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x());
vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(),2) + pow(transform.getOrigin().y(),2));
turtle_vel.publish(vel_msg);
rate.sleep();
}
return 0;
}
2. 添加编译信息到MakeLists文件
3. 回到工作空间进行编译
catkin_make
4. 修改环境变量
source devel/setup.bash
5.运行(用到重映射)
roscore
启动master
rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster turtle1
这样节点的名字就变成了turtle1_tf_broadcast, 需要输入一个参数turtle1作为海龟的名字
执行一上指令就创立了turtle1和world的坐标关系,并且广播出去
rosrun learning_tf turtle_tf_listener
这个代码生成监听者接收广播,并且根据广播生成一个海龟2,跟随海龟1
python
1. 代码
- 广播器
#!/usr/bin/env python3
#-*- coding:utf-8 -*-
import roslib
roslib.load_manifest("learning_tf")
import rospy
import tf
import turtlesim.msg
def handle_turtle_pose(msg, turtlename):
br = tf.TransformBroadcaster()
br.sendTransform((msg.x, msg.y, 0),
tf.transformations.quaternion_from_euler(0, 0, msg.theta),
rospy.Time.now(),
turtlename,
"world")
if __name__ == "__main__":
rospy.init_node("turtle_tf_broadcaster")
turtlename = rospy.get_param('~turtle')
rospy.Subscriber("%s/pose" %turtlename,
turtlesim.msg.Pose,
handle_turtle_pose,
turtlename)
rospy.spin()
- 监听者代码
2. 修改环境变量
source devel/setup.bash
3. python代码运行重映射的方法
发布者需要运行2个小海龟,用一个代码的直接执行只有一个节点,所以通过重映射的方式改变节点的名字
完整的运行过程:
- 运行roscero
- 运行广播器1:广播world和海龟1的位置关系
rosrun learning_tf turtle_tf_broadcaster.py __name:=turtle1_tf_broadcaster _turtle:=turtle1
- 运行广播器2:广播world和海龟2的位置关系
rosrun learning_tf turtle_tf_broadcaster.py __name:=turtle2_tf_broadcaster _turtle:=turtle2
- 运行小海龟
rosrun turtlesim turtlesim_node
- 运行监听者,生成海龟2,监听海龟1和海龟2的位置关系,同时发布速度指令让海龟2向海龟1运动
rosrun learning_tf turtle_tf_listener.py
运行结果:
生成的新海龟向第一个海龟运行
总结
- 重映射
- 建立广播者
- 定义TF广播器(TransformBroadcaster)
- 创建坐标变换值
- 发布坐标变换值