需求描述:
程序启动之初: 产生两只乌龟,中间的乌龟(A) 和 左下乌龟(B), B 会自动运行至A的位置,并且键盘控制时,只是控制 A 的运动,但是 B 可以跟随 A 运行。
实现分析:
乌龟跟随实现的核心,是乌龟A和B都要发布相对世界坐标系的坐标信息,然后,订阅到该信息需要转换获取A相对于B坐标系的信息,最后,再生成速度信息,并控制B运动。
- 启动乌龟显示节点
- 在乌龟显示窗体中生成一只新的乌龟(需要使用服务)
- 编写两只乌龟发布坐标信息的节点
- 编写订阅节点订阅坐标信息并生成新的相对关系生成速度信息
实现流程: C++ 与 Python 实现流程一致
- 新建功能包,添加依赖
- 编写服务客户端,用于生成一只新的乌龟
- 编写发布方,发布两只乌龟的坐标信息
- 编写订阅方,订阅两只乌龟信息,生成速度信息并发布
- 运行
准备工作:
1.了解如何创建第二只乌龟,且不受键盘控制
创建第二只乌龟需要使用rosservice,话题使用的是 spawn
rosservice call /spawn "x: 1.0
y: 1.0
theta: 1.0
name: 'turtle_flow'"
name: "turtle_flow"
键盘是无法控制第二只乌龟运动的,因为使用的话题: /第二只乌龟名称/cmd_vel,对应的要控制乌龟运动必须发布对应的话题消息
2.了解如何获取两只乌龟的坐标
是通过话题 /乌龟名称/pose 来获取的
x: 1.0 //x坐标
y: 1.0 //y坐标
theta: -1.21437060833 //角度
linear_velocity: 0.0 //线速度
angular_velocity: 1.0 //角速度
1. 创建功能包
创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim
catkin_create_pkg tflearn tf2 tf2_ros tf2_geometry_msgs roscpp rospy std_msgs geometry_msgs turtlesim
cd tflearn
mkdir scripts
touch tf_service.py
touch tf_publisher.py
touch tf_subscriber.py
2. 服务客户端(生成乌龟)
tf_service.py
#! /usr/bin/env python
# coding=utf-8
"""
调用 service 服务在窗体指定位置生成一只乌龟
流程:
1.导包
2.初始化 ros 节点
3.创建服务客户端
4.等待服务启动
5.创建请求数据
6.发送请求并处理响应
"""
#1.导包
import rospy
from turtlesim.srv import Spawn, SpawnRequest, SpawnResponse
if __name__ == "__main__":
# 2.初始化 ros 节点
rospy.init_node("turtle_spawn_p")
# 3.创建服务客户端
client = rospy.ServiceProxy("/spawn",Spawn)
# 4.等待服务启动
client.wait_for_service()
# 5.创建请求数据
req = SpawnRequest()
req.x = 1.0
req.y = 1.0
req.theta = 3.14
req.name = "turtle2"
# 6.发送请求并处理响应
try:
response = client.call(req)
rospy.loginfo("乌龟创建成功,名字是:%s",response.name)
except Exception as e:
rospy.loginfo("服务调用失败....")
3. 发布方(发布两只乌龟的坐标信息)
tf_publisher.py
#! /usr/bin/env python
# coding=utf-8
"""
该文件实现:需要订阅 turtle1 和 turtle2 的 pose,然后广播相对 world 的坐标系信息
注意: 订阅的两只 turtle,除了命名空间(turtle1 和 turtle2)不同外,
其他的话题名称和实现逻辑都是一样的,
所以我们可以将所需的命名空间通过 args 动态传入
实现流程:
1.导包
2.初始化 ros 节点
3.解析传入的命名空间
4.创建订阅对象
5.回调函数处理订阅的 pose 信息
5-1.创建 TF 广播器
5-2.将 pose 信息转换成 TransFormStamped
5-3.发布
6.spin
"""
# 1.导包
import rospy
import sys
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped
import tf2_ros
import tf_conversions
turtle_name = ""
def doPose(pose):
# rospy.loginfo("x = %.2f",pose.x)
#1.创建坐标系广播器
broadcaster = tf2_ros.TransformBroadcaster()
#2.将 pose 信息转换成 TransFormStamped
tfs = TransformStamped()
tfs.header.frame_id = "world"
tfs.header.stamp = rospy.Time.now()
tfs.child_frame_id = turtle_name
tfs.transform.translation.x = pose.x
tfs.transform.translation.y = pose.y
tfs.transform.translation.z = 0.0
qtn = tf_conversions.transformations.quaternion_from_euler(0, 0, pose.theta)
tfs.transform.rotation.x = qtn[0]
tfs.transform.rotation.y = qtn[1]
tfs.transform.rotation.z = qtn[2]
tfs.transform.rotation.w = qtn[3]
#3.广播器发布 tfs
broadcaster.sendTransform(tfs)
if __name__ == "__main__":
# 2.初始化 ros 节点
rospy.init_node("sub_tfs_p")
# 3.解析传入的命名空间
rospy.loginfo("-------------------------------%d",len(sys.argv))
if len(sys.argv) < 2:
rospy.loginfo("请传入参数:乌龟的命名空间")
else:
turtle_name = sys.argv[1]
rospy.loginfo("///////////////////乌龟:%s",turtle_name)
rospy.Subscriber(turtle_name + "/pose",Pose,doPose)
# 4.创建订阅对象
# 5.回调函数处理订阅的 pose 信息
# 5-1.创建 TF 广播器
# 5-2.将 pose 信息转换成 TransFormStamped
# 5-3.发布
# 6.spin
rospy.spin()
4. 订阅方(解析坐标信息并生成速度信息)
tf_subscriber.py
#! /usr/bin/env python
# coding=utf-8
"""
订阅 turtle1 和 turtle2 的 TF 广播信息,查找并转换时间最近的 TF 信息
将 turtle1 转换成相对 turtle2 的坐标,在计算线速度和角速度并发布
实现流程:
1.导包
2.初始化 ros 节点
3.创建 TF 订阅对象
4.处理订阅到的 TF
4-1.查找坐标系的相对关系
4-2.生成速度信息,然后发布
"""
# 1.导包
import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped, Twist
import math
if __name__ == "__main__":
# 2.初始化 ros 节点
rospy.init_node("sub_tfs_p")
# 3.创建 TF 订阅对象
buffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(buffer)
# 4.处理订阅到的 TF
rate = rospy.Rate(10)
# 创建速度发布对象
pub = rospy.Publisher("/turtle2/cmd_vel",Twist,queue_size=1000)
while not rospy.is_shutdown():
rate.sleep()
try:
#def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)):
trans = buffer.lookup_transform("turtle2","turtle1",rospy.Time(0))
# rospy.loginfo("相对坐标:(%.2f,%.2f,%.2f)",
# trans.transform.translation.x,
# trans.transform.translation.y,
# trans.transform.translation.z
# )
# 根据转变后的坐标计算出速度和角速度信息
twist = Twist()
# 间距 = x^2 + y^2 然后开方
twist.linear.x = 0.5 * math.sqrt(math.pow(trans.transform.translation.x,2) + math.pow(trans.transform.translation.y,2))
twist.angular.z = 4 * math.atan2(trans.transform.translation.y, trans.transform.translation.x)
pub.publish(twist)
except Exception as e:
rospy.logwarn("警告:%s",e)
chmod +x tf_service.py
chmod +x tf_publisher.py
chmod +x tf_subscriber.py
编辑tflearn
下的CmakeLists.txt文件
catkin_install_python(PROGRAMS
scripts/tf_service.py
scripts/tf_publisher.py
scripts/tf_subscriber.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
5. 运行
使用 launch 文件组织需要运行的节点,内容示例如下:
<launch>
<node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
<node pkg="turtlesim" type="turtle_teleop_key" name="key_control" output="screen"/>
<node pkg="demo06_test_flow_p" type="test01_turtle_spawn_p.py" name="turtle_spawn" output="screen"/>
<node pkg="demo06_test_flow_p" type="test02_turtle_tf_pub_p.py" name="tf_pub1" args="turtle1" output="screen"/>
<node pkg="demo06_test_flow_p" type="test02_turtle_tf_pub_p.py" name="tf_pub2" args="turtle2" output="screen"/>
<node pkg="demo06_test_flow_p" type="test03_turtle_tf_sub_p.py" name="tf_sub" output="screen"/>
</launch>
工作空间下,编译并初始化环境变量
catkin_make
source devel/setup.bash
roslaunch tflearn tf_learn.launch