ROS学习第五章

本章学习一些ROS内置的组件，提升开发效率

坐标用于描述物体与机器人在横向，纵向以及高度上的距离信息；但传感器返回的坐标通常需要先进行处理，比如：复杂的手眼协调

两个常用的msg

Header：frame_id是被参考的坐标系名字

Transform：child与参考坐标系的偏移量，quaternion则是使用了四元数，描述角度

Header部分：seq序列号 stamp时间戳 frame_id是说以哪个坐标系为参考系

先学习静态坐标变换，静态的意思是两坐标系的相对位置固定，比如车载雷达

案例：订阅方收到坐标系相对关系，处理坐标点信息（借助tf函数实现坐标变换）

发布案例要好好学习，关键点有pub定义，欧拉角转换四元数xyzw四个参数以及组织被发布的数据

#! /usr/bin/env python

import rospy

import tf2_ros

from geometry_msgs.msg import TransformStamped

import tf

import tf.transformations

"""

车辆底盘：base_link 雷达：laser

"""

if __name__ =="__main__":

    rospy.init_node("bianhuan")

    pub=tf2_ros.StaticTransformBroadcaster()

    ts=TransformStamped()

    ts.header.stamp=rospy.Time.now()

    ts.header.frame_id="base_link"

    ts.child_frame_id="laser"

    ts.transform.translation.x=2.0

    ts.transform.translation.y=0.0

    ts.transform.translation.z=0.5

    qtn=tf.transformations.quaternion_from_euler(0,0,0)

    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)

    rospy.spin()

可以打开rviz来看

创建订阅对象时还要创建缓存对象，将缓存传入订阅对象

写起来比较艰难

#! /usr/bin/env python

import rospy

import tf2_ros

from tf2_geometry_msgs import tf2_geometry_msgs

if __name__=="__main__":

    rospy.init_node("subber")

    buffer=tf2_ros.Buffer()

    sub=tf2_ros.TransformListener(buffer)

    ps=tf2_geometry_msgs.PointStamped()

    ps.header.frame_id="laser"

    ps.header.stamp=rospy.Time.now()

    ps.point.x=2.0

    ps.point.y=3.0

    ps.point.z=5.0

    reta=rospy.Rate(10)

    while not rospy.is_shutdown():

        try:

            psout=buffer.transform(ps,"base_link")

            rospy.logerr("坐标:%.2f,%.2f,%.2f",psout.point.x,psout.point.y,psout.point.z)

            reta.sleep()

            rospy.logerr("1")

        except Exception as e:

            rospy.logerr("2")

    rospy.spin()

比较坑的就是数据的组织格式是tf2_geometry_msgs，转换时可能还没有订阅到坐标系的相关信息报错

比如再添加一个固定摄像头，方法与雷达一样，改点参数就可以

直接命令行发布的一种方法，简洁，后期一般在launch实现

动态坐标变换，两个坐标系参考位置不断变化

坐标关系的发布方需要解析/turtle1/pose话题，欧拉角三个参数分别是绕x,y,z的旋转量

#! /usr/bin/env python

import rospy

import turtlesim

from turtlesim.msg import Pose

from geometry_msgs.msg import TransformStamped

import tf2_ros

import tf、

import tf.transformations

def popo(p):

    pub=tf2_ros.StaticTransformBroadcaster()

    ts=TransformStamped()

    ts.header.frame_id="base_link"

    ts.header.stamp=rospy.Time.now()

    ts.child_frame_id="guigui"

    ts.transform.translation.x=p.x

    ts.transform.translation.y=p.y

    ts.transform.translation.z=0

    tte=tf.transformations.quaternion_from_euler(0,0,p.theta)

    ts.transform.rotation.x=tte[0]

    ts.transform.rotation.y=tte[1]

    ts.transform.rotation.z=tte[2]

    ts.transform.rotation.w=tte[3]

    pub.sendTransform(ts)

   

if __name__=="__main__":

    rospy.init_node("dongtai")

    sub=rospy.Subscriber("/turtle1/pose",Pose,popo,queue_size=10)

    rospy.spin()

PS：此时如果要写一个sub，要注意时间戳最好不要设置，防止动态坐标转换出bug（静态不用考虑这些，因为是恒定的坐标关系）

之后来看多坐标变换，需求：当前有三个坐标系，son1,son2与world，son1与son2分别相对于world的关系已知，求son1一点在son2中的坐标

首先发布son1,son2对world的关系，使用launch文件

<launch>

    <node pkg="tf2_ros" type="static_transform_publisher" name="son1" output="screen" args="5 0 0 0 0 0 /world /son1"/>

    <node pkg="tf2_ros" type="static_transform_publisher" name="son2" output="screen" args="3 0 0 0 0 0 /world /son2"/>

</launch>

操作的要点是一个函数

ts=buffer.lookup_transform("son2","son1",rospy.Time(0))

#! /usr/bin/env python

import rospy

import tf2_ros

import tf2_geometry_msgs

from geometry_msgs.msg import TransformStamped

if __name__=="__main__":

    rospy.init_node("zhuanhuan")

    buffer=tf2_ros.Buffer()

    sub=tf2_ros.TransformListener(buffer)  

    ps=tf2_geometry_msgs.PointStamped()

    ps.header.stamp=rospy.Time.now()

    ps.header.frame_id="son1"

    ps.point.x=1

    ps.point.y=1

    ps.point.z=1

    rata=rospy.Rate(10)

    while not rospy.is_shutdown():

        try:

            ts=buffer.lookup_transform("son2","son1",rospy.Time(0))

            rospy.loginfo("%s,%s",ts.child_frame_id,ts.header.frame_id)

            psout=buffer.transform(ps,"son2")

            rospy.loginfo("%s,%.2f",psout.header.frame_id,psout.point.x)

        except Exception as e:

            rospy.loginfo("错误")

学习一种查看坐标关系的新方法

rosrun tf2_tools view_frames.py

在调用的目录下生成这样的PDF

实战项目：程序运行会创建两只乌龟，追击乌龟刷新在world(0,0)的位置，追中间的龟，而中间的逃跑乌龟可以受键盘控制，把第二只乌龟的坐标在第一只龟运动坐标系的坐标求出是我们的思路

#! /usr/bin/env python

import rospy

import turtlesim

from turtlesim.srv import Spawn

if __name__=="__main__":

    rospy.init_node("sheng")

    cliant=rospy.ServiceProxy("/spawn",Spawn)

    s=Spawn()

    s.x=1.0

    s.y=1.0

    s.theta=0.837

    s.name="turtle2"

    rospy.wait_for_service("/spawn")

    cliant.call(s.x,s.y,s.theta,s.name)

第一步完成，生成一个名字叫turtle2的

<launch>

    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen"/>

    <node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen"/>

    <node pkg="zuobiao" type="shenggui.py" output="screen" name="turtle2"/>

</launch>

第二步将位置信息转换成坐标信息，已经做过，再敲一遍吧

import下sys,因为我们通过arg传入参数，通过解析参数可以实现两只乌龟的复用

在launch下启动会传入多个参数，分别是文件全路径+传入的参数+节点名称+日志文件路径

#! /usr/bin/env python

import rospy

from turtlesim.msg import Pose

from tf2_geometry_msgs.tf2_geometry_msgs import *

import tf2_ros

import tf.transformations

from geometry_msgs.msg import TransformStamped

import sys

def doo(poose):

    ts=TransformStamped()

    qtn=tf.transformations.quaternion_from_euler(0,0,poose.theta)

    ts.header.frame_id="world"

    ts.header.stamp=rospy.Time.now()

    ts.child_frame_id=turtle_name

    ts.transform.translation.x=poose.x

    ts.transform.translation.y=poose.y

    ts.transform.translation.z=0

    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=tf2_ros.StaticTransformBroadcaster()

    pub.sendTransform(ts)

   

if __name__ == "__main__":

    rospy.sleep(1)

    if(len(sys.argv)==4):

        turtle_name=sys.argv[1]

        rospy.logerr("%s",turtle_name)

    rospy.init_node(turtle_name+"2222")

    sub=rospy.Subscriber(turtle_name+"/pose",Pose,doo,queue_size=100)

    rospy.spin()

这种sys.argv的方式要学会

最后一步，运动控制，两坐标系原点矩

#! /usr/bin/env python

import rospy

import tf2_ros

import tf2_geometry_msgs

from geometry_msgs.msg import TransformStamped

from geometry_msgs.msg import Twist

import math

if __name__=="__main__":

    rospy.init_node("zhuanhuan")

    buffer=tf2_ros.Buffer()

    sub=tf2_ros.TransformListener(buffer)  

    ps=tf2_geometry_msgs.PointStamped()

    ps.header.stamp=rospy.Time(0)

    ps.header.frame_id="turtle1"

    ps.point.x=0

    ps.point.y=0

    ps.point.z=0

    rata=rospy.Rate(10)

    while not rospy.is_shutdown():

        try:

            ts=buffer.lookup_transform("turtle2","turtle1",rospy.Time(0))

            psout=buffer.transform(ps,"turtle2")

            rospy.loginfo("%.2f,%.2f",psout.point.x,psout.point.y)

            pub=rospy.Publisher("/turtle2/cmd_vel",Twist,queue_size=10)

            tw=Twist()

            tw.linear.x=math.sqrt(psout.point.x*psout.point.x+psout.point.y*psout.point.y)/5

            tw.angular.z=math.atan2(psout.point.y,psout.point.x)*3

            pub.publish(tw)

        except Exception as e:

            rospy.loginfo("错误")

TF2是TF的超集，TF2做了分包处理

学习ROSBAG组件，用来存取数据，比如有一种地图生成方式，就是将机器人感知到的数据留存，事后读取数据，生成地图信息，关于数据的留存以及读取实现，就用到了这个工具，他是用于录制和回放ROS主题的一个工具集，实现了数据的复用，方便程序的测试

ROSBAG本质也是ROS节点，当录制时，他是一个订阅节点，可以订阅话题消息并将数据写入磁盘文件，重放时，发布文件中的话题消息

命令行录制乌龟运动：

-a是所有消息都录制，-o后面跟输出目录 按CTRL+C结束录制

rosbag play hello.bag就可以复现之前操作

之后代码实现，注意创建rosbag对象同时会打开数据流，最后要记得关闭

#! /usr/bin/env python

import rospy

import rosbag

from geometry_msgs.msg import Twist

if __name__=="__main__":

    rospy.init_node("write")

    bag=rosbag.Bag("hello.bag",'w')

    t=Twist()

    bag.write("/turtle/cmd_vel",t)

    bag.close()

真就只获取了一条

之后玩下读数据

#! /usr/bin/env python

import rospy

import rosbag

from geometry_msgs.msg import Twist

if __name__ == "__main__":

    rospy.init_node("read")

    bag=rosbag.Bag("hello.bag",'r')

    msgs=bag.read_messages(topics="/turtle/cmd_vel")

    for topic,msg,time in msgs:

        rospy.loginfo("%s %s %s",topic,msg,time)

    bag.close()

返回的msgs分三个大类，topic,msg还有time

本章最后是RQT工具箱

命令行键入rqt即可启动

在插件中选择功能

之后看下之前就用过的rqt_graph，跑个复杂点的先前的跟随案例

之后看rqt_console插件，它是用于显示和过滤日志的

rqt_plot可以订阅topic并以图像化方式画出

rqt_bag用来录制

文件循环播放，注意播放要选publish