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

ros之tf坐标系广播与监听的编程实现的更多相关文章

  1. 26 Flutter仿京东商城项目 购物车之 event_bus事件广播 事件监听

    event_bus 介绍 在前面的课程我们给大家讲过状态管理 Provider 的使用. 通俗的讲状态管理就是:当我们想在多个页面(组件/Widget)之间共享状态(数据),或 者一个页面(组件/Wi ...

  2. spring4.1.8扩展实战之三:广播与监听

    提到广播与监听,我们常常会想到RabbitMQ.Kafka等消息中间件,这些常用于分布式系统中多个应用之间,有时候应用自身内部也有广播和监听的需求(例如某个核心数据发生变化后,有些业务模块希望立即被感 ...

  3. Android初级教程使用服务注册广播接收者监听手机解锁屏变化

    之前第七章广播与服务理论篇写到: 特殊的广播接收者(一般发广播次数频率很高) 安卓中有一些广播接收者,必须使用代码注册,清单文件注册是无效的 屏幕锁屏和解锁 电量改变 今天在这里就回顾一下,且用代码方 ...

  4. 为什么监听不到开机广播action.BOOT_COMPLETED

    为什么监听不到开机广播action.BOOT_COMPLETED 1. 说明 Android手机开机后,会发送android.intent.action.BOOT_COMPLETED广播,监听这个广播 ...

  5. Android手机使用广播监听手机收到的短信

    我们使用的Android手机在收到短信的时候会发出一条系统广播.该条广播中存放着接收到的短信的详细信息.本文将详细介绍如何通过动态注册广播来监听短信. 注册广播有两种方式,一种是动态注册,另一种是静态 ...

  6. Android-服务中监听电源键和Home键的广播、在锁屏下仍然工作的方法

    Android-服务中监听电源键和Home键的广播  http://blog.csdn.net/u014657752/article/details/49512485 Android开发之如何监听让服 ...

  7. android动态注册监听网络变化异常

    在使用广播接收器监听网络变化的时候,在AndroidManifest.xml中加入<user-permission android:name="android.permission.A ...

  8. android平板Home键的监听

    关于android4.2平板Home键的监听 1.android4.0以上的平板Home键是不能屏蔽的,至少在应用层是这样.但是可以监听Home键的触发,主要是通过广播的监听来接受Hone键的触发广播 ...

  9. Android 动态监听网络 断网重连

    需求: 网络连接断开 弹出popupwindow 当前网络连接断开 网络恢复时popupwindow 消失重新请求网络. 需求描述完毕 上一张帅图 思路:广播 发送及时消息 断网flag  popup ...

随机推荐

  1. ThreadLocal源码阅读

    package java.lang; import java.lang.ref.WeakReference; import java.util.Objects; import java.util.co ...

  2. IteratorPattern(迭代器模式)-----Java/.Net

    迭代器模式(Iterator Pattern)是 Java 和 .Net 编程环境中非常常用的设计模式.这种模式用于顺序访问集合对象的元素,不需要知道集合对象的底层表示

  3. Windows To Go 企业版2019 LTSC 开发环境部署

    Windows To Go 是一项非常实用的功能,与传统方式安装Windows 10相比更具有灵活性,会根据每次接入的硬件型号保留不同版本驱动. 由于博主是一名全栈程序员(截至发稿处于菜鸟级别),对灵 ...

  4. JVM性能优化系列-(1) Java内存区域

    1. Java内存区域 1.1 运行时数据区 Java虚拟机在执行Java程序的过程中会把它所管理的内存划分为若干个不同的数据区域.主要包括:程序计数器.虚拟机栈.本地方法栈.Java堆.方法区(运 ...

  5. HDU3652 B-number 题解 数位DP

    题目链接:http://acm.hdu.edu.cn/showproblem.php?pid=3652 题目大意: 求区间 \([1, n]\) 范围内包含连续的数位"13"并且能 ...

  6. 1092 最好吃的月饼 (20分)C语言

    月饼是久负盛名的中国传统糕点之一,自唐朝以来,已经发展出几百品种. 若想评比出一种"最好吃"的月饼,那势必在吃货界引发一场腥风血雨-- 在这里我们用数字说话,给出全国各地各种月饼的 ...

  7. 为什么TCP建立连接协议是三次握手,而关闭连接却是四次握手呢?

    看到了一道面试题:"为什么TCP建立连接协议是三次握手,而关闭连接却是四次握手呢?为什么不能用两次握手进行连接?",想想最近也到金三银四了,所以就查阅了相关资料,整理出来了这篇文章 ...

  8. EXE和DLL调用关系,DLL制作,钩子

    制作DLL时,在cpp种引入了头文件,但头文件里的全局变量在cpp种却不能用 参考大佬博客https://blog.csdn.net/speargod/article/details/88854344 ...

  9. 【转】python及其工具包安装基本流程

    昨天晚上在家里的旧电脑上安装了<利用python进行数据分析>的部分环境,遇到若干问题,在此予以记录. 部分细节转:http://blog.csdn.net/huanbia/article ...

  10. hexo搭建博客系列(三)美化主题

    文章目录 其他搭建 1. 添加博客图标 2. 鼠标点击特效(二选一) 2.1 红心特效 2.2 爆炸烟花 3. 设置头像 4. 侧边栏社交小图标设置 5. 文章末尾的标签图标修改 6. 访问量统计 7 ...