ROS学习笔记9：TF坐标变换实操

已于 2024-08-15 11:18:58 修改
阅读量998 收藏 23
点赞数 19
分类专栏： ROS操作系统入门 文章标签： 学习 笔记 python c++ vscode
于 2024-01-30 00:25:58 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/s13352784013/article/details/135923712
版权
本文介绍了如何使用ROS（RobotOperatingSystem）环境中的turtlesim包，通过Python编程实现两个虚拟乌龟的跟随系统，包括生成乌龟、发布坐标信息、订阅并动态调整第二只乌龟的速度以跟随第一只乌龟。
摘要由CSDN通过智能技术生成

前言

本人ROS小白，利用寒假时间学习ROS，在此以笔记的方式记录自己每天的学习过程。争取写满15篇(9/15)。
环境：Ubuntu20.04、ROS1：noetic
环境配置：严格按照下方学习链接的教程配置，基本一次成功。
学习链接：【Autolabor初级教程】ROS机器人入门
对应链接文档：ROS机器人入门课程《ROS理论与实践》
笔记绝大部分代码使用Python语言编写。
本期关键词：乌龟跟随

要求

产生两只乌龟，中间的乌龟(A) 和 左下乌龟(B), B 会自动运行至A的位置，并且键盘控制时，只是控制 A 的运动，但是 B 可以跟随 A 运行。

思路：

  1. 生成乌龟
  2. 发布方：发布两只乌龟的坐标信息
  3. 订阅方：解析坐标信息并生成速度信息

一、生成乌龟

新乌龟的生成与服务调用有关。这里新乌龟的生成方法和以前我们提到过的基本一致，这里直接展示示例代码：

#! 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"的后缀，不然会报错！

二、发布方实现

  1. 难点：发布两只乌龟的坐标信息
  2. 在之前的学习过程中，我们发布过一只乌龟的坐标信息，而本次实操我们需要发布两只乌龟的坐标信息，代码要如何实现呢？
  3. 利用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()
  1. 对应的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>
  1. 还有另一种方法，就是创建两个订阅者，分别监听两个乌龟的位姿话题，示例代码如下：
#! /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()
  1. 对应的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>
  1. 经过测试，以上方法也是可行的，不过对比之下，个人认为第一种方法更好。

三、订阅方实现

  1. 订阅方实现只需要将两只乌龟的坐标关系进行比较，然后通过距离设置第二只乌龟的运动参数即可，示例代码如下：
#! /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()
  1. 对应的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>

往期内容

  1. ROS学习笔记1：基础知识
  2. ROS学习笔记2：话题通信
  3. ROS学习笔记3：服务通信与参数服务器
  4. ROS常用命令记录
  5. ROS学习笔记4：通信机制实操
  6. ROS学习笔记5：常用API和模块导入
  7. ROS学习笔记6：launch文件
  8. ROS学习笔记7：重名解决与名称设置
  9. ROS学习笔记8：TF坐标变换
代码能跑就可以
关注
专栏目录
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值