第五章--ROS常用组件学习笔记
第五章–ROS常用组件学习笔记
学习来源：
- 官方文档：http://wiki.ros.org/
- Introduction · Autolabor-ROS机器人入门课程《ROS理论与实践》零基础教程
本文仅作学习笔记和回顾使用
学习目标
- 了解坐标变换的原理、应用场景
- 编写复用性强的代码备用
- 学习适用rosbag、rqt等工具箱以便调试
正文
- TF坐标变换，实现不同类型的坐标系之间的转换；
- rosbag 用于录制ROS节点的执行过程并可以重放该过程；
- rqt 工具箱，集成了多款图形化的调试工具。
坐标变换应用场景
-
车载雷达定位
车载雷达定位的信息需要转化为相对于机器人系统的定位信息
-
机械臂定位
各种传感器定位信息转换为机械臂中心的定位信息
-
slam建图
涉及刚体运动变换
tf坐标常用包及信息
-
tf常用包：
- tf2：坐标变换的常用信息
- tf2_ros：常用API
- tf2_geometrt_msgs：ros信息转换成tf2信息
-
坐标信息：geometry_msgs下的两种类型
-
TransformStamped：坐标系的转换信息
std_msgs/Header header #头信息 uint32 seq #|-- 序列号 time stamp #|-- 时间戳 string frame_id #|-- 坐标 ID string child_frame_id #子坐标系的 id geometry_msgs/Transform transform #坐标信息 geometry_msgs/Vector3 translation #偏移量 float64 x #|-- X 方向的偏移量 float64 y #|-- Y 方向的偏移量 float64 z #|-- Z 方向上的偏移量 geometry_msgs/Quaternion rotation #四元数 float64 x float64 y float64 z float64 w-
PointStamped:坐标点的信息
std_msgs/Header header #头 uint32 seq #|-- 序号 time stamp #|-- 时间戳 string frame_id #|-- 所属坐标系的 id geometry_msgs/Point point #点坐标 float64 x #|-- x y z 坐标 float64 y float64 z -
静态坐标变换
定义
两个坐标系之间的相对位置是固定的
学习背景
-
雷达和小车的坐标系上的点相互转换
-
拓展应用：各种传感器的坐标点的转换，以便路径规划
编码实现
-
分析
-
node1:雷达和小车坐标系相对关系TransformStamped->发布方发布
-
node2:订阅方订阅相对关系，传入坐标点信息PointStamped，输出结果
-
-
编码
发布方
/* 静态坐标变换发布方: 发布关于 laser 坐标系的位置信息 实现流程: 1.包含头文件 2.初始化 ROS 节点 3.创建静态坐标转换广播器 4.创建坐标系信息 5.广播器发布坐标系信息 6.spin() */ // 1.包含头文件 #include "ros/ros.h" #include "tf2_ros/static_transform_broadcaster.h" #include "geometry_msgs/TransformStamped.h" #include "tf2/LinearMath/Quaternion.h" int main(int argc, char *argv[]) { setlocale(LC_ALL,""); // 2.初始化 ROS 节点 ros::init(argc,argv,"static_brocast"); // 3.创建静态坐标转换广播器 tf2_ros::StaticTransformBroadcaster broadcaster; // 4.创建坐标系信息 geometry_msgs::TransformStamped ts; //----设置头信息 ts.header.seq = 100; ts.header.stamp = ros::Time::now(); ts.header.frame_id = "base_link"; //----设置子级坐标系 ts.child_frame_id = "laser"; //----设置子级相对于父级的偏移量 ts.transform.translation.x = 0.2; ts.transform.translation.y = 0.0; ts.transform.translation.z = 0.5; //----设置四元数:将 欧拉角数据转换成四元数 tf2::Quaternion qtn; qtn.setRPY(0,0,0); ts.transform.rotation.x = qtn.getX(); ts.transform.rotation.y = qtn.getY(); ts.transform.rotation.z = qtn.getZ(); ts.transform.rotation.w = qtn.getW(); // 5.广播器发布坐标系信息 broadcaster.sendTransform(ts); ros::spin(); return 0; }
订阅方
/*
订阅坐标系信息，生成一个相对于 子级坐标系的坐标点数据，转换成父级坐标系中的坐标点
实现流程:
1.包含头文件
2.初始化 ROS 节点
3.创建 TF 订阅节点
4.生成一个坐标点(相对于子级坐标系)
5.转换坐标点(相对于父级坐标系)
6.spin()
*/
//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h" //注意: 调用 transform 必须包含该头文件
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化 ROS 节点
ros::init(argc,argv,"tf_sub");
ros::NodeHandle nh;
// 3.创建 TF 订阅节点
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
ros::Rate r(1);
while (ros::ok())
{
// 4.生成一个坐标点(相对于子级坐标系)
geometry_msgs::PointStamped point_laser;
point_laser.header.frame_id = "laser";
point_laser.header.stamp = ros::Time::now();
point_laser.point.x = 1;
point_laser.point.y = 2;
point_laser.point.z = 7.3;
// 5.转换坐标点(相对于父级坐标系)
//新建一个坐标点，用于接收转换结果
//--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------
try
{
geometry_msgs::PointStamped point_base;
point_base = buffer.transform(point_laser,"base_link");
ROS_INFO("转换后的数据:(%.2f,%.2f,%.2f),参考的坐标系是:",point_base.point.x,point_base.point.y,point_base.point.z,point_base.header.frame_id.c_str());
}
catch(const std::exception& e)
{
// std::cerr << e.what() << '\n';
ROS_INFO("程序异常.....");
}
r.sleep();
ros::spinOnce();
}
return 0;
}
动态坐标变换
定义
两个坐标系之间的相对位置是变化的
学习背景
-
世界坐标系和乌龟坐标系上的点的转换
-
拓展应用：各种传感器的坐标点的转换，以便路径规划
编码实现
-
分析
-
node1:TF广播
- 普通订阅方：/turtle1/pose话题动态订阅乌龟在世界坐标系上的位置信息
- TF广播发布方：设置源坐标系、子坐标系、相对信息、四元数转换
-
node2:
- TF订阅方：使用transform API实现坐标转换
-
-
编码
发布方
/* 动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的) 需求: 启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘 控制乌龟运动，将两个坐标系的相对位置动态发布 实现分析: 1.乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点 2.订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度 3.将 pose 信息转换成 坐标系相对信息并发布 实现流程: 1.包含头文件 2.初始化 ROS 节点 3.创建 ROS 句柄 4.创建订阅对象 5.回调函数处理订阅到的数据(实现TF广播) 5-1.创建 TF 广播器 5-2.创建 广播的数据(通过 pose 设置) 5-3.广播器发布数据 6.spin */ // 1.包含头文件 #include "ros/ros.h" #include "turtlesim/Pose.h" #include "tf2_ros/transform_broadcaster.h" #include "geometry_msgs/TransformStamped.h" #include "tf2/LinearMath/Quaternion.h" void doPose(const turtlesim::Pose::ConstPtr& pose){ // 5-1.创建 TF 广播器 static tf2_ros::TransformBroadcaster broadcaster; // 5-2.创建 广播的数据(通过 pose 设置) geometry_msgs::TransformStamped tfs; // |----头设置 tfs.header.frame_id = "world"; tfs.header.stamp = ros::Time::now(); // |----坐标系 ID tfs.child_frame_id = "turtle1"; // |----坐标系相对信息设置 tfs.transform.translation.x = pose->x; tfs.transform.translation.y = pose->y; tfs.transform.translation.z = 0.0; // 二维实现，pose 中没有z，z 是 0 // |--------- 四元数设置 tf2::Quaternion qtn; qtn.setRPY(0,0,pose->theta); tfs.transform.rotation.x = qtn.getX(); tfs.transform.rotation.y = qtn.getY(); tfs.transform.rotation.z = qtn.getZ(); tfs.transform.rotation.w = qtn.getW(); // 5-3.广播器发布数据 broadcaster.sendTransform(tfs); } int main(int argc, char *argv[]) { setlocale(LC_ALL,""); // 2.初始化 ROS 节点 ros::init(argc,argv,"dynamic_tf_pub"); // 3.创建 ROS 句柄 ros::NodeHandle nh; // 4.创建订阅对象 ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,doPose); // 5.回调函数处理订阅到的数据(实现TF广播) // // 6.spin ros::spin(); return 0; }
订阅方
//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h" //注意: 调用 transform 必须包含该头文件
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化 ROS 节点
ros::init(argc,argv,"dynamic_tf_sub");
ros::NodeHandle nh;
// 3.创建 TF 订阅节点
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
ros::Rate r(1);
while (ros::ok())
{
// 4.生成一个坐标点(相对于子级坐标系)
geometry_msgs::PointStamped point_laser;
point_laser.header.frame_id = "turtle1";
point_laser.header.stamp = ros::Time();
point_laser.point.x = 1;
point_laser.point.y = 1;
point_laser.point.z = 0;
// 5.转换坐标点(相对于父级坐标系)
//新建一个坐标点，用于接收转换结果
//--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------
try
{
geometry_msgs::PointStamped point_base;
point_base = buffer.transform(point_laser,"world");
ROS_INFO("坐标点相对于 world 的坐标为:(%.2f,%.2f,%.2f)",point_base.point.x,point_base.point.y,point_base.point.z);
}
catch(const std::exception& e)
{
// std::cerr << e.what() << '\n';
ROS_INFO("程序异常:%s",e.what());
}
r.sleep();
ros::spinOnce();
}
return 0;
}
多坐标变换
定义
多个坐标系的相互转换
学习背景
-
父级坐标系统 world,下有两子级系统 son1，son2，son1 相对于 world，以及 son2 相对于 world 的关系是已知的，求 son1原点在 son2中的坐标，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标
-
拓展应用：各种传感器的坐标点的转换，以便路径规划
编码实现
-
分析
-
node1:发布两个坐标系相对于世界坐标系的坐标转换信息
-
node2:订阅发布的坐标信息，tf2 API transform 实现两个坐标系的转换，然后实现两个坐标系中的点的转换
-
-
编码
<launch> <node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="0.2 0.8 0.3 0 0 0 /world /son1" output="screen" /> <node pkg="tf2_ros" type="static_transform_publisher" name="son2" args="0.5 0 0 0 0 0 /world /son2" output="screen" /> launch>
TF坐标变化实操：乌龟跟随
学习背景
产生两只乌龟，中间的乌龟(A) 和 左下乌龟(B), B 会自动运行至A的位置，并且键盘控制时，只是控制 A 的运动，但是 B 可以跟随 A 运行
编码实现
-
分析
-
node1：服务客户端利用"/spawn"话题生成第二只乌龟，不受键盘控制
-
node2：订阅 ”/乌龟名称/pose“ 话题，回调函数里发布方 动态传参args配合launch文件解析乌龟命令空间，TF广播发布两只乌龟的位姿信息
-
node3：订阅方订阅 ”/turtle2/cmd_vel“话题信息，通过 lookupTransform 获取两只乌龟的相对坐标信息，速度转换
-
-
编码
<launch> <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" /> <node pkg="turtlesim" type="turtle_teleop_key" name="key_control" output="screen"/> <node pkg="demo_tf2_test" type="Test01_Create_Turtle2" name="turtle2" output="screen" /> <node pkg="demo_tf2_test" type="Test02_TF2_Caster" name="caster1" output="screen" args="turtle1" /> <node pkg="demo_tf2_test" type="Test02_TF2_Caster" name="caster2" output="screen" args="turtle2" /> <node pkg="demo_tf2_test" type="Test03_TF2_Listener" name="listener" output="screen" /> launch>
- 客户端
/*
创建第二只小乌龟
*/
#include "ros/ros.h"
#include "turtlesim/Spawn.h"
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
//执行初始化
ros::init(argc,argv,"create_turtle");
//创建节点
ros::NodeHandle nh;
//创建服务客户端
ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");
ros::service::waitForService("/spawn");
turtlesim::Spawn spawn;
spawn.request.name = "turtle2";
spawn.request.x = 1.0;
spawn.request.y = 2.0;
spawn.request.theta = 3.12415926;
bool flag = client.call(spawn);
if (flag)
{
ROS_INFO("乌龟%s创建成功!",spawn.response.name.c_str());
}
else
{
ROS_INFO("乌龟2创建失败!");
}
ros::spin();
return 0;
}
- 发布方
/*
该文件实现:需要订阅 turtle1 和 turtle2 的 pose，然后广播相对 world 的坐标系信息
注意: 订阅的两只 turtle,除了命名空间(turtle1 和 turtle2)不同外,
其他的话题名称和实现逻辑都是一样的，
所以我们可以将所需的命名空间通过 args 动态传入
实现流程:
1.包含头文件
2.初始化 ros 节点
3.解析传入的命名空间
4.创建 ros 句柄
5.创建订阅对象
6.回调函数处理订阅的 pose 信息
6-1.创建 TF 广播器
6-2.将 pose 信息转换成 TransFormStamped
6-3.发布
7.spin
*/
//1.包含头文件
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"
#include "geometry_msgs/TransformStamped.h"
//保存乌龟名称
std::string turtle_name;
void doPose(const turtlesim::Pose::ConstPtr& pose){
// 6-1.创建 TF 广播器 ---------------------------------------- 注意 static
static tf2_ros::TransformBroadcaster broadcaster;
// 6-2.将 pose 信息转换成 TransFormStamped
geometry_msgs::TransformStamped tfs;
tfs.header.frame_id = "world";
tfs.header.stamp = ros::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;
tf2::Quaternion qtn;
qtn.setRPY(0,0,pose->theta);
tfs.transform.rotation.x = qtn.getX();
tfs.transform.rotation.y = qtn.getY();
tfs.transform.rotation.z = qtn.getZ();
tfs.transform.rotation.w = qtn.getW();
// 6-3.发布
broadcaster.sendTransform(tfs);
}
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化 ros 节点
ros::init(argc,argv,"pub_tf");
// 3.解析传入的命名空间
if (argc != 2)
{
ROS_ERROR("请传入正确的参数");
} else {
turtle_name = argv[1];
ROS_INFO("乌龟 %s 坐标发送启动",turtle_name.c_str());
}
// 4.创建 ros 句柄
ros::NodeHandle nh;
// 5.创建订阅对象
ros::Subscriber sub = nh.subscribe<turtlesim::Pose>(turtle_name + "/pose",1000,doPose);
// 6.回调函数处理订阅的 pose 信息
// 6-1.创建 TF 广播器
// 6-2.将 pose 信息转换成 TransFormStamped
// 6-3.发布
// 7.spin
ros::spin();
return 0;
}
- 订阅方
/*
订阅 turtle1 和 turtle2 的 TF 广播信息，查找并转换时间最近的 TF 信息
将 turtle1 转换成相对 turtle2 的坐标，在计算线速度和角速度并发布
实现流程:
1.包含头文件
2.初始化 ros 节点
3.创建 ros 句柄
4.创建 TF 订阅对象
5.处理订阅到的 TF
6.spin
*/
//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化 ros 节点
ros::init(argc,argv,"sub_TF");
// 3.创建 ros 句柄
ros::NodeHandle nh;
// 4.创建 TF 订阅对象
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
// 5.处理订阅到的 TF
// 需要创建发布 /turtle2/cmd_vel 的 publisher 对象
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",1000);
ros::Rate rate(10);
while (ros::ok())
{
try
{
//5-1.先获取 turtle1 相对 turtle2 的坐标信息
geometry_msgs::TransformStamped tfs = buffer.lookupTransform("turtle2","turtle1",ros::Time(0));
//5-2.根据坐标信息生成速度信息 -- geometry_msgs/Twist.h
geometry_msgs::Twist twist;
twist.linear.x = 0.5 * sqrt(pow(tfs.transform.translation.x,2) + pow(tfs.transform.translation.y,2));
twist.angular.z = 4 * atan2(tfs.transform.translation.y,tfs.transform.translation.x);
//5-3.发布速度信息 -- 需要提前创建 publish 对象
pub.publish(twist);
}
catch(const std::exception& e)
{
// std::cerr << e.what() << '\n';
ROS_INFO("错误提示:%s",e.what());
}
rate.sleep();
// 6.spin
ros::spinOnce();
}
return 0;
}
rosbag
官方文档：rosbag - ROS Wiki
概念
是用于录制和回放 ROS 主题的一个工具集。
作用
实现了数据的复用，方便调试、测试。
本质
rosbag本质也是ros的节点，当录制时，rosbag是一个订阅节点，可以订阅话题消息并将订阅到的数据写入磁盘文件；当重放时，rosbag是一个发布节点，可以读取磁盘文件，发布文件中的话题消息。
使用方法
- 开始录制
rosbag record -a -O 目标文件
Copy
操作小乌龟一段时间，结束录制使用 ctrl + c，在创建的目录中会生成bag文件。
- 查看文件
rosbag info 文件名
Copy
- 回放文件
rosbag play 文件名
Copy
重启乌龟节点，会发现，乌龟按照录制时的轨迹运动。
- 也可以使用编码实现，具备灵活性
rqt工具箱
概念
ROS基于 QT 框架，针对机器人开发提供了一系列可视化的工具，这些工具的集合就是rqt
作用
可以方便的实现 ROS 可视化调试，并且在同一窗口中打开多个部件，提高开发效率，优化用户体验。
组成
rqt 工具箱组成有三大部分
- rqt——核心实现，开发人员无需关注
- rqt_common_plugins——rqt 中常用的工具套件
- rqt_robot_plugins——运行中和机器人交互的插件(比如: rviz)
重要的图形化插件
- rqt_graph：可视化显示计算图
- rqt_console：用于显示和过滤日志的图形化插件
- rqt_plot ：可以以 2D 绘图的方式绘制发布在 topic 上的数据
- rqt_bag：录制和重放 bag 文件的图形化插件
总结
-
坐标变换√
-
rosbag调试√
-
rqt的图形化调试√
后续进入机器人系统仿真，实现复杂的功能，继续冲！！！！
rosbag record -a -O 目标文件
Copy
操作小乌龟一段时间，结束录制使用 ctrl + c，在创建的目录中会生成bag文件。
- 查看文件
rosbag info 文件名
Copy
- 回放文件
rosbag play 文件名
Copy
重启乌龟节点，会发现，乌龟按照录制时的轨迹运动。
- 也可以使用编码实现，具备灵活性
rqt工具箱
概念
ROS基于 QT 框架，针对机器人开发提供了一系列可视化的工具，这些工具的集合就是rqt
作用
可以方便的实现 ROS 可视化调试，并且在同一窗口中打开多个部件，提高开发效率，优化用户体验。
组成
rqt 工具箱组成有三大部分
- rqt——核心实现，开发人员无需关注
- rqt_common_plugins——rqt 中常用的工具套件
- rqt_robot_plugins——运行中和机器人交互的插件(比如: rviz)
重要的图形化插件
- rqt_graph：可视化显示计算图
- rqt_console：用于显示和过滤日志的图形化插件
- rqt_plot ：可以以 2D 绘图的方式绘制发布在 topic 上的数据
- rqt_bag：录制和重放 bag 文件的图形化插件
总结
-
坐标变换√
-
rosbag调试√
-
rqt的图形化调试√
后续进入机器人系统仿真，实现复杂的功能，继续冲！！！！
