您正在使用IE低版浏览器，为了您的雷锋网账号安全和更好的产品体验，强烈建议使用更快更安全的浏览器
发私信给新智造
导语：雷锋网新智造按：本文来源于知乎，作者卓求，雷锋网新智造已获得授权，也欢迎大家关注作者的知乎账号，将不定期更新ROS, Robotics,Python, Matl
同步到新浪微博
当月热门文章
为了您的账户安全，请
您的邮箱还未验证,完成可获20积分哟！
您的账号已经绑定，现在您可以以方便用邮箱登录
请填写申请人资料ROS Learning-015 learning_tf（编程） 编写一个监听器程序 (Python版) - 博客频道 - CSDN.NET
胖爷，4年后见！ --- 日
分类：ros-tfLinuxUbuntuPythonros
ROS Indigo learning_tf-02 编写一个 监听器 程序 (Python版)
我使用的虚拟机软件：VMware Workstation 11
使用的Ubuntu系统：Ubuntu 14.04.4 LTS
ROS 版本：ROS Indigo
1. 这一节要做的事情：
我们已经编写了一个广播员的程序，下面我们编写一个监听器程序来监听广播员发布的小海龟的坐标系信息：
监听器程序：利用广播员程序广播的坐标信息，在同一个窗口中，有2只小海龟，我们控制 小海龟1 ，让 小海龟2 自动跟随 小海龟1 运行。
2. 程序如何实现：
这个监听器，监听广播员发出的广播（turtle1 和 turtle2 之间坐标系的变化信息），然在同个运算，把结果发布给 turtle2 订阅的 turtle2/cmd_vel 话题上，从而控制 turtle2 运动。
废话少说，开始编程。
2.1. 将路径切换到 learning_tf 软件包，在 nodes 文件夹里创建一个 turtle_tf_listener.py 文件：
$ roscd learning_tf
nodes/turtle_tf_listener.py
2.2. 将下面的代码添加进去：
import roslib
roslib.load_manifest('learning_tf')
import rospy
import math
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)
while not rospy.is_shutdown():
(trans,rot) = listener.lookupTransform('/turtle2', '/turtle1',rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
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()
2.3. 讲解代码：
2.4. 最后一步，给这个 turtle_tf_listener.py 文件加上可执行权限：
$ chmod +x turtle_tf_listener.py
3. 运行程序
3.1. 再运行程序之前， 需要编写一个 start_demo2.launch 启动脚本文件：
$ roscd learning_tf/launch
$ gedit start_demo2.launch
pkg="turtlesim" type="turtlesim_node" name="sim"/&
pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/&
name="turtle1_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" &
name="turtle" type="string" value="turtle1" /&
name="turtle2_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" &
name="turtle" type="string" value="turtle2" /&
pkg="learning_tf" type="turtle_tf_listener.py" name="listener"/&
3.2. 讲解启动脚本程序：
3.3. 运行启动脚本文件：
$ roslaunch learning_tf start_demo2.launch
当你在当前终端中操控键盘方向键时，turtle2（踪色的）跟随 turtle1（白色的）：
下面这幅图就是 world 固定坐标图：
我们使用下面的命令，来打印 turtle1 和 turtle2 的姿态信息：(分别开2个终端)
$ rosrun tf tf_echo /world /turtle1
$ rosrun tf tf_echo /world /turtle2
上节讲过：
tf_echo 关键字 ： 打印出 源坐标系 和 目标坐标系 之间的特定转换信息。
[源坐标系(父类)] [目标坐标系(子类)]
那么到这里，广播 tf 信息的程序 和 监听 tf 变化的监听器程序 我们都学完了。
你有没有发现，turtle2 跟随 turtle1 ，turtle2 最终会移动到与 turtle1 重叠。看到这样的运行现象，我想你肯定不舒服，想个办法来解决这个不算是问题的问题。
下一节，我教你如何将一个额外的坐标系添加到 TF树 中，从而解决这个问题。（这非常类似于创建 TF 广播员程序）
排名：第185名
更多精彩内容，请浏览网站：
、OpenCV、OpenNI、Numpy、Python、、PCL、ARM、Linux、嵌入式、机器学习、智能科学与技术、自动化、Qt、Arduino、、C/C++
运用Python成为顶级黑客
（5）（68）（8）（1）（7）（0）（6）（23）（1）（9）（42）（10）（41）（2）（3）（1）（3）（3）（1）（1）（45）（10）（22）（8）（47）（6）（56）（11）（20）（3）（2）（3）（4）（8）（34）（4）（4）（4）（2）（1）（3）（3）（5）（6）（1）（6）（3）（2）（8）（10）（38）（4）（4）（1）（9）（4）（5）（12）（1）（3）（1）（2）（6）（9）（1）（1）（2）（1）（1）（2）（6）（14）（1）（1）推荐这篇日记的豆列
······ROS里面的几个python写的工具 - 博客频道 - CSDN.NET
贾金金金昭的blog
我是一个准半职业足球运动员
分类：python
1，读取编码器 （check angular）
import rospy
from geometry_msgs.msg import Twist, Quaternion
from nav_msgs.msg import Odometry
from math import radians, copysign
import PyKDL
from math import pi
def quat_to_angle(quat):
rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
return rot.GetRPY()[2]
def normalize_angle(angle):
res = angle
while res & pi:
res -= 2.0 * pi
while res & -pi:
res += 2.0 * pi
return res
class CalibrateAngular():
def __init__(self):
rospy.init_node('calibrate_angular', anonymous=False)
rospy.on_shutdown(self.shutdown)
self.rate = rospy.get_param('~rate', 20)
r = rospy.Rate(self.rate)
self.test_angle = radians(rospy.get_param('~test_angle', 360.0))
self.speed = rospy.get_param('~speed', 0.5)
self.tolerance = radians(rospy.get_param('tolerance', 1))
self.odom_angular_scale_correction = rospy.get_param('~odom_angular_scale_correction', 1.0)
self.start_test = rospy.get_param('~start_test', True)
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
self.base_frame = rospy.get_param('~base_frame', '/base_footprint')
self.odom_frame = rospy.get_param('~odom_frame', '/odom')
self.tf_listener = tf.TransformListener()
rospy.sleep(2)
self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0))
rospy.loginfo("Bring up rqt_reconfigure to control the test.")
reverse = 1
while not rospy.is_shutdown():
if self.start_test:
self.odom_angle = self.get_odom_angle()
last_angle = self.odom_angle
turn_angle = 0
self.test_angle *= reverse
error = self.test_angle - turn_angle
reverse = -reverse
while abs(error) & self.tolerance and self.start_test:
if rospy.is_shutdown():
move_cmd = Twist()
move_cmd.angular.z = copysign(self.speed, error)
self.cmd_vel.publish(move_cmd)
self.odom_angle = self.get_odom_angle()
delta_angle = self.odom_angular_scale_correction * normalize_angle(self.odom_angle - last_angle)
turn_angle += delta_angle
error = self.test_angle - turn_angle
last_angle = self.odom_angle
self.cmd_vel.publish(Twist())
self.start_test = False
params = {'start_test': False}
rospy.sleep(0.5)
self.cmd_vel.publish(Twist())
def get_odom_angle(self):
(trans, rot)
= self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return quat_to_angle(Quaternion(*rot))
def shutdown(self):
rospy.loginfo("Stopping the robot...")
self.cmd_vel.publish(Twist())
rospy.sleep(1)
if __name__ == '__main__':
CalibrateAngular()
rospy.loginfo("Calibration terminated.")
2，检查校正走一米
import rospy
from geometry_msgs.msg import Twist, Point
from math import copysign, sqrt, pow
class CalibrateLinear():
def __init__(self):
rospy.init_node('calibrate_linear', anonymous=False)
rospy.on_shutdown(self.shutdown)
self.rate = rospy.get_param('~rate', 20)
r = rospy.Rate(self.rate)
self.test_distance = rospy.get_param('~test_distance', 1.0)
self.speed = rospy.get_param('~speed', 0.15)
self.tolerance = rospy.get_param('~tolerance', 0.01)
self.odom_linear_scale_correction = rospy.get_param('~odom_linear_scale_correction', 1.0)
self.start_test = rospy.get_param('~start_test', True)
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
self.base_frame = rospy.get_param('~base_frame', '/base_footprint')
self.odom_frame = rospy.get_param('~odom_frame', '/odom')
self.tf_listener = tf.TransformListener()
rospy.sleep(2)
self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0))
rospy.loginfo("Bring up rqt_reconfigure to control the test.")
self.position = Point()
self.position = self.get_position()
x_start = self.position.x
y_start = self.position.y
move_cmd = Twist()
while not rospy.is_shutdown():
move_cmd = Twist()
if self.start_test:
self.position = self.get_position()
distance = sqrt(pow((self.position.x - x_start), 2) +
pow((self.position.y - y_start), 2))
distance *= self.odom_linear_scale_correction
distance - self.test_distance
if not self.start_test or abs(error) &
self.tolerance:
self.start_test = False
params = {'start_test': False}
rospy.loginfo(params)
move_cmd.linear.x = copysign(self.speed, -1 * error)
self.position = self.get_position()
x_start = self.position.x
y_start = self.position.y
self.cmd_vel.publish(move_cmd)
self.cmd_vel.publish(Twist())
def get_position(self):
(trans, rot)
= self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return Point(*trans)
def shutdown(self):
rospy.loginfo("Stopping the robot...")
self.cmd_vel.publish(Twist())
rospy.sleep(1)
if __name__ == '__main__':
CalibrateLinear()
rospy.spin()
rospy.loginfo("Calibration terminated.")
3，键盘控制
import roslib.load_manifest('teleop_twist_keyboard')
import rospy
from geometry_msgs.msg import Twist
import sys, select, termios, tty
Reading from the keyboard
and Publishing to Twist!
---------------------------
Moving around:
For Holonomic mode (strafing), hold down the shift key:
---------------------------
t : up (+z)
b : down (-z)
anything else : stop
q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
CTRL-C to quit
moveBindings = {
'i':(1,0,0,0),
'o':(1,0,0,-1),
'j':(0,0,0,1),
'l':(0,0,0,-1),
'u':(1,0,0,1),
',':(-1,0,0,0),
'.':(-1,0,0,1),
'm':(-1,0,0,-1),
'O':(1,-1,0,0),
'I':(1,0,0,0),
'J':(0,1,0,0),
'L':(0,-1,0,0),
'U':(1,1,0,0),
'&':(-1,0,0,0),
'&':(-1,-1,0,0),
'M':(-1,1,0,0),
't':(0,0,1,0),
'b':(0,0,-1,0),
speedBindings={
'q':(1.1,1.1),
'z':(.9,.9),
'w':(1.1,1),
'x':(.9,1),
'e':(1,1.1),
'c':(1,.9),
def getKey():
tty.setraw(sys.stdin.fileno())
select.select([sys.stdin], [], [], 0)
key = sys.stdin.read(1)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
return key
speed = 0.30
turn = 0.6
def vels(speed,turn):
return "currently:\tspeed %s\tturn %s " % (speed,turn)
if __name__=="__main__":
settings = termios.tcgetattr(sys.stdin)
pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1)
rospy.init_node('teleop_twist_keyboard')
status = 0
print vels(speed,turn)
key = getKey()
if key in moveBindings.keys():
x = moveBindings[key][0]
y = moveBindings[key][1]
z = moveBindings[key][2]
th = moveBindings[key][3]
elif key in speedBindings.keys():
speed = speed * speedBindings[key][0]
turn = turn * speedBindings[key][1]
print vels(speed,turn)
if (status == 14):
status = (status + 1) % 15
if (key == '\x03'):
twist = Twist()
twist.linear.x = x* twist.linear.y = y* twist.linear.z = z*
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*turn
pub.publish(twist)
twist = Twist()
twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
pub.publish(twist)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
排名：千里之外
（32）（1）（5）（3）