創建功能包－learning_tf
$ cd ~/catkin_ws/src
$ catkin_create_pkg learning_tf roscpp rospy tf turtlesim
如何創建一個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數據時需要設置一個時間戳(一般為１０秒)。
創建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之間的坐標系關系，根據turtle１與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