ROS Learning-022 learning_tf-06(编程) 现在与过去中穿梭 (Python版) --- waitForTransformFull() 函数
ROS Indigo learning_tf-06 现在与过去中穿梭 (Python版) — waitForTransformFull()
函数
我使用的虚拟机软件:VMware Workstation 11
使用的Ubuntu系统:Ubuntu 14.04.4 LTS
ROS 版本:ROS Indigo
一 . 前言
这一节要做的事情:使用 tf 的一个强大功能:可以再现在与过去中穿梭。(就是:如何使用 waitForTransformFull()
函数。)
我们不让 turtle2 跟随当前的 turtle1 的坐标, 而是让 turtle2 去跟随 过去的 turtle1 的坐标。
一样,我们只需要改变 监听器 程序 ( turtle_tf_listener.py
)中的一点点代码就可以实现。我们就让 turtle2 去跟随5秒钟前的 turtle1 的坐标吧。
跟随过去 的 程序编写
1 . 重点讲解
我们将之前编写的 监听器 程序 ( turtle_tf_listener.py
)里面的这一段代码:
try:
(trans, rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
修改为:
try:
now = rospy.Time.now()
past = now - rospy.Duration(5.0)
listener.waitForTransformFull('/turtle2', now, '/turtle1', now, '/world', rospy.Duration(4.0))
(trans,rot) = listener.lookupTransform('/turtle2', now, '/turtle1', past, '/world')
我们来具体了解一下上面代码中出现的:lookupTransformFull()
函数,这个函数也是坐标系信息转换的功能。这个函数有6个形参: ( 其实它的功能和lookupTransform()
函数的功能一样,只不过是个升级版函数 ) :
1 . 父类坐标系节点名
2 . 指定时间
3 . 子类坐标系节点名
4 . 指定时间
5 . 指定不随时间改变的坐标系, 在这里是 “/world
” 坐标系.
通过 形参1 和 形参2 就可以得到一个 姿态的信息(线速度和角速度),通过 形参3 和 形参4 就可以得到另一个姿态的信息。
图示 lookupTransformFull()
函数的作用:
解说一下这幅图的意思: lookupTransformFull()
函数 做了下面三步的事情。
第1步 : 将 形参1 在指定的时间 形参2 时的姿态信息转换到 形参5 坐标系上;
第2步 : 接着,将 形参3 在指定的时间 形参4 时的姿态信息转换到 形参5 坐标系上;
第3步 : 最后,将 第1步 得到的姿态信息做为坐标原点(即:以 形参1 的位置做为参考坐标系),求出 第二步 得到的姿态信息 在 这个参考坐标中的姿态信息,将其做为 lookupTransformFull()
函数返回值输出。
2 . 编写代码
在learning_tf
程序包中的 nodes
路径下,新建一个文件:turtle_tf_listener_pastNow.py
:
roscd learning_tf/node/
gedit turtle_tf_listener_pastNow.py
下面是完整的程序。将这段程序复制到 turtle_tf_listener_pastNow.py
文件里面:
#!/usr/bin/env python
import roslib
roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv
if __name__ == '__main__':
rospy.init_node('tf_turtle')
listener = tf.TransformListener()
rospy.wait_for_service('spawn')
spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
spawner(4, 2, 0, 'turtle2')
turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist, queue_size=1)
rate = rospy.Rate(10.0)
listener.waitForTransform("/turtle2", "/turtle1", rospy.Time(), rospy.Duration(4.0))
while not rospy.is_shutdown():
try:
now = rospy.Time.now()
past = now - rospy.Duration(5.0)
listener.waitForTransformFull('/turtle2', now, '/turtle1', now, '/world', rospy.Duration(4.0))
(trans,rot) = listener.lookupTransform('/turtle2', now, '/turtle1', past, '/world')
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
angular = 4 * math.atan2(trans[1], trans[0])
linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
cmd = geometry_msgs.msg.Twist()
cmd.linear.x = linear
cmd.angular.z = angular
turtle_vel.publish(cmd)
rate.sleep()
给这个文件添加可执行权限:
chmod 777 turtle_tf_listener_pastNow.py
3 . 编写启动文件
在learning_tf
程序包中的 launch
路径下,新建一个文件:start_demo6.launch
:
roscd learning_tf/launch/
gedit start_demo6.launch
将下面的代码拷贝进去。(下面这段代码就是通过 start_demo2.launch
文件改写的,基本和它一模一样)
<launch>
<!-- Turtlesim Node -->
<node pkg="turtlesim" type="turtlesim_node" name="sim" />
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen" />
<node name="turtle1_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
<param name="turtle" type="string" value="turtle1" />
</node>
<node name="turtle2_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
<param name="turtle" type="string" value="turtle2" />
</node>
<node pkg="learning_tf" type="turtle_tf_listener_pastNow.py" name="listener" />
</launch>
4 . 运行
运行 start_demo6.launch
这个启动脚本文件:
$ roslaunch learning_tf start_demo6.launch
这次的运行效果,很不错,当前时间的 turtle2
小海龟 就在跟随这个 过去5秒钟之前的 turtle1
小海龟做运动。
搞定
5 . 扩展: ROS 官方上的错误
下面这段是 ROS 官网中的程序:(这个网站)
try:
now = rospy.Time.now()
past = now - rospy.Duration(5.0)
listener.waitForTransformFull('/turtle2', now, '/turtle1', past, '/world', rospy.Duration(4.0))
(trans,rot) = listener.lookupTransform('/turtle2', now, '/turtle1', past, '/world')
这段代码是错误的。如果运行的话,会输出下面的错误信息:
[listener-6] process has died [pid 8919, exit code 1, cmd /home/aobosir/catkin_ws/src/learning_tf/nodes/turtle_tf_listener_pastNow.py __name:=listener __log:=/home/aobosir/.ros/log/71c1eedc-75ad-11e6-8577-000c29fd0c33/listener-6.log].
log file: /home/aobosir/.ros/log/71c1eedc-75ad-11e6-8577-000c29fd0c33/listener-6*.log
正确的代码就是:(这样就不会出现问题)
try:
now = rospy.Time.now()
past = now - rospy.Duration(5.0)
listener.waitForTransformFull('/turtle2', now, '/turtle1', now, '/world', rospy.Duration(4.0))
(trans,rot) = listener.lookupTransform('/turtle2', now, '/turtle1', past, '/world')
总结:
现在,我们学会了如果在ROS 中进行不同时间段之间的 tf 转换。
接下来,下一节,介绍:如果调试使用 tf 转换的程序(C++ 版)。
ROS Learning-022 learning_tf-06(编程) 现在与过去中穿梭 (Python版) --- waitForTransformFull() 函数的更多相关文章
- ROS Learning-015 learning_tf(编程) 编写一个监听器程序 (Python版)
ROS Indigo learning_tf-02 编写一个 监听器 程序 (Python版) 我使用的虚拟机软件:VMware Workstation 11 使用的Ubuntu系统:Ubuntu 1 ...
- ROS Learning-014 learning_tf(编程) 坐标系变换(tf)广播员 (Python版)
ROS Indigo learning_tf-01 坐标系变换(tf)广播员 (Python版) 我使用的虚拟机软件:VMware Workstation 11 使用的Ubuntu系统:Ubuntu ...
- ROS Learning-019 learning_tf-03(编程) 添加额外的坐标系 (Python版)
ROS Indigo learning_tf-03 添加额外的坐标系 (Python版) 我使用的虚拟机软件:VMware Workstation 11 使用的Ubuntu系统:Ubuntu 14.0 ...
- ROS Learning-021 learning_tf-05(编程) now() 和 Time(0) 的区别 (Python版)
ROS Indigo learning_tf-05 now() 和 Time(0)的区别 (Python版) - waitForTransform() 函数 我使用的虚拟机软件:VMware Work ...
- 【C语言编程入门笔记】C语言果然博大精深!函数还分内部和外部?
۞ 外部函数与内部函数 前面我们讲解了关于函数的调用都是针对同一个源文件中其他函数进行调用的,而在有些情况下,函数也可以对另外一个源文件中的函数进行调用.当一个程序由多个源文件组成时,根据函数是否能被 ...
- ROS Learning-020 learning_tf-04(编程)让turtle2 海龟跟随turtle1海龟,并绕着 turtle1海龟转圈 (Python版)
ROS Indigo learning_tf-04 (编程)让 turtle2 海龟跟随 turtle1 海龟,并绕着 turtle1 海龟转圈 (Python版) 我使用的虚拟机软件:VMware ...
- ROS Learning-011 beginner_Tutorials (编程) 编写 ROS 话题版的 Hello World 程序(Python版)
ROS Indigo beginner_Tutorials-10 编写 ROS 话题版的 Hello World 程序(Python版) 我使用的虚拟机软件:VMware Workstation 11 ...
- Python并发编程06 /阻塞、异步调用/同步调用、异步回调函数、线程queue、事件event、协程
Python并发编程06 /阻塞.异步调用/同步调用.异步回调函数.线程queue.事件event.协程 目录 Python并发编程06 /阻塞.异步调用/同步调用.异步回调函数.线程queue.事件 ...
- ROS Learning-013 beginner_Tutorials (编程) 编写ROS服务版的Hello World程序(Python版)
ROS Indigo beginner_Tutorials-12 编写ROS服务版的Hello World程序(Python版) 我使用的虚拟机软件:VMware Workstation 11 使用的 ...
随机推荐
- Handsontable-一款仿 Excel效果的表格插件使用总结 96
最近在做一个关于报表管理的项目,发现了一款很好用的jQuery插件-Handsontable.它真的特别给力,在 Excel 中可进行的操作,你几乎都可以在网页中做到,如拖动复制.Ctrl+C .Ct ...
- 2017.11.27 stm8 low power-consumption debugging
1 STM8L+LCD The STM8L-DISCOVERY helps you to discover the STM8L ultralow power features and todevelo ...
- mysql1130远程连接没有权限的解决方法
网上查了半天,终于解决 远程连接没有权限的原因有两种,一个是因为mysql的限制,一个是防火墙的限制. ,解决防火墙限制: 在mysql服务主机上将防火墙关闭或者在防火墙高级设置里面加入出入站规则,加 ...
- C8051F340 USB Fn hacking
/************************************************************************************ * C8051F340 US ...
- B+树和LSM存储引擎代表树和B-树
B+树和LSM比较 https://blog.csdn.net/u013928917/article/details/75912045 在关系型数据库mysql中普遍使用B+树作为索引,在实际中 ...
- docker容器与宿主机之间内容拷贝
来自:http://blog.csdn.net/yangzhenping/article/details/43667785 常用的方式有3种: 从容器内拷贝文件到主机上 docker cp <c ...
- 《Javascript高级程序设计》阅读记录(六):第六章 下
这个系列以往文字地址: <Javascript高级程序设计>阅读记录(一):第二.三章 <Javascript高级程序设计>阅读记录(二):第四章 <Javascript ...
- lower_bound()函数与quicksort()函数的简单掌握
lower_bound 这个序列中可能会有很多重复的元素,也可能所有的元素都相同,为了充分考虑这种边界条件,STL中的lower_bound算法总体上是才用了二分查找的方法,但是由于是查找序列中的第一 ...
- hl7 v2.X 版本中RSP_K23消息的构造
RSP_K23消息有MSH, MSA, ERR, QAK, QPD, PID几个segment,其中ERR,PID为可选. 1. 当MSA有err时,ERR段填充出错的详细信息. 2. 当MSA为AA ...
- git revert reset
git revert是用一次新的commit来回滚之前的commit,git reset是直接删除指定的commit. git reset 是把HEAD向后移动了一下,而git revert是HEAD ...