前言
本人ROS小白，利用寒假时间学习ROS，在此以笔记的方式记录自己每天的学习过程。争取写满20篇(9/20)。
环境：Ubuntu20.04、ROS1：noetic
环境配置：严格按照下方学习链接的教程配置，基本一次成功。
学习链接：【Autolabor初级教程】ROS机器人入门
对应链接文档：ROS机器人入门课程《ROS理论与实践》
笔记绝大部分代码使用Python语言编写。
本期关键词：乌龟跟随
要求
产生两只乌龟，中间的乌龟(A) 和 左下乌龟(B), B 会自动运行至A的位置，并且键盘控制时，只是控制 A 的运动，但是 B 可以跟随 A 运行。
思路：
- 生成乌龟
- 发布方：发布两只乌龟的坐标信息
- 订阅方：解析坐标信息并生成速度信息
一、生成乌龟
新乌龟的生成与服务调用有关。这里新乌龟的生成方法和以前我们提到过的基本一致，这里直接展示示例代码：
#! usr/bin/env python
import rospy
from turtlesim.srv import Spawn,SpawnRequest,SpawnResponse
if __name__=="__main__":
rospy.init_node("turtle_create1")
client=rospy.ServiceProxy("/spawn",Spawn)
# wait for service
client.wait_for_service()
# send request
req=SpawnRequest()
req.x=3
req.y=3
req.theta=0
req.name="my_turtle"
try:
response = client.call(req)
rospy.loginfo("turtle create successfully! Named: %s",response.name)
rospy.loginfo("turtle's location: x=%.2f,y=%.2f",req.x,req.y)
except Exception as e:
rospy.logerr("create false!")
配置launch文件，如下：
<launch>
<node pkg="turtlesim" type="turtlesim_node" name="t1" />
<node pkg="turtlesim" type="turtle_teleop_key" name="t2" />
<node pkg="helloworld" type="double_turtle1.py" name="t3" />
</launch>
注意：如果launch文件的"type"参数包含的是Python文件，那么一定要加上".py"的后缀，不然会报错！
二、发布方实现
- 难点：发布两只乌龟的坐标信息
- 在之前的学习过程中，我们发布过一只乌龟的坐标信息，而本次实操我们需要发布两只乌龟的坐标信息，代码要如何实现呢？
- 利用Python的系统模块
sys来获取输入参数，从而实现对两只乌龟坐标信息的发布，示例代码如下：
#! /usr/bin/env python
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):
#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__":
rospy.init_node("sub_tfs_p")
# 需要和launch文件的args（将参数传递给节点）搭配使用
if len(sys.argv) < 2:
rospy.loginfo("请传入参数:乌龟的命名空间")
else:
# 第一个参数默认是脚本名称
turtle_name = sys.argv[1]
# 位姿话题：/turtle1/pose /turtle2/pose
rospy.Subscriber(turtle_name + "/pose",Pose,doPose)
rospy.spin()
- 对应的launch文件代码：
<launch>
<node pkg="turtlesim" type="turtlesim_node" name="t1" />
<node pkg="turtlesim" type="turtle_teleop_key" name="t2" />
<node pkg="helloworld" type="double_turtle1.py" name="t3" output="screen"/>
<node pkg="helloworld" type="double_turtle2.py" name="t4" args="turtle1" output="screen" />
<node pkg="helloworld" type="double_turtle2.py" name="t5" args="turtle2" output="screen" />
</launch>
- 还有另一种方法，就是创建两个订阅者，分别监听两个乌龟的位姿话题，示例代码如下：
#! /usr/bin/env python
import rospy
import tf2_ros
import tf
from turtlesim.msg import Pose
import tf.transformations
from geometry_msgs.msg import TransformStamped
def doPose1(pose):
# 创建一个广播器
broadcaster=tf2_ros.TransformBroadcaster()
# 创建要被广播的数据
d_pose=TransformStamped()
d_pose.header.frame_id="world"
d_pose.header.stamp=rospy.Time.now()
d_pose.child_frame_id="turtle1"
d_pose.transform.translation.x=pose.x
d_pose.transform.translation.y=pose.y
d_pose.transform.translation.z=0.0
qua=tf.transformations.quaternion_from_euler(0,0,pose.theta)
d_pose.transform.rotation.x=qua[0]
d_pose.transform.rotation.y=qua[1]
d_pose.transform.rotation.z=qua[2]
d_pose.transform.rotation.w=qua[3]
broadcaster.sendTransform(d_pose)
def doPose2(pose):
# 创建一个广播器
broadcaster=tf2_ros.TransformBroadcaster()
# 创建要被广播的数据
d_pose=TransformStamped()
d_pose.header.frame_id="world"
d_pose.header.stamp=rospy.Time.now()
d_pose.child_frame_id="turtle2"
d_pose.transform.translation.x=pose.x
d_pose.transform.translation.y=pose.y
d_pose.transform.translation.z=0.0
qua=tf.transformations.quaternion_from_euler(0,0,pose.theta)
d_pose.transform.rotation.x=qua[0]
d_pose.transform.rotation.y=qua[1]
d_pose.transform.rotation.z=qua[2]
d_pose.transform.rotation.w=qua[3]
broadcaster.sendTransform(d_pose)
if __name__=="__main__":
rospy.init_node("dynamic_pub")
# 我们要根据/turtle1/pose的数据来设置tf坐标系的数据，所以先订阅/turtle1/pose
sub1=rospy.Subscriber("/turtle1/pose",Pose,doPose1)
sub2=rospy.Subscriber("/turtle2/pose",Pose,doPose2)
rospy.spin()
- 对应的launch文件为：
<launch>
<node pkg="turtlesim" type="turtlesim_node" name="t1" />
<node pkg="turtlesim" type="turtle_teleop_key" name="t2" />
<node pkg="helloworld" type="double_turtle1.py" name="t3" output="screen"/>
<node pkg="helloworld" type="double_turtle2_test.py" name="t7" output="screen" />
<node pkg="helloworld" type="double_turtle3.py" name="t6" output="screen" />
</launch>
- 经过测试，以上方法也是可行的，不过对比之下，个人认为第一种方法更好。
三、订阅方实现
- 订阅方实现只需要将两只乌龟的坐标关系进行比较，然后通过距离设置第二只乌龟的运动参数即可，示例代码如下：
#! /usr/bin/env python
import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped, Twist
import math
if __name__ == "__main__":
rospy.init_node("sub_tfs_p")
buffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(buffer)
rate = rospy.Rate(10)
# 创建速度发布对象
pub = rospy.Publisher("/turtle2/cmd_vel",Twist,queue_size=1000)
while not rospy.is_shutdown():
try:
# turtle1-turtle2
trans = buffer.lookup_transform("turtle2","turtle1",rospy.Time(0))
twist = Twist()
# 间距 = x^2 + y^2 然后开方
# 通过距离（角度）推理速度，简单的公式转换，参数可根据需求自行设置
twist.linear.x = 2 * 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)
rate.sleep()
- 对应的launch文件：
<launch>
<node pkg="turtlesim" type="turtlesim_node" name="t1" />
<node pkg="turtlesim" type="turtle_teleop_key" name="t2" />
<node pkg="helloworld" type="double_turtle1.py" name="t3" output="screen"/>
<node pkg="helloworld" type="double_turtle2.py" name="t4" args="turtle1" output="screen" />
<node pkg="helloworld" type="double_turtle2.py" name="t5" args="turtle2" output="screen" />
<node pkg="helloworld" type="double_turtle3.py" name="t6" output="screen" />
</launch>
1914
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
