ROS 入门基础（十）监听坐标系

现在自己写一个监听的试试。

新的功能，那就新的package啊。

catkin_create_pkg learning_tf roscpp rospy tf turtlesim

**仔细看看啊，比平时多了一个tf

开始写Python了。

turtle_tf_broadcast.py

#!/usr/bin/env python  
import roslib
roslib.load_manifest('learning_tf')
import rospy

import tf
import turtlesim.msg

def handle_turtle_pose(msg, turtlename):
    br = tf.TransformBroadcaster()
    br.sendTransform((msg.x, msg.y, 0),
                     tf.transformations.quaternion_from_euler(0, 0, msg.theta),
                     rospy.Time.now(),
                     turtlename,
                     "world")

if __name__ == '__main__':
    rospy.init_node('turtle_tf_broadcaster')
    turtlename = rospy.get_param('~turtle')
    rospy.Subscriber('/%s/pose' % turtlename,
                     turtlesim.msg.Pose,
                     handle_turtle_pose,
                     turtlename)
    rospy.spin()

turtle_tf_listener.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('turtle_tf_listener')
	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():
		try:
			(trans,rot) = listener.lookupTransform('/turtle2', 'turtle1', rospy.Time(0))
		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()

。。。。。。

接着就悲剧了。

弄了一天，卡死在运行上了。

按照古月的21天教程，有一句

rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1

一直过不去。

之前虽然也是磕磕碰碰的过来了，不过这个坑有点深。找各种范例，解释说明。都过不去。

网上也有同样问题的，大神解救的方法是代码出错了。可是我都复制黏贴还是过不去。

我放弃了。。。

换一个教程吧。

感觉新教程也按照古月来的，只是加强版。

不过他现在整合了launch。先在src里面新建文件夹launch。再来start_demo.launch。

<launch>
	<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.py" name="listener" />
</launch>

roslaunch learning_tf start_demo.launch

 好了，自己写的两个小乌龟尾行，用launch 启动。真香。