![[ROS] 小乌龟跟随--学习笔记](http://www.mshxw.com/aiimages/31/1012410.png "[ROS] 小乌龟跟随--学习笔记")
首先是使用到的依赖项：rospy、tf，可以手动在package.xml里添加，如果使用vscode创建功能包，则在填写依赖项时会帮我们补充完整
接下来是创建第一只小乌龟的并广播坐标系的py文件代码：
命名为demo01_turtle1
#! /usr/bin/env python
# coding: utf-8
import rospy
from turtlesim.msg import Pose
from tf.broadcaster import TransformBroadcaster
from tf.transformations import quaternion_from_euler
def pose_callback(msg):
if not isinstance(msg, Pose):
return
x = msg.x
y = msg.y
theta = msg.theta
translation = (x, y, 0)
rotation = quaternion_from_euler(0, 0, theta)
broadcaster.sendTransform(translation, rotation, rospy.Time().now(), "turtle1", "world")
if __name__ == '__main__':
node_name = "demo01_turtle1"
rospy.init_node(node_name)
broadcaster = TransformBroadcaster()
pose_topic_name = "/turtle1/pose"
rospy.Subscriber(pose_topic_name, Pose, pose_callback)
rospy.spin()
下面详细地解释每一行代码的作用：
import rospy # 导入ROS库 from turtlesim.msg import Pose # 从msg文件里导入乌龟Pose消息 from tf.broadcaster import TransformBroadcaster # 通过 TransformBroadcaster 类来发布变换关系的接口 from tf.transformations import quaternion_from_euler # 导入欧拉角转化成四元数的函数
def pose_callback(msg):
if not isinstance(msg, Pose): # 判断数据类型，数据不符合就退出函数
return
x = msg.x # 赋值，用于计算四元数和广播坐标系
y = msg.y
theta = msg.theta
translation = (x, y, 0)
rotation = quaternion_from_euler(0, 0, theta)
#发布tf转换
broadcaster.sendTransform(translation, rotation, rospy.Time().now(), "turtle1","world")
broadcaster.sendTransform：第一、二个参数传入transform本身; 第三个参数是时间戳ros::Time::now()（给将要发布的transform一个时间戳，将其与现在的时间绑定在一起）； 第四个参数是父节点坐标系的名字； 第五个参数传入子节点坐标系的名字
if __name__ == '__main__':
node_name = "demo01_turtle1" #初始化、命名节点
rospy.init_node(node_name)
broadcaster = TransformBroadcaster() #发布变换关系的接口
pose_topic_name = "/turtle1/pose"
rospy.Subscriber(pose_topic_name, Pose, pose_callback) # 从/turtle1/pose订阅小乌龟1位置信息
rospy.spin()
然后是构建第二个小乌龟并接收广播的py文件：
命名为deme01_turtle2
#! /usr/bin/env python
# coding: utf-8
import rospy
from turtlesim.msg import Pose
from tf.broadcaster import TransformBroadcaster
from tf.transformations import quaternion_from_euler
from turtlesim.srv import Spawn, SpawnRequest, SpawnResponse
from geometry_msgs.msg import Twist
from tf.listener import TransformListener
from math import pow, sqrt, atan2
def pose_callback(msg):
if not isinstance(msg, Pose):
return
x = msg.x
y = msg.y
theta = msg.theta
translation = (x, y, 0)
rotation = quaternion_from_euler(0, 0, theta)
broadcaster.sendTransform(translation, rotation, rospy.Time().now(), "turtle2", "world")
if __name__ == '__main__':
node_name = "demo01_turtle2"
rospy.init_node(node_name)
service_name = "/spawn"
client = rospy.ServiceProxy(service_name, Spawn)
client.wait_for_service()
request = SpawnRequest()
request.x = 1.0
request.y = 1.0
request.theta = 0
request.name = "turtle2"
client.call(request)
broadcaster = TransformBroadcaster()
pose_topic_name = "/turtle2/pose"
rospy.Subscriber(pose_topic_name, Pose, pose_callback)
vel_topic_name = "/turtle2/cmd_vel"
publisher = rospy.Publisher(vel_topic_name, Twist, queue_size=1000)
listener = TransformListener()
rate = rospy.Rate(10)
while not rospy.is_shutdown():
try:
transform = listener.lookupTransform("turtle2", "turtle1", rospy.Time())
except:
rate.sleep()
continue
x, y, z = transform[0]
distance = sqrt(pow(x, 2) + pow(y, 2))
angular = atan2(y, x)
twist = Twist()
twist.linear.x = 0.5 * distance
twist.angular.z = 3 * angular
publisher.publish(twist)
rate.sleep()
rospy.spin()
下面详细地解释每一行代码的作用：
#! /usr/bin/env python # coding: utf-8 import rospy from turtlesim.msg import Pose from tf.broadcaster import TransformBroadcaster from tf.transformations import quaternion_from_euler from turtlesim.srv import Spawn, SpawnRequest, SpawnResponse # 导入孵化第二只乌龟的函数 from geometry_msgs.msg import Twist # 获取角速度、线速度数据类型 from tf.listener import TransformListener # 用于接收广播 from math import pow, sqrt, atan2
def pose_callback(msg):
if not isinstance(msg, Pose):
return
x = msg.x
y = msg.y
theta = msg.theta
translation = (x, y, 0)
rotation = quaternion_from_euler(0, 0, theta)
broadcaster.sendTransform(translation, rotation, rospy.Time().now(), "turtle2", "world")
基本与demo01_turtle1相同，只有父节点坐标系选用发生了变化
if __name__ == '__main__':
node_name = "demo01_turtle2"
rospy.init_node(node_name)
service_name = "/spawn"
client = rospy.ServiceProxy(service_name, Spawn) # 向孵化小乌龟的话题发布消息
client.wait_for_service()
request = SpawnRequest() # 新生成的小乌龟的初始信息
request.x = 1.0
request.y = 1.0
request.theta = 0
request.name = "turtle2"
client.call(request)
broadcaster = TransformBroadcaster()
pose_topic_name = "/turtle2/pose"
rospy.Subscriber(pose_topic_name, Pose, pose_callback) # 订阅小乌龟的位置信息
vel_topic_name = "/turtle2/cmd_vel"
publisher = rospy.Publisher(vel_topic_name, Twist, queue_size=1000) # 新发布一个小乌龟2的速度信息
listener = TransformListener() # 接收小乌龟1广播的信息
rate = rospy.Rate(10)
while not rospy.is_shutdown():
try:
transform = listener.lookupTransform("turtle2", "turtle1", rospy.Time()) # 返回坐标系的变换：基于turtle2得到新的turtle1坐标
except:
rate.sleep()
continue
x, y, z = transform[0]
distance = sqrt(pow(x, 2) + pow(y, 2)) #计算两个小乌龟的距离差
angular = atan2(y, x) #计算两个小乌龟的弧度差
twist = Twist()
twist.linear.x = 0.5 * distance #可以看成小乌龟2跟随小乌龟1的线速度、角速度系数
twist.angular.z = 3 * angular
publisher.publish(twist)
rate.sleep()
rospy.spin()
除此之外，还需要额外配置CMakelists.txt文件：
在catkin_install_python{PROGRAMS...DESTINATION}中添加py文件
想快捷启动的话，就新建一个launch文件：
launch>
其中，在编写与新建的py文件相关的条目时，要更改package名，此处我创建的pkg为“gensui”
source后进行编译，运行话题如下：
虽然大致了解了总体运行的框架，但里面tf相关的使用还是优点弄不清，还需要继续阅读和寻找实例
