【Autolabor初级教程】ROS机器人入门
1. TF 坐标变换
- 背景
- 现有一移动式机器人底盘，在底盘上安装了一雷达，雷达相对于底盘的偏移量已知，现雷达检测到一障碍物信息，获取到坐标分别为(x,y,z)，该坐标是以雷达为参考系的，如何将这个坐标转换成以小车为参考系的坐标呢？
-
概念
- tf：TransForm Frame，坐标变换
- 坐标系：ROS 中是通过坐标系统开标定物体的，确切的将是通过右手坐标系来标定的
-
作用
- 在 ROS 中用于实现不同坐标系之间的点或向量的转换
-
tf2 对应的常用功能包
- tf2_geometry_msgs：可以将 ROS 消息转换成 tf2 消息
- tf2：封装了坐标变换的常用消息
- tf2_ros：为 tf2 提供了 roscpp 和 rospy 绑定，封装了坐标变换常用的 API
1.1 坐标 msg 消息
-
geometry_msgs/TransformStamped：用于传输坐标系相关位置信息
$ rosmsg info geometry_msgs/TransformStampedstd_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 -
geometry_msgs/PointStamped：用于传输某个坐标系内坐标点的信息
$ rosmsg info geometry_msgs/PointStampedstd_msgs/Header header #头 uint32 seq #|-- 序号 time stamp #|-- 时间戳 string frame_id #|-- 所属坐标系的 id geometry_msgs/Point point #点坐标 float64 x #|-- x y z 坐标 float64 y float64 z
1.2 静态坐标变换
-
所谓静态坐标变换，是指两个坐标系之间的相对位置是固定的
-
需求描述
现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，坐标系的原点分别位于主体与雷达的物理中心，已知雷达原点相对于主体原点位移关系如下: x 0.2 y 0.0 z 0.5。当前雷达检测到一障碍物，在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0)，请问该障碍物相对于主体的坐标是多少？
-
结果演示
-
实现流程
-
创建功能包
$ mkdir -p demo04_ws/src $ cd demo04_ws $ catkin_make $ code . # 下方代码运行前记得启动 roscore- 在 src 下创建功能包 tf01_static，并添加依赖 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs
-
发布方：在功能包下的 src 创建 demo01_static_pub.cpp 节点文件
#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, ""); ros::init(argc, argv, "static_pub"); ros::NodeHandle nh; // 创建发布对象（静态坐标转换广播器） tf2_ros::StaticTransformBroadcaster pub; // 组织被发布的消息（创建坐标系信息） geometry_msgs::TransformStamped tfs; // 设置头信息 tfs.header.seq = 100; tfs.header.stamp = ros::Time::now(); tfs.header.frame_id = "base_link"; // 相对坐标系关系中被参考的那一个 // 设置子级坐标系 tfs.child_frame_id = "laser"; // 设置子级相对于父级的偏移量 tfs.transform.translation.x = 0.2; tfs.transform.translation.y = 0.0; tfs.transform.translation.z = 0.5; // 设置四元数：将欧拉角数据转换成四元数 tf2::Quaternion qtn; // 向该对象设置欧拉角，这个对象可以将欧拉角转换成四元数 qtn.setRPY(0, 0, 0); // 欧拉角的单位是弧度 tfs.transform.rotation.x = qtn.getX(); tfs.transform.rotation.y = qtn.getY(); tfs.transform.rotation.z = qtn.getZ(); tfs.transform.rotation.w = qtn.getW(); // 广播器发布坐标系信息 pub.sendTransform(tfs); ros::spin(); return 0; } -
订阅方：在功能包下的 src 创建 demo02_static_sub.cpp 节点文件
#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, ""); ros::init(argc, argv, "static_sub"); ros::NodeHandle nh; // 创建一个 buffer 缓存 tf2_ros::Buffer buffer; // 创建监听对象（监听对象可以将订阅的数据存入 buffer） tf2_ros::TransformListener listener(buffer); // 组织一个坐标点数据 geometry_msgs::PointStamped ps; ps.header.frame_id = "laser"; ps.header.stamp = ros::Time::now(); ps.point.x = 2.0; ps.point.y = 3.0; ps.point.z = 5.0; // 转换算法，需要调用 TF 内置实现 //ros::Duration(2).sleep(); // 添加休眠 ros::Rate rate(10); while (ros::ok()) { try { // 将 ps 转换成相对于 base_link 的坐标点 geometry_msgs::PointStamped ps_out; /* 调用了 buffer 的转换函数 transform 参数1: 被转换的坐标点 参数2: 目标坐标系 返回值: 输出的坐标点 PS1：调用时必须包含头文件 tf2_geometry_msgs/tf2_geometry_msgs.h PS2：运行时存在的问题：抛出一个异常 base link 不存在 原因：订阅数据是一个耗时操作，可能在调用 transform 转换函数时，坐标系的相对关系还没有订阅到，因此出现异常 解决： 方案1: 在调用转换函数前，执行休眠 方案2: 进行异常处理（建议使用该方案） */ ps_out = buffer.transform(ps, "base_link"); // 最后输出转换后的坐标点 ROS_INFO("转换后的坐标:(%.2f,%.2f,%.2f), 参考的坐标系: %s", ps_out.point.x, ps_out.point.y, ps_out.point.z, ps_out.header.frame_id.c_str()); } catch(const std::exception& e) { std::cerr << e.what() << '\n'; } rate.sleep(); ros::spinOnce(); } return 0; } -
编辑配置文件 CMakeLists.txt
add_executable(demo01_static_pub src/demo01_static_pub.cpp) add_executable(demo02_static_sub src/demo02_static_sub.cpp) target_link_libraries(demo01_static_pub ${catkin_LIBRARIES} ) target_link_libraries(demo02_static_sub ${catkin_LIBRARIES} )
-
-
补充
- 当坐标系之间的相对位置固定时，那么所需参数也是固定的: 父系坐标名称、子级坐标系名称、x 偏移量、y 偏移量、z 偏移量、x 翻滚角度、y 俯仰角度、z 偏航角度，实现逻辑相同，参数不同，那么 ROS 系统就已经封装好了专门的节点，使用方式如下
$ rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滚角度 父级坐标系 子级坐标系 $ rosrun tf2_ros static_transform_publisher 0.2 0 0.5 0 0 0 /baselink /laser
也可以借助于 rviz 显示坐标系关系
- 新建窗口输入命令：rviz
- 在启动的 rviz 中设置 Fixed Frame 为 base_link
- 点击左下的 add 按钮，在弹出的窗口中选择 TF 组件，即可显示坐标关系
- 当坐标系之间的相对位置固定时，那么所需参数也是固定的: 父系坐标名称、子级坐标系名称、x 偏移量、y 偏移量、z 偏移量、x 翻滚角度、y 俯仰角度、z 偏航角度，实现逻辑相同，参数不同，那么 ROS 系统就已经封装好了专门的节点，使用方式如下
1.3 动态坐标变换
-
所谓动态坐标变换，是指两个坐标系之间的相对位置是变化的
-
需求描述
- 启动 turtlesim_node，该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布
-
实现流程
-
创建功能包
$ cd demo04_ws $ code . # 下方代码运行前记得启动 roscore- 在 src 下创建功能包 tf02_dynamic，并添加依赖 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim
-
发布方：在功能包下的 src 创建 demo01_dynamic_pub.cpp 节点文件
#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) { // 获取位姿信息，转换成坐标系相对关系并发布 // 创建发布对象 static tf2_ros::TransformBroadcaster pub; // 组织被发布的数据 geometry_msgs::TransformStamped ts; ts.header.frame_id = "world"; ts.header.stamp = ros::Time::now(); ts.child_frame_id = "turtle1"; // 坐标系偏移量设置 ts.transform.translation.x = pose->x; ts.transform.translation.y = pose->y; ts.transform.translation.z = 0.0; // 二维实现，pose 中没有z，z 是 0 // 坐标系四元数：位姿信息中没有四元数，但是有个偏航角度，又已知乌龟是 2D ，没有翻滚与俯仰角度 // 所以可以得出乌龟的欧拉角：0 0 theta tf2::Quaternion qtn; qtn.setRPY(0, 0, pose->theta); ts.transform.rotation.x = qtn.getX(); ts.transform.rotation.y = qtn.getY(); ts.transform.rotation.z = qtn.getZ(); ts.transform.rotation.w = qtn.getW(); // 发布数据 pub.sendTransform(ts); } int main(int argc, char *argv[]) { setlocale(LC_ALL,""); ros::init(argc,argv,"dynamic_pub"); ros::NodeHandle nh; // 创建订阅对象，订阅 /turtle1/pose ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose", 100, doPose); // 回调函数 doPose 处理订阅的信息：将位姿信息转换成坐标相对关系并发布 ros::spin(); return 0; } -
订阅方：在功能包下的 src 创建 demo02_dynamic_sub.cpp 节点文件
#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, ""); ros::init(argc, argv, "dynamic_sub"); ros::NodeHandle nh; // 创建一个 buffer 缓存 tf2_ros::Buffer buffer; // 发布的每一个坐标关系都有时间戳，时间戳有变动并且进入缓存有延时 tf2_ros::TransformListener listener(buffer); // 创建监听对象（将订阅的数据存入 buffer） // 组织一个坐标点数据 geometry_msgs::PointStamped ps; ps.header.frame_id = "turtle1"; // 坐标点也有时间戳，转换时 ROS 会拿着坐标点的时间戳和坐标关系的时间戳进行比对 // 判断两个时间戳是否接近，如果接近就直接转换了，如果相差较大就抛异常不进行转换 // 直接把坐标点的时间戳不设置值，ROS 就不关心坐标关系时间戳多少了直接转换 ps.header.stamp = ros::Time(); ps.point.x = 1.0; ps.point.y = 1.0; ps.point.z = 0.0; // 转换算法，需要调用 TF 内置实现 //ros::Duration(2).sleep(); // 添加休眠 ros::Rate rate(10); while (ros::ok()) { try { // 将 ps 转换成相对于 base_link 的坐标点 geometry_msgs::PointStamped ps_out; /* 调用了 buffer 的转换函数 transform 参数1: 被转换的坐标点 参数2: 目标坐标系 返回值: 输出的坐标点 PS1：调用时必须包含头文件 tf2_geometry_msgs/tf2_geometry_msgs.h PS2：运行时存在的问题：抛出一个异常 base link 不存在 原因：订阅数据是一个耗时操作，可能在调用 transform 转换函数时，坐标系的相对关系还没有订阅到，因此出现异常 解决： 方案1: 在调用转换函数前，执行休眠 方案2: 进行异常处理（建议使用该方案） */ ps_out = buffer.transform(ps, "world"); // 最后输出转换后的坐标点 ROS_INFO("转换后的坐标:(%.2f,%.2f,%.2f), 参考的坐标系: %s", ps_out.point.x, ps_out.point.y, ps_out.point.z, ps_out.header.frame_id.c_str()); } catch(const std::exception& e) { std::cerr << e.what() << '\n'; } rate.sleep(); ros::spinOnce(); } return 0; } -
编辑配置文件 CMakeLists.txt
add_executable(demo01_dynamic_pub src/demo01_dynamic_pub.cpp) add_executable(demo02_dynamic_sub src/demo02_dynamic_sub.cpp) target_link_libraries(demo01_dynamic_pub ${catkin_LIBRARIES} ) target_link_libraries(demo02_dynamic_sub ${catkin_LIBRARIES} ) -
执行
$ roscore$ rosrun turtlesim turtlesim_node $ rosrun turtlesim turtlesim_teleop_key $ rviz # 可视化，可选$ cd demo04_ws $ source ./devel/setup.bash $ rosrun tf02_dynamic demo01_dynamic_pub $ rosrun tf02_dynamic demo02_dynamic_sub
-
1.4 多坐标变换
-
需求描述
现有坐标系统，父级坐标系统 world，下有两子级系统 son1，son2，son1 相对于 world，以及 son2 相对于 world 的关系是已知的，求 son1 原点在 son2 中的坐标，又已知在 son1 中一点的坐标，要求求出该点在 son2 中的坐标
-
实现分析
- 首先，需要发布 son1 相对于 world，以及 son2 相对于 world 的坐标消息
- 然后，需要订阅坐标发布消息，并取出订阅的消息，借助于 tf2 实现 son1 和 son2 的转换
- 最后，还要实现坐标点的转换
-
实现流程
-
新建功能包，添加依赖
$ cd demo04_ws $ code .- 在 src 下创建功能包 tf03_tfs，并添加依赖 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs
-
发布方：在 src 下新建 launch 文件夹，并新建 tfs_c.launch 文件
<launch> // 发布 son1 相对于 world 以及 son2 相对于 world 的坐标关系 <node pkg = "tf2_ros" type = "static_transform_publisher" name = "son1" args = "5 0 0 0 0 0 /world /son1" output = "screen" /> <node pkg = "tf2_ros" type = "static_transform_publisher" name = "son2" args = "3 0 0 0 0 0 /world /son2" output = "screen" /> </launch> -
订阅方：在功能包下的 src 创建 demo01_tfs.cpp 节点文件
#include "ros/ros.h" #include "tf2_ros/transform_listener.h" #include "tf2_ros/buffer.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.h" #include "geometry_msgs/TransformStamped.h" #include "geometry_msgs/PointStamped.h" // 订阅方实现: 1.计算sn1与son2的相对关系 // 2.计算son1的中某个坐标点在 son2 中的坐标值 int main(int argc, char *argv[]) { setlocale(LC_ALL, ""); ros::init(argc, argv, "tfs_sub"); ros::NodeHandle nh; // 创建订阅对象 tf2_ros::Buffer buffer; tf2_ros::TransformListener sub(buffer); // 创建坐标点 geometry_msgs::PointStamped psAtSon1; psAtSon1.header.stamp = ros::Time::now(); psAtSon1.header.frame_id = "son1"; psAtSon1.point.x = 1.0; psAtSon1.point.y = 2.0; psAtSon1.point.z = 3.0; ros::Rate rate(10); while (ros::ok()) { try { // 1.计算 son1 与 son2 的相对关系 geometry_msgs::TransformStamped son1ToSon2 = buffer.lookupTransform("son2", "son1", ros::Time(0)); ROS_INFO("son1 相对于 son2 的信息: 父级:%s, 子级:%s, 偏移量(%.2f, %.2f, %.2f)", son1ToSon2.header.frame_id.c_str(), // son2 son1ToSon2.child_frame_id.c_str(), // son1 son1ToSon2.transform.translation.x, son1ToSon2.transform.translation.y, son1ToSon2.transform.translation.z); // 2.计算 son1 的中某个坐标点在 son2 中的坐标值 geometry_msgs::PointStamped psAtSon2 = buffer.transform(psAtSon1, "son2"); ROS_INFO("坐标点在 son2 中的值(%.2f, %.2f, %.2f)", psAtSon2.point.x, psAtSon2.point.y, psAtSon2.point.z); } catch(const std::exception& e) { ROS_INFO("异常信息:%s", e.what()); } rate.sleep(); ros::spinOnce(); } return 0; } -
编辑配置文件 CMakeLists.txt
add_executable(demo01_tfs src/demo01_tfs.cpp) target_link_libraries(demo01_tfs ${catkin_LIBRARIES} ) -
执行
$ roscore$ roslaunch tf03_tfs tfs_c.launch $ rviz # 可视化，可选$ cd demo04_ws $ source ./devel/setup.bash $ rosrun tf03_tfs demo01_tfs
-
1.5 坐标系关系查看工具 tf2_tools
-
安装
$ sudo apt install ros-melodic-tf2-tools -
使用
$ rosrun tf2_tools view_frames.py查看当前目录会生成一个 frames.pdf 文件
1.6 TF 坐标变换实操（待更新…）
2. rosbag
- 背景
- 机器人传感器获取到的信息，有时可能需要实时处理，有时可能只是采集数据，然后事后分析
机器人导航实现中，可能需要绘制导航所需的全局地图，地图绘制实现，有两种方式
- 方式1：可以控制机器人运动，将机器人传感器感知到的数据实时处理，生成地图信息
- 方式2：同样是控制机器人运动，将机器人传感器感知到的数据留存，事后再重新读取数据，生成地图信息
- 两种方式比较，显然方式 2 使用上更为灵活方便
在 ROS 中关于数据的留存以及读取实现，提供了专门的工具：rosbag
-
概念
- 用于录制和回放 ROS 主题的一个工具集
-
作用
- 实现了数据的复用，方便调试与测试
-
本质
- rosbag 本质也是 ros 的节点，当录制时，rosbag 是一个订阅节点，可以订阅话题消息并将订阅到的数据写入磁盘文件；当回放时，rosbag 是一个发布节点，可以读取磁盘文件，发布文件中的话题消息
2.1 rosbag 使用：命令行
-
需求
- ROS 内置的乌龟案例并操作，操作过程中使用 rosbag 录制，录制结束后，实现重放
-
实现
-
准备：创建目录保存录制的文件
$ mkdir bags $ cd bags$ roscore $ rosrun turtlesim turtlesim_node $ rosrun turtlesim turtle_teleop_key -
开始录制
$ rosbag record -a -O hello.bag # -a 表示录制所有话题； -O 表示输出的文件名及路径操作小乌龟一段时间，结束录制使用 ctrl + c，在创建的目录中会生成 bag 文件
-
查看文件
$ rosbag info hello.bag -
回放文件
$ rosbag play hello.bag
-
2.2 rosbag使用：编码
-
写 bag
#include "ros/ros.h" #include "rosbag/bag.h" #include "std_msgs/String.h" int main(int argc, char *argv[]) { ros::init(argc,argv,"bag_write"); ros::NodeHandle nh; // 创建 bag 对象 rosbag::Bag bag; // 打开 bag.open("hello.bag", rosbag::BagMode::Write); // 写 std_msgs::String msg; msg.data = "hello world"; // 参数1：话题； 参数2：时间戳； 参数3：消息 bag.write("/chatter", ros::Time::now(), msg); bag.write("/chatter", ros::Time::now(), msg); bag.write("/chatter", ros::Time::now(), msg); bag.write("/chatter", ros::Time::now(), msg); //关闭 bag.close(); return 0; } -
读 bag
#include "ros/ros.h" #include "rosbag/bag.h" #include "rosbag/view.h" #include "std_msgs/String.h" #include "std_msgs/Int32.h" int main(int argc, char *argv[]) { setlocale(LC_ALL,""); ros::init(argc,argv,"bag_read"); ros::NodeHandle nh; // 创建 bag 对象 rosbag::Bag bag; // 打开 bag 文件 bag.open("hello.bag", rosbag::BagMode::Read); // 读数据 for (rosbag::MessageInstance const m : rosbag::View(bag)){ std::string topic = m.getTopic(); ros::Time time = m.getTime(); std_msgs::StringPtr p = m.instantiate<std_msgs::String>(); if (p != nullptr){ ROS_INFO("读取的数据:%s",p->data.c_str()); } } //关闭文件流 bag.close(); return 0; }
3. rqt 工具箱
在 ROS 中，提供了 rqt 工具箱，在调用工具时以图形化操作代替了命令操作，应用更便利，提高了操作效率，优化了用户体验
-
概念
- ROS 基于 QT 框架，针对机器人开发提供了一系列可视化的工具，这些工具的集合就是 rqt
-
作用
- 方便的实现 ROS 可视化调试，并且在同一窗口中打开多个部件，提高开发效率，优化用户体验
-
组成
- rqt：核心实现，开发人员无需关注
- rqt_common_plugins：rqt 中常用的工具套件
- rqt_robot_plugins：运行中和机器人交互的插件 (比如: rviz)
3.1 rqt 安装启动与基本使用
-
安装
$ sudo apt install ros-melodic-rqt $ sudo apt install ros-melodic-rqt-common-plugins -
启动
$ rqt # 方式1$ rosrun rqt_gui rqt_gui # 方式2 -
基本使用
- 启动 rqt 之后，可以通过 plugins 添加所需的插件
- 启动 rqt 之后，可以通过 plugins 添加所需的插件
3.2 rqt 常用插件 rqt_graph
- 简介：可视化显示计算图
- 启动：可以在 rqt 的 plugins 中添加，或者使用rqt_graph启动
3.3 rqt常用插件 rqt_console
- 简介：rqt_console 是 ROS 中用于显示和过滤日志的图形化插件
- 准备：编写 Node 节点输出各个级别的日志信息
// ROS 节点：输出各种级别的日志信息 #include "ros/ros.h" int main(int argc, char *argv[]) { ros::init(argc,argv,"log_demo"); ros::NodeHandle nh; ros::Rate r(0.3); while (ros::ok()) { ROS_DEBUG("Debug message d"); ROS_INFO("Info message oooooooooooooo"); ROS_WARN("Warn message wwwww"); ROS_ERROR("Erroe message EEEEEEEEEEEEEEEEEEEE"); ROS_FATAL("Fatal message FFFFFFFFFFFFFFFFFFFFFFFFFFFFF"); r.sleep(); } return 0; } - 启动：可以在 rqt 的 plugins 中添加，或者使用 rqt_console 启动
3.4 rqt 常用插件：rqt_plot
-
简介：图形绘制插件，可以以 2D 绘图的方式绘制发布在 topic 上的数据
-
准备：启动 turtlesim 乌龟节点与键盘控制节点，通过 rqt_plot 获取乌龟位姿
-
启动：可以在 rqt 的 plugins 中添加，或者使用 rqt_plot 启动
3.5 rqt 常用插件：rqt_bag
-
简介：录制和重放 bag 文件的图形化插件
-
准备：启动 turtlesim 乌龟节点与键盘控制节点
-
启动：可以在 rqt 的 plugins 中添加，或者使用 rqt_bag 启动
-
录制
-
回放
![[特征提取与匹配]基于Open CV使用SIFT提取特征，并使用FLANN完成单应性匹配](https://img-blog.csdnimg.cn/c22d5bbfaa7f475e9441f92f8420aa72.png)
