创建功能包-learning_tf
$ cd ~/catkin_ws/src
$ catkin_create_pkg learning_tf roscpp rospy tf turtlesim
如何创建一个tf广播器
*定义TF广播器(TransformBroadcaster)
*创建坐标变换值
*发布坐标变换(sendTransform)
创建tf广播器代码(C++)如下
1 /** 2 * 该例程产生tf数据,并计算、发布turtle2的速度指令 3 */ 4 5 #include <ros/ros.h> 6 #include <tf/transform_broadcaster.h> 7 #include <turtlesim/Pose.h> 8 9 std::string turtle_name; 10 11 void poseCallback(const turtlesim::PoseConstPtr& msg) 12 { 13 // 创建tf的广播器 14 static tf::TransformBroadcaster br; 15 16 // 初始化tf数据 17 tf::Transform transform; 18 transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) ); 19 tf::Quaternion q; 20 q.setRPY(0, 0, msg->theta); 21 transform.setRotation(q); 22 23 // 广播world与海龟坐标系之间的tf数据 24 br.sendTransform(tf::StampedTransform(transform, ros::Time::now( ), "world", turtle_name)); 25 } 26 27 int main(int argc, char** argv) 28 { 29 // 初始化ROS节点 30 ros::init(argc, argv, "my_tf_broadcaster"); 31 32 // 输入参数作为海龟的名字 33 if (argc != 2) 34 { 35 ROS_ERROR("need turtle name as argument"); 36 return -1; 37 } 38 39 turtle_name = argv[1]; 40 41 // 订阅海龟的位姿话题 42 ros::NodeHandle node; 43 ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &p oseCallback); 44 45 // 循环等待回调函数 46 ros::spin(); 47 48 return 0; 49 }; 50
首先程序要注意头文件的书写,能够是海龟正常运行,不管是海龟turtle1或海龟turtle2与word坐标系之间的位置关系都是通用的
程序,在程序中通过输入参数确定当前小海龟的名称,并且去跟word坐标系去建立关系,因此程序会运行两遍,但是我们需要注意的是,任意的一个ros节点它的节点名在整个ros环境中只能有一个,如果程序运行两遍,会因为节点名字一样冲突,因此用到了重映射。后面会详细说明
程序中将创建一个tf广播器,初始化tf数据,广播word与海龟坐标系之间的tf数据时需要设置一个时间戳(一般为10秒)。
创建tf监听器
如何实现一个tf监听器
*定义TF监听器(TransformListener)
*查找坐标变换(waitForTransform 、lookupTransform)
创建tf监听器代码(C++)
1 /** 2 * 该例程监听tf数据,并计算、发布turtle2的速度指令 3 */ 4 5 #include <ros/ros.h> 6 #include <tf/transform_listener.h> 7 #include <geometry_msgs/Twist.h> 8 #include <turtlesim/Spawn.h> 9 10 int main(int argc, char** argv) 11 { 12 // 初始化ROS节点 13 ros::init(argc, argv, "my_tf_listener"); 14 15 // 创建节点句柄 16 ros::NodeHandle node; 17 18 // 请求产生turtle2 19 ros::service::waitForService("/spawn"); 20 ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Sp awn>("/spawn"); 21 turtlesim::Spawn srv; 22 add_turtle.call(srv); 23 24 // 创建发布turtle2速度控制指令的发布者 25 ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist> ("/turtle2/cmd_vel", 10); 26 27 // 创建tf的监听器 28 tf::TransformListener listener; 29 30 ros::Rate rate(10.0); 31 while (node.ok()) 32 { 33 // 获取turtle1与turtle2坐标系之间的tf数据 34 tf::StampedTransform transform; 35 try 36 { 37 listener.waitForTransform("/turtle2", "/turtle1", ros::T ime(0), ros::Duration(3.0)); 38 listener.lookupTransform("/turtle2", "/turtle1", ros::Ti me(0), transform); 39 } 40 catch (tf::TransformException &ex) 41 { 42 ROS_ERROR("%s",ex.what()); 43 ros::Duration(1.0).sleep(); 44 continue; 45 } 46 47 // 根据turtle1与turtle2坐标系之间的位置关系,发布turtle2的速 度控制指令 48 geometry_msgs::Twist vel_msg; 49 vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), 50 transform.getOrigin().x()); 51 vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + 52 pow(transform.getOrigin().y(), 2)); 53 turtle_vel.publish(vel_msg); 54 55 rate.sleep(); 56 } 57 return 0; 58 };
通过tf监听器去监听turtle1和turtle2之间的坐标系关系,根据turtle1与turtle2坐标之间的关系,发布turtle2的速度控制指令。
配置tf广播器和监听器代码编译规则
配置CMakeLists.txt中的编译规则
*设置需要编译的代码和生成的可执行文件
*设置链接库
在CMakeLists.txt中应加入以下代码
add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})
add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})
编译并运行
$ cd ~/catkin_ws
$ catkin_make
$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun learning_tf turtle_tf_broadcaster__name:=turtle1_tf_broadcaster/turtle1
$ rosrun learning_tf turtle_tf_broadcaster__name:=turtle1_tf_broadcaster/turtle2
$ rosrun learning_tf turtle_tf_listener
$ rosrun turtlesim turtle_teleop_key
下面展示一下(python)的代码实现,其编译是一样的
创建tf广播器
1 #!/usr/bin/env python 2 # -*- coding: utf-8 -*- 3 # 该例程将请求/show_person服务,服务数据类型learning_service::Person 4 5 import roslib 6 roslib.load_manifest('learning_tf') 7 import rospy 8 9 import tf 10 import turtlesim.msg 11 12 def handle_turtle_pose(msg, turtlename): 13 br = tf.TransformBroadcaster() 14 br.sendTransform((msg.x, msg.y, 0), 15 tf.transformations.quaternion_from_euler(0, 0, msg.theta), 16 rospy.Time.now(), 17 turtlename, 18 "world") 19 20 if __name__ == '__main__': 21 rospy.init_node('turtle_tf_broadcaster') 22 turtlename = rospy.get_param('~turtle') 23 rospy.Subscriber('/%s/pose' % turtlename, 24 turtlesim.msg.Pose, 25 handle_turtle_pose, 26 turtlename) 27 rospy.spin()
创建tf监听器
1 #!/usr/bin/env python 2 # -*- coding: utf-8 -*- 3 # 该例程将请求/show_person服务,服务数据类型learning_service::Person 4 5 import roslib 6 roslib.load_manifest('learning_tf') 7 import rospy 8 import math 9 import tf 10 import geometry_msgs.msg 11 import turtlesim.srv 12 13 if __name__ == '__main__': 14 rospy.init_node('turtle_tf_listener') 15 16 listener = tf.TransformListener() 17 18 rospy.wait_for_service('spawn') 19 spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn) 20 spawner(4, 2, 0, 'turtle2') 21 22 turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.ms g.Twist,queue_size=1) 23 24 rate = rospy.Rate(10.0) 25 while not rospy.is_shutdown(): 26 try: 27 (trans,rot) = listener.lookupTransform('/turtle2', '/t urtle1', rospy.Time(0)) 28 except (tf.LookupException, tf.ConnectivityException, tf.E xtrapolationException): 29 continue 30 31 angular = 4 * math.atan2(trans[1], trans[0]) 32 linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2) 33 cmd = geometry_msgs.msg.Twist() 34 cmd.linear.x = linear 35 cmd.angular.z = angular 36 turtle_vel.publish(cmd) 37 38 rate.sleep() 39 40