在Jetson Nano上学习ROS的记录(版本Ubuntu18.04,课程来源赵虚左老师的《ROS理论与实践》)第十章-第二节 TF坐标变换实操

发布时间:2023-02-25 13:30

系列文章目录

第一章 ROS空间创建、helloworld的实现、开启多个节点
第二章 话题通信
第三章 服务通信
第四章 参数服务器
第五章 常用指令
第六章 通信机制实操
第七章 ROS通信机制进阶(常用API、Python模块的导入)
第八章 元功能包、节点运行管理launch文件(teleop_twist安装方法)
第九章 重名问题、分布式通信
第十章-第一节 TF坐标变换(内含PyKDL 和PyInit__tf2功能缺失等解决)
第十章-第二节 TF坐标变换实操

文章目录

  • 系列文章目录
  • 前言
  • 一、创建功能包
  • 二、服务客户端(生成乌龟)
  • 三、发布方(发布两只乌龟的坐标信息)
  • 四、订阅方(解析坐标信息并生成速度信息)
  • 五、运行
  • 六、小结
  • 总结


前言

现在大二,之前大一有幸参加了2021的国赛,很壮烈的拿了个江苏赛区的二等奖。但发现无人机这个题,真的是往堆钱上走了。不上ROS不行,现在来记录一下一个纯小白学习ROS的过程和遇到的问题。防止学弟、学妹们再走我走过的弯路。板子用的是学长给的Jetson Nano(4GB),版本是Ubuntu18.04(已配置好基础ROS所需配置)

需求描述:
程序启动之初: 产生两只乌龟,中间的乌龟(A) 和 左下乌龟(B), B 会自动运行至A的位置,并且键盘控制时,只是控制 A 的运动,但是 B 可以跟随 A 运行

实现分析:
乌龟跟随实现的核心,是乌龟A和B都要发布相对世界坐标系的坐标信息,然后,订阅到该信息需要转换获取A相对于B坐标系的信息,最后,再生成速度信息,并控制B运动。

  • 启动乌龟显示节点
  • 在乌龟显示窗体中生成一只新的乌龟(需要使用服务)
  • 编写两只乌龟发布坐标信息的节点
  • 编写订阅节点订阅坐标信息并生成新的相对关系生成速度信息

实现流程:

  • 新建功能包,添加依赖
  • 编写服务客户端,用于生成一只新的乌龟
  • 编写发布方,发布两只乌龟的坐标信息
  • 编写订阅方,订阅两只乌龟信息,生成速度信息并发布
  • 运行

准备工作:
1.了解如何创建第二只乌龟,且不受键盘控制
创建第二只乌龟需要使用rosservice,话题使用的是 spawn
键盘是无法控制第二只乌龟运动的,因为使用的话题: /第二只乌龟名称/cmd_vel,对应的要控制乌龟运动必须发布对应的话题消息
2.了解如何获取两只乌龟的坐标
是通过话题 /乌龟名称/pose 来获取的

一、创建功能包

创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

二、服务客户端(生成乌龟)

代码如下:

#! /usr/bin/env python
#encoding:utf-8
import rospy
from turtlesim.srv import *
"""
    需求:向服务器发送请求生成一只乌龟
        话题:/spawn
        消息:turtlesim/Spawn

        1.导包
        2.初始化ros节点
        3.创建服务的客户端对象
        4.组织数据并发送请求
        5.处理响应结果
"""

if __name__ == "__main__":
    # 2.初始化ros节点
    rospy.init_node("service_call_p")
    # 3.创建服务的客户端对象 参数:name:话题名称, service_class:消息名称
    client = rospy.ServiceProxy("/spawn",Spawn)
    # 4.组织数据并发送请求
    request = SpawnRequest()#创建请求数据对象
    request.x = 4.5
    request.y = 2.0
    request.theta = -3 #向右转-3个弧度
    request.name = "turtle2"
    #判断服务器状态,再发送
    client.wait_for_service()
    try:
        response = client.call(request)      #发送请求
        # 5.处理响应结果
        rospy.loginfo("生成乌龟到名字叫:%s",response.name)
    except Exception as e:
        rospy.logerr("请求处理异常")

三、发布方(发布两只乌龟的坐标信息)

代码如下:

#! /usr/bin/env python2.7
#encoding:utf-8
import rospy
from turtlesim.msg import Pose
import tf2_ros
from geometry_msgs.msg import TransformStamped
from tf.transformations import quaternion_from_euler
import sys
"""
    发布方:订阅乌龟的位姿信息,转换成坐标系的相对关系,再发布
    准备
            话题:/turtle/pose
            类型:/turtlesim/Pose

    流程:
            1.导包
            2.初始化ROS节点
            3.创建订阅对象
            4.回调函数处理订阅到的消息(核心)
            5.spin()

"""
# 接收乌龟名称的全局变量
turtle_name = ""

#回调函数:
def doPose(pose):   #参数是订阅到的消息
    #创建发布坐标系相对关系的对象
    pub = tf2_ros.TransformBroadcaster()

    #将pose转换成坐标系相对关系消息
    ts = TransformStamped()
    ts.header.frame_id = "world"    #被参考的坐标系是世界坐标系 父级坐标系
    ts.header.stamp = rospy.Time.now()

    #修改2:子级坐标系名称-----------------------------------------------------------------------------------------
    ts.child_frame_id = turtle_name
    #修改2:子级坐标系名称-----------------------------------------------------------------------------------------

    #子级坐标系相对于父级坐标系的偏移量
    ts.transform.translation.x = pose.x
    ts.transform.translation.y = pose.y
    ts.transform.translation.z = 0 #2D 没有z轴

    # 四元数
    #从欧拉角转换四元数
    """
        乌龟是2D的,不存在x上的翻滚,y上的俯仰,只有z上的偏航
        欧拉角:0 0 pose.theta
    """
    qtn = quaternion_from_euler(0,0,pose.theta)
    ts.transform.rotation.x = qtn[0]
    ts.transform.rotation.y = qtn[1]
    ts.transform.rotation.z = qtn[2]
    ts.transform.rotation.w = qtn[3]

    #发布
    pub.sendTransform(ts)

if __name__ == "__main__":
    # 2.初始化ROS节点
    rospy.init_node("dynamic_pub_p")

    # 解析传入的参数(文件全路径 + 传入的参数 + 节点名称 + 日志文件路径)
    if len(sys.argv)!=4:
        rospy.loginfo("参数个数不对")
        sys.exit
    else:
        turtle_name = sys.argv[1]

    # 3.创建订阅对象

    #修改1:话题名称-----------------------------------------------------------------------------------------
    sub = rospy.Subscriber(turtle_name + "/pose",Pose,doPose,queue_size=100)    #name话题名称, data_class消息类型, callback=None回调函数, queue_size=None队列长度
    #修改1:话题名称-----------------------------------------------------------------------------------------

    # 4.回调函数处理订阅到的消息(核心)
    # 5.spin()
    rospy.spin()

四、订阅方(解析坐标信息并生成速度信息)

代码如下:

#! /usr/bin/env python2.7
#encoding:utf-8
import rospy
import tf2_ros
import tf2_geometry_msgs
from geometry_msgs.msg import TransformStamped,Twist
import math

if __name__ == "__main__":

    # 2.初始化
    rospy.init_node("control_p")
    # 3.创建订阅对象
    #3-1创建缓存对象
    buffer = tf2_ros.Buffer()
    #3-2创建订阅对象(将缓存存入)
    sub = tf2_ros.TransformListener(buffer)

    #4.创建速度消息发布对象 键盘是无法控制第二只乌龟运动的,
    #因为使用的话题: /第二只乌龟名称/cmd_vel,对应的要控制乌龟运动必须发布对应的话题消息
    pub = rospy.Publisher("/turtle2/cmd_vel",Twist,queue_size=100)#话题;消息类型;queue_size

    # 5.转换逻辑实现,调用tf封装的算法
    rate = rospy.Rate(10)#循环发送 频率是10
    while not rospy.is_shutdown():
        try:
            #核心:计算turtle1相对于turtle2的坐标关系
            ts = buffer.lookup_transform("turtle2","turtle1",rospy.Time(0))   #target_frame: 目标坐标系, source_frame: 原坐标系, time: 0 即时间间隔最近的关系 返回值:son1与son2的坐标系关系
            rospy.loginfo("父级坐标系:%s,子级坐标系:%s,偏移量(%.2f,%.2f,%.2f)",
                                        ts.header.frame_id,
                                        ts.child_frame_id,
                                        ts.transform.translation.x,
                                        ts.transform.translation.y,
                                        ts.transform.translation.z
                                        )

            #组织Twist消息
            twist = Twist()
            #发布线速度和角速度
            #线速度 = 系数*两个坐标系原点的间距 = 系数*(x^2+y^2)再开方
            #角速度 = 系数*两个坐标系之间的夹角=系数*arctan(y,x)
            twist.linear.x = 0.5 * math.sqrt(math.pow(ts.transform.translation.x,2)+math.pow(ts.transform.translation.y,2))#线速度
            twist.angular.z=4*math.atan2(ts.transform.translation.y,ts.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="turtle1" output="screen"/>
    <node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen"/>

    
    
    <node pkg="tf04_test" type="test01_new_turtle_p.py" name="turtle2" output="screen"/>

    
    <node pkg="tf04_test" type="test02_pub_turtle_p.py" name="pub1" args="turtle1" output="screen"/>
    <node pkg="tf04_test" type="test02_pub_turtle_p.py" name="pub2" args="turtle2" output="screen"/>

    
    <node pkg="tf04_test" type="test03_control_turtle2_p.py" name="control" output="screen"/>
    
launch>

打开一个命令行:

cd workspace
roslaunch tf04_test test.launch 

在Jetson Nano上学习ROS的记录(版本Ubuntu18.04,课程来源赵虚左老师的《ROS理论与实践》)第十章-第二节 TF坐标变换实操_第1张图片

六、小结

坐标变换在机器人系统中是一个极其重要的组成模块,在 ROS 中 TF2 组件是专门用于实现坐标变换的,TF2 实现具体内容又主要介绍了如下几部分:

1.静态坐标变换广播器,可以编码方式或调用内置功能包来实现(建议后者),适用于相对固定的坐标系关系
2.动态坐标变换广播器,以编码的方式广播坐标系之间的相对关系,适用于易变的坐标系关系
3.坐标变换监听器,用于监听广播器广播的坐标系消息,可以实现不同坐标系之间或同一点在不同坐标系之间的变换
4.机器人系统中的坐标系关系是较为复杂的,还可以通过 tf2_tools 工具包来生成 ros 中的坐标系关系图
5.当前 TF2 已经替换了 TF,官网建议直接学习 TF2,并且 TF 与 TF2 的使用流程与实现 API 比较类似,只要有任意一方的使用经验,另一方也可以做到触类旁通

总结

以上就是今天要讲的内容,本文仅仅简单记录了TF坐标变换实操,如果有问题请在博客下留言或者咨询邮箱:layraliu@foxmail.com。

ItVuer - 免责声明 - 关于我们 - 联系我们

本网站信息来源于互联网,如有侵权请联系:561261067@qq.com

桂ICP备16001015号