ROS——坐标系管理、监听与广播、常用可视化工具

坐标系管理

在这里插入图片描述

TF功能包

在这里插入图片描述

小海龟追踪实验

ros版本(20.04)的tf安装命令: sudo apt-get install ros-noetic-turtle-tf
解决因python版本出现的无法生成跟随海龟： sudo ln -s /usr/bin/python3 /usr/bin/python ( -s 软链接,符号链接)
ln命令（英文全拼：link files）命令是一个非常重要命令，它的功能是为某一个文件在另外一个位置建立一个同步的链接。
当我们需要在不同的目录，用到相同的文件时，我们不需要在每一个需要的目录下都放一个必须相同的文件，我们只要在某个固定的目录，放上该文件，然后在其它的目录下用ln命令链接（link）它就可以，不必重复的占用磁盘空间。
Linux文件系统中，有所谓的链接(link)，我们可以将其视为档案的别名，而链接又可分为两种 : 硬链接(hard link)与软链接(symbolic link)，硬链接的意思是一个档案可以有多个名称，而软链接的方式则是产生一个特殊的档案，该档案的内容是指向另一个档案的位置。硬链接是存在同一个文件系统中，而软链接却可以跨越不同的文件系统。
不论是硬链接或软链接都不会将原本的档案复制一份，只会占用非常少量的磁碟空间。
在执行view_frames出现错误: cannot use a string pattern on a bytes-like object
解决方案: 将/opt/ros/noetic/lib/tf/view_frames文件第89行由 m = r.search(vstr) 修改为 m = r.search(str(vstr))（用str将vstr转化为string）。

可视化工具

rviz： noetic 版本，直接在终端执行 rviz即可
Fixed Frame：选择world
Add选项：添加TF

tf监听和广播

创建功能包

在这里插入图片描述

四元数

Quaternion（四元数）是一种扩展了复数概念的数学系统，由爱尔兰数学家威廉·罗文·汉密尔顿在1843年首次描述。四元数在三维空间的力学中被广泛应用。

一个四元数可以表示为：

q = a + bi + cj + dk

其中：

q 是四元数，
a, b, c, 和 d 是实数，
i, j, 和 k 是四元数的基本单位。
四元数的关键特性是乘法不满足交换律，也就是说 pq 并不一定等于 qp。

四元数在多个领域中都有应用，尤其是在计算机图形学、控制理论、信号处理、姿态控制、物理学和轨道力学中，因为它们提供了一种有效的方式去表示三维空间中的旋转或方向。

在电脑游戏和3D动画中，四元数常用于避免万向锁问题（Gimbal Lock），以及进行平滑的旋转插值（例如SLERP）。

RPY

在坐标系中，RPY代表“Roll（横滚）、Pitch（俯仰）和Yaw（偏航）”，这是描述一个刚体在三维空间中旋转的三个基本角度。这些术语最初源自航空和航海领域，用来描述飞行器或船只相对于地球的定向。

Roll（横滚）：绕着物体自身的 x 轴旋转。如果考虑的是一个飞机，横滚是指机翼的上下翻转。
Pitch（俯仰）：绕着物体自身的 y 轴旋转。俯仰是指飞机机头的上下移动。
Yaw（偏航）：绕着物体自身的 z 轴旋转。偏航是指飞机机头向左或向右的转动。
在机器人学、计算机图形学和导航系统中，RPY 角也被称为欧拉角的一种特定序列。当描述物体的旋转时，通常会按照一个固定的顺序应用这三个旋转，这个顺序可以是 XYZ 或者 ZYX 等，这取决于具体的应用场景和坐标系约定。

RPY 角可以单独使用，也可以组合起来形成一个旋转矩阵或者四元数，以便更有效地进行计算和表示刚体的完整姿态。在机器人技术中，RPY 常用来描述机械臂末端执行器或相机等传感器相对于世界坐标系的位置和方向。

实现 tf 广播器

// 例程产生 tf 数据，并计算、发布 turtle2的速度指令
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>std::string turtle_name;//pose回调函数
void poseCallback(const turtlesim::PoseConstPtr& msg){//创建tf的广播器static tf::TransformBroadcaster br;//初始化tf数据tf::Transform transform;transform.setOrigin(tf::Vector3(msg->x,msg->y,0.0));tf::Quaternion q;q.setRPY(0,0,msg->theta);transform.setRotation(q);//广播world与海龟坐标系之间的tf数据br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"world",turtle_name));
}int main(int argc,char** argv){ros::init(argc,argv,"my_broadcaster");//输入参数作为海龟的名字if(argc != 2){ROS_ERROR("need turtle name as argument!");return -1;}turtle_name = argv[1];//订阅海龟位置ros::NodeHandle node;ros::Subscriber sub = node.subscribe(turtle_name+"/pose",10,&poseCallback);// 循环等待回调函数ros::spin();return 0;
}

实现tf监听器

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>int main(int argc,char **argv){ros::init(argc,argv,"my_tf_listener");ros::NodeHandle node;//请求产生turtle2ros::service::waitForService("/spawn");ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");turtlesim::Spawn srv;add_turtle.call(srv);//创建发布turtle2速度控制指令的发布者ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",10);//创建tf的监听器tf::TransformListener listener;ros::Rate rate(10.0);while(node.ok()){//获取turtle1与turtle2坐标系之间的tf数据tf::StampedTransform transform;try{// 判断坐标系中是否存在 turtle1 和 turtle2，持续三秒listener.waitForTransform("/turtle2","/turtle1",ros::Time(0),ros::Duration(3.0));// 获取tutle1 和 turtle2 的实时位置，将四元数存到transform变量中listener.lookupTransform("turtle2","/turtle1",ros::Time(0),transform);}catch(tf::TransformException &ex){ROS_ERROR("%s",ex.what());ros::Duration(1.0).sleep();continue;}//根据turtle1与turtle2坐标系之间的位置关系，发布turtle2的速度指令geometry_msgs::Twist vel_msg;vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),transform.getOrigin().x());vel_msg.angular.x = 0.5 * sqrt(pow(transform.getOrigin().x(),2) + pow(transform.getOrigin().y(),2));turtle_vel.publish(vel_msg);rate.sleep();}return 0;
}