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

前言

本人ROS小白,利用寒假时间学习ROS,在此以笔记的方式记录自己每天的学习过程。争取写满20篇(9/20)。
环境: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坐标变换

你可能感兴趣的:(学习,笔记,python,c++,vscode)