ros之tf坐标系广播与监听的编程实现

Posted 2021-03-13 tanshengjiang

创建功能包－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