机器人操作系统（ROS）课件6.ROS常用工具

机器人操作系统（ROS）机器人操作系统（ROS）ROS常用工具5机器人操作系统（ROS）5.ROS常用工具ROS为机器人开发提供大量实用的组件工具。本章我们来学习ROS中的几种常用工具，包括Qt工具箱、rviz可视化工具、gazebo仿真环境、rosbag数据记录与回放和TF工具。Qt工具箱：提供多种机器人开发的可视化工具，如日志输出、计算图可视化、数据绘图等功能。rviz三维可视化平台：实现机器人开发过程中多种数据的可视化显示。gazebo仿真环境：创建仿真环境并实现带有物理属性的机器人仿真。rosbag数据记录与回放：记录并回放ROS系统中运行时的所有话题信息，方便后期调试使用。TF工具：管理机器人系统中坐标系变换关系。机器人操作系统（ROS）6.1Qt工具箱Qt工具箱提供多种机器人开发的可视化工具，如日志输出、计算图可视化、数据绘图、参数动态配置等功能。ROS提供了一个Qt架构的后台图形工具套件——rqt_common_plugins，其中包含不少实用的工具，可通过以下命令进行安装：$sudoapt-getinstallros-melodic-rqt$sudoapt-getinstallros-melodic-rqt-common-plugins机器人操作系统（ROS）6.1.1日志输出工具（rqt_console）日志输出工具用于输出日志内容（Message）、日志级别（Severity）、节点（Node）、时间戳（Stamp）、话题（Topics）、位置（Location）等信息。使用以下命令即可启动该工具：$roscore$rqt_console启动成功后可以看到如图6-1所示的可视化界面。当系统中有不同级别的日志消息时，rqt_console的界面中就会依次显示这些日志的相关内容，包括日志内容、时间戳、级别等。当日志较多时，也可以使用该工具进行过滤显示。日志输出工具界面机器人操作系统（ROS）6.1.2计算图可视化工具（rqt_graph）计算图是ROS处理数据的一种点对点的网络形式。程序运行时，所有进程及它们所进行的数据处理，将会通过一种点对点的网络形式表现出来，即通过节点、节点管理器、话题、服务等来进行表现。在系统运行时，使用如下命令即可启动该工具：$rqt_graph启动成功后的计算图显示如图所示。rqt_graph工具界面机器人操作系统（ROS）6.1.3数据绘图工具（rqt_plot）rqt_plot是一个二维数值曲线绘制工具，可以将需要显示的数据在xy坐标系中使用曲线描绘，用于观察变量随时间的变化趋势曲线。使用如下命令即可启动该工具：$rqt_plot然后在界面上方的Topic输入框中输入需要显示的话题消息，如果不确定话题名称，可以在终端中使用“rostopiclist”命令查看。例如在乌龟例程中，通过rqt_plot工具描绘乌龟x、y坐标变化的效果如图6-3所示。rqt_plot工具机器人操作系统（ROS）6.1.4参数动态配置工具（rqt_reconfigure）rqt_reconfigure工具可以在不重启系统的情况下，动态配置ROS系统中的参数，但是该功能的使用需要在代码中设置参数的相关属性，从而支持动态配置。使用如下命令即可启动该工具：$rosrunrqt_reconfigurerqt_reconfigure启动后的界面将显示当前系统中所有可动态配置的参数如图6-4所示，在界面中使用输入框、滑动条或下拉框进行设置即可实现参数的动态配置。rqt_reconfigure工具界面机器人操作系统（ROS）6.2rviz三维可视化平台ROS针对机器人系统的可视化需求，为用户提供了一款显示多种数据的三维可视化平台rviz。rviz是ROS中一款三维可视化平台，一方面能够实现对外部信息的图形化显示，另外还可以通过rviz发布控制信息，从而实现对机器人的监测与控制。rviz可以帮助开发者实现所有可监测信息的图形化显示，开发者也可以在rviz的控制界面下，通过按钮、滑动条、数值等方式控制机器人的行为。机器人操作系统（ROS）6.2.1运行rvizrviz已经集成在桌面完整版的ROS中，如果已经成功安装桌面完整版的ROS，可以直接跳过这一步骤，否则，请使用如下命令进行安装：$sudoapt-getinstallros-melodic-rviz安装完成后，在终端中分别运行如下命令即可启动ROS和rviz平台：$roscore$rosrunrvizrviz启动成功的rviz主界面如图所示。rviz的主界面机器人操作系统（ROS）6.2.1运行rvizDisplays面板位于rviz的左侧，包含rviz的显示插件列表及其属性，可以通过点击左侧的Add按钮来添加所需的插件，也可以编写自己的显示插件并将其添加到面板上。3D视图显示区位于rviz的中间部分，用于可视化不同类型的ROS消息，例如激光雷达3D点云、地图信息、里程计数据、机器人模型、tf等。工具栏位于rviz的顶部，这些工具可以用来与机器人模型交互、调整相机视图、设置导航目标以及设置机器人2D位姿估计等，同时还可以在工具栏中以插件的形式添加自定义的工具。Views显示面板位于rviz的右侧，可以保存3D视角的不同视图，也可以通过加载已保存的配置来切换到相应的视图模式下。Time时间显示面板位于rviz的底部，展示了仿真花费的时间。可以通过点击面板上的Reset按钮重置rviz的初始设置。机器人操作系统（ROS）6.2.2数据可视化为了解释数据可视化，我们将用到turtlesim的示例，运行程序：$roslaunchturtle_tfturtle_tf_demo.launch这是tf坐标变换的示例程序，通过键盘控制小海龟1，小海龟2会跟随其运动,效果如图所示。接下来我们可以通过rviz工具查看其运动的变化。机器人操作系统（ROS）6.2.2数据可视化首先，在Display面板里的全局选项，更改FixedFrame为/world。然后将TF树添加到Display面板，点击rviz界面左侧下方的“Add”按钮，rviz会将默认支持的所有数据类型的显示插件罗列出来，如图所示。机器人操作系统（ROS）6.2.2数据可视化在上一张图所示的列表中选择需要的TF，然后在“DisplayName”文本框中填入一个唯一的名称tf_turtle，用来识别显示的数据。添加完成后，rviz左侧的Dispalys中会列出已经添加的显示插件；点击插件列表前的加号，可以打开一个属性列表，根据需求设置属性。如果订阅成功，在中间的显示区应该会出现可视化后的数据，结果如图所示。机器人操作系统（ROS）6.3Gazebo仿真环境Gazebo是一款3D动态模拟器，能够在复杂的室内和室外环境中准确有效地模拟机器人。与游戏引擎提供高保真度的视觉模拟类似，Gazebo提供高保真度的物理模拟，其提供一整套传感器模型，以及对用户和程序非常友好的交互方式。虽然Gazebo中的机器人模型与rviz使用的模型相同，但是需要在模型中加入机器人和周围环境的物理属性，例如质量、摩擦系数、弹性系数等。机器人的传感器信息也可以通过插件的形式加入仿真环境，以可视化的方式进行显示。机器人操作系统（ROS）6.3.1安装并运行Gazebo与rviz一样，如果已经安装了桌面完整版的ROS，那么可以直接跳过这一步，否则，可以使用以下命令进行安装：$sudoapt-getinstallros-melodic-gazebo-ros-pkgsros-melodic-gazebo-ros-control安装完成后，在终端中使用如下命令启动ROS和Gazebo：$roscore$rosrungazebo_rosgazeboGazebo启动成功后的界面如图所示。机器人操作系统（ROS）6.3.1安装并运行Gazebo主界面中主要包含3D视图区、工具栏、模型列表、模型属性项和时间显示区。验证Gazebo是否与ROS系统成功连接，可以查看ROS的话题列表：$rostopiclist如果连接成功，应该可以看到Gazebo发布/订阅的话题，如图所示。机器人操作系统（ROS）6.3.1安装并运行Gazebo当然，还有Gazebo提供的服务列表，如图所示，使用以下命令查看：$rosservicelist机器人操作系统（ROS）6.3.2构建仿真环境Gazebo中有两种创建仿真环境的方法，包括直接插入模型和BuildingEditor。（1）直接插入模型首先需要下载相关模型库到本地路径~/.gazebo下，模型下载地址：/osrf/gazebo_models，使用如下命令：$cd~/.gazebo$gitclone/osrf/gazebo_models.git在Gazebo左侧的模型列表中，有一个insert选项罗列了所有可使用的模型。选择需要使用的模型，放置在主显示区中，如图所示，就可以在仿真环境中添加机器人和外部物体等仿真实例。机器人操作系统（ROS）6.3.2构建仿真环境（2）BuildingEditor第二种方法是使用Gazebo提供的BuildingEditor工具手动绘制地图。在Gazebo菜单栏中选择Edit→BuildingEditor，可以打开如图6-13所示的BuildingEditor界面。选择左侧的绘制选项，然后在上侧窗口中使用鼠标绘制，下侧窗口中即可实时显示绘制的仿真环境。机器人操作系统（ROS）6.4rosbag数据记录与回放ROS能够存储所有节点通过话题发布的消息。ROS提供了数据记录与回放的功能包rosbag，包含消息所有字段参数和时间戳，可以帮助开发者收集ROS系统运行时的消息数据，然后在离线状态下进行回放和处理。本节将通过小海龟例程介绍rosbag数据记录和回放的实现方法。机器人操作系统（ROS）6.4.1使用rosbag首先启动键盘控制小海例程所需的所有节点：$roscore$rosrunturtlesimturtlesim_node$rosrunturtlesimturtle_teleop_key启动成功后，可以看到可视化界面中的小乌龟了，此时可以在终端中通过键盘控制小乌龟移动。然后我们使用以下命令查看在当前ROS系统中存在的话题列表，结果如图6-14所示：$rostopiclist-v机器人操作系统（ROS）6.4.1使用rosbag接下来使用rosbag抓取这些话题的消息，并且打包成一个文件放置到指定文件夹中：$mkdir~/rosbagfiles$cd~/rosbagfiles$rosbagrecord-a其中，-a(all)参数表示记录所有发布的消息。此时消息记录已经开始，我们可以在终端中控制小乌龟移动一段时间，然后进入刚才创建的文件夹~/bagfiles中，应该会有一个以时间命名并且以.bag为后缀的文件。机器人操作系统（ROS）6.4.2回放数据我们已经有一个消息记录的文件包，之后我们就可以用它回放数据。关闭之前打开的turtle_teleop_key键盘控制节点并重启turtlesim_node，使用如下命令回放所记录的话题数据：$rosbagplay<yourbagfile>在短暂的等待时间后，数据开始回放，小乌龟的运动轨迹应该与之前数据记录过程中的状态完全相同，在终端中也可以看到如图所示的回放时间信息，运行结果小海龟运动轨迹对比如图所示。回放数据小海龟轨迹对比图机器人操作系统（ROS）6.4.2回放数据rosbag功能包还提供了info命令，可以查看数据记录文件的详细信息，命令的使用格式如下：$rosbaginfo<yourbagfile>使用info命令来查看之前生成的数据记录文件，可以看到类似图所示的信息。查看数据记录文件的相关信息从以上信息中我们可以看到，数据记录包中包含的所有话题、消息类型、消息数量等信息。机器人操作系统（ROS）6.5TF工具坐标变换作为机器人学的基础，也是机器人运动控制不可或缺的部分，掌握机器人坐标变换十分必要。在机器人开发中，无论是相机坐标与机器人坐标之间还是世界坐标与机器人坐标间，都需要用坐标变换统一坐标系。ROS为开发者提供了TF库，可以很方便地实现系统中任一个点在所有坐标系之间的坐标变换。本节首先介绍机器人运动学相关基础知识，再介绍TF功能包相关知识及其使用方法。机器人操作系统（ROS）6.5.1机器人运动学基本原理（1）空间坐标系的描述坐标变换是机器人运动学中的基础概念。机器人作为一个完整的系统，结构上往往由大量组件元素组成，例如机器人的主体部分、连杆、活动关节等。在进行机器人设计和应用的过程中涉及各个组件位姿的变化，机器人的构型通常根据他们的坐标系来确定，这就引入了空间坐标系的概念。在机器人运动学中，使用矩阵来表示坐标系和坐标系中的物体。我们用x、y和z轴表示固定的全局参考坐标系F_(x,y,z),用n、o和a轴表示相对于参考坐标系的另一个运动坐标系F_(n,o,a)。字母n、o和a取自于单词normal、orientation和approach的首字母。参考图6-18,很显然为了避免在抓取物体时发生碰撞，机器人必须沿着抓手的z轴方向来接近该物体。用机器人的术语，这个轴称为接近(approach)轴，简称为a轴。在抓手坐标系中接近物体的方向称为方向(orientation)轴，简称为o轴。又因为x轴垂直(normal)于上述两轴，所以简称为n轴。因此，在本节中均称由normal、orientation和approach轴组成的运动坐标系为F_(n,o,a)。机器人操作系统（ROS）6.5.1机器人运动学基本原理（2）空间坐标系的转换空间坐标系的转换可以分为三类：纯平移、绕一个轴纯旋转、平移和旋转相结合。①纯平移如果坐标系在空间以不变的姿态运动，那么该变换就是纯平移变换。在这种情况下，他的方向单位向量保持统一方向不变。所有的改变只是坐标系原点相对于参考坐标系的变化。机器人操作系统（ROS）6.5.1机器人运动学基本原理

机器人操作系统（ROS）6.5.1机器人运动学基本原理

机器人操作系统（ROS）6.5.1机器人运动学基本原理

机器人操作系统（ROS）6.5.1机器人运动学基本原理

机器人操作系统（ROS）6.5.1机器人运动学基本原理

机器人操作系统（ROS）6.5.2TF功能包的原理分析（/tf#tf_echo）TF是一个让用户随时间跟踪多个坐标系的功能包，它维护多个坐标系之间的坐标关系，可以帮助开发者在任意时间，在坐标系间完成点、向量等坐标的变换。机器人操作系统（ROS）6.5.2TF功能包的原理分析（/tf#tf_echo）如图6-22所示，一个机器人系统通常由很多三维坐标系，而且会随时间的推移发生变化，如世界坐标系（WorldFrame）、基坐标系（BaseFrame）、机械夹爪坐标系（GripperFrame）、机器人头部坐标系（HeadFrame）等。TF以时间为轴跟踪这些坐标系，并且允许开发者请求如下类型的数据：(1)机器人夹取的物体相对于机器人头部的位置在哪里？(2)机器人头部坐标系相对于中心坐标系的关系是什么样的？(3)机器人中心坐标系相对于激光雷达传感器的位置在哪里？TF可以在分布式系统中进行操作，也就是说，一个机器人系统中所有的坐标变换关系，对于所有的节点组件都是可用的，所有订阅TF消息的节点都会缓冲一份所有坐标系的变换关系数据，所以这种结构不需要中心服务器来存储任何数据。想要使用TF功能包，总体来说需要以下两个步骤。(1)监听TF变换：接收并缓存系统中发布的所有坐标变换数据，并从中查询所需要的坐标变换关系。(2)广播TF变换：向系统中广播坐标系之间的坐标变换关系。系统中可能会存在多个不同部分的TF变换广播，每个广播都可以直接将坐标变换关系插入TF树中，不需要再进行同步。机器人操作系统（ROS）6.5.3TF工具TF提供了丰富的终端工具来帮助开发者调试和创建TF变换。如果已经安装了桌面完整版的ROS，那么可以不用单独安装TF工具，否则，可以使用以下命令进行安装：$sudoapt-getinstallros-melodic-tf(1) tf_monitor:查看指定坐标系之间的发布状态，命令格式如下：$rosruntftf_monitor<source_frame><target_target>(2) tf_echo:查看指定坐标系之间的变换关系，命令格式如下：$rosruntftf_echo<source_frame><target_frame>机器人操作系统（ROS）6.5.3TF工具(3) view_frame:可视化调试工具，生成pdf文件，显示整个TF数，命令格式如下：$rosruntfview_frames(4) static_transform_publisher:发布两个坐标系之间的静态坐标变换，这两个坐标系不发生相对位置变化，命令格式如下：$rosruntfstatic_transform_publisherxyzyawpitchrollframe_idchild_frame_idperiod_in_ms$rosruntfstatic_transform_publisherxyzqxqyqzqwframe_idchild_frame_idperiod_in_ms机器人操作系统（ROS）6.5.4机器人的ROStf应用实例本节将通过一个乌龟例程来简要介绍ROS中TF的作用，学习常用的TF工具。首先使用如下命令安装Ubuntu发布的turtle_tf功能包。$sudoapt-getinstallros-melodic-turtle-tf机器人操作系统（ROS）6.5.4机器人的ROStf应用实例现在我们已经完成了turtle_tf功能包的获取，接下来就可以使用如下命令运行例程了：$roslaunchturtle_tfturtle_tf_demo.launch机器人操作系统（ROS）6.5.4机器人的ROStf应用实例仿真器中将出现小乌龟，说明turtle_tf功能包的启动文件已经成功打开，并且处于下方的小乌龟会自动向中心位置的小乌龟靠拢。打开一个新的终端，打开键盘控制节点，从而可以使用键盘上的上下左右键控制中心位置的小乌龟运动。$rosrunturtlesimturtle_teleop_key上述代码运行了turtlesim功能包下的turtle_teleop_key键盘控制节点，通过键盘控制可以发现另一只乌龟总是跟随我们控制的那只乌龟运行。机器人操作系统（ROS）6.5.4机器人的ROStf应用实例通过TF工具view_frames可以查看当前例程中的TF树。$rosruntfview_frames机器人操作系统（ROS）6.5.4机器人的ROStf应用实例在这里我们可以看到tf广播的三个坐标系：世界坐标系、海龟1坐标系和海龟2坐标系。我们还可以看到world是turtle1和turtle2坐标系的父级。出于调试目的，view_frames还会报告一些有关何时接收到最早和最近的坐标转换以及tf坐标发布到tf.frame的速度诊断信息。通过tf_echo工具查看两个乌龟坐标系的变换关系，命令如下：$rosruntftf_echoturtle1turtle2机器人操作系统（ROS）6.5.4机器人的ROStf应用实例其中打印结果中Translation表示位置信息，Rotation表示角度信息。当控制海龟四处走动时，就可以看到随着两只海龟相对移动而发生的坐标变化。通过rviz图形界面可以更加形象地看到这三者之间的坐标关系。$rosrunrvizrviz-d`rospackfindturtle_tf`/rviz/turtle_rviz.rviz机器人操作系统（ROS）6.5.4机器人的ROStf应用实例

机器人操作系统（ROS）6.5.4机器人的ROStf应用实例（1）TF的广播TF能在ROS中建立坐标系，并且能够简化各个坐标系之间的转换关系。可以使用C++或者Python来编写tf广播器。我们需要创建一个发布乌龟坐标系与世界坐标系之间TF变换的节点，实现源码src/turtle_tf_broadcaster.cpp的具体内容如下：#include<ros/ros.h>#include<tf/transform_broadcaster.h>#include<turtlesim/Pose.h>std::stringturtle_name;voidposeCallback(constturtlesim::PoseConstPtr&msg){//tf广播器statictf::TransformBroadcasterbr;//根据乌龟当前的位姿，设置相对于世界坐标系的坐标变换tf::Transformtransform;transform.setOrigin(tf::Vector3(msg->x,msg->y,0.0));tf::Quaternionq;q.setRPY(0,0,msg->theta);transform.setRotation(q);//发布坐标变换br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"world",turtle_name));}intmain(intargc,char**argv){//初始化节点ros::init(argc,argv,"my_tf_broadcaster");if(argc!=2){ROS_ERROR("needturtlenameasargument");return-1;};turtle_name=argv[1];//订阅乌龟的pose信息ros::NodeHandlenode;ros::Subscribersub=node.subscribe(turtle_name+"/pose",10,&poseCallback);ros::spin();return0;};机器人操作系统（ROS）6.5.4机器人的ROStf应用实例下面简单剖析以上代码的实现过程。①头文件部分#include<ros/ros.h>#include<tf/transform_broadcaster.h>#include<turtlesim/Pose.h>为了避免包含繁杂的ROS功能包头文件，ros/ros.h已经帮我们包含了大部分ROS中通用的头文件。要使用TransformBroadcaster，我们需要包含tf/transform_broadcaster.h头文件。机器人操作系统（ROS）6.5.4机器人的ROStf应用实例②poseCallback函数部分statictf::TransformBroadcasterbr;tf::Transformtransform;定义一个广播，相当于发布话题时定义一个发布器。定义存放转换信息（平移，旋转）的变量。transform.setOrigin(tf::Vector3(msg->x,msg->y,0.0));setOrigin()函数设置坐标原点，需要注意该函数的类型需要为tf::Vector3类型，假设是要发布一个子坐标系为“turtle1”父坐标系为“world”，那么其中(msg->x,msg->y,0.0)是指“turtle1”的坐标原点在“world”坐标系下的坐标。tf::Quaternionq;q.setRPY(0,0,msg->theta);transform.setRotation(q);此部分定义了旋转，需要注意setRPY()函数的参数为”turtle1”在“world”坐标系下的roll(绕X轴)，pitch(绕Y轴)，yaw(绕Z轴)。br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"world",turtle_name));此部分中包含四个参数，transform是存储变换关系的变量；ros::Time::now()是广播tf时的时间戳；“world”是父坐标系的名字；turtle_name是子坐标系的名字。机器人操作系统（ROS）6.5.4机器人的ROStf应用实例（2）TF的监听TF消息广播之后，其他节点就可以监听该TF消息，从而获取需要的坐标变换了。目前我们已经将乌龟相对于world坐标系的TF变换进行了广播，接下来需要监听TF消息，并从中获取turtle2相对于turtle1坐标系的变换，从而控制turtle2移动。简单的来说，通过监听tf，我们可以避免繁琐的旋转矩阵的计算，而直接获取我们需要的相关信息。实现源码src/turtle_tf_listener.cpp的详细内容如下：#include<ros/ros.h>#include<tf/transform_listener.h>#include<geometry_msgs/Twist.h>#include<turtlesim/Spawn.h>intmain(intargc,char**argv){//初始化节点ros::init(argc,argv,"my_tf_listener");ros::NodeHandlenode;//通过服务调用，产生第二只乌龟turtle2ros::service::waitForService("spawn");ros::ServiceClientadd_turtle=node.serviceClient<turtlesim::Spawn>("spawn");turtlesim::Spawnsrv;add_turtle.call(srv);//定义turtle2的速度控制发布器ros::Publisherturtle_vel=node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel",10);//tf监听器tf::TransformListenerlistener;ros::Raterate(10.0);while(node.ok()){tf::StampedTransformtransform;try{//查找turtle2与turtle1的坐标变换listener.waitForTransform("/turtle2","/turtle1",ros::Time(0),ros::Duration(3.0));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需要运动的线速度和角速度//并发布速度控制指令，使turtle2向turtle1移动geometry_msgs::Twistvel_msg;vel_msg.angular.z=4.0*atan2(transform.getOrigin().y(),transform.getOrigin().x());vel_msg.linear.x=0.5*sqrt(pow(transform.getOrigin().x(),2)+pow(transform.getOrigin().y(),2));turtle_vel.publish(vel_msg);rate.sleep();}return0;};机器人操作系统（ROS）6.5.4机器人的ROStf应用实例①初始化部分tf::TransformListenerlistener;ros::Raterate(10.0);创建一个tf::TransformListener类型的监听器，创建成功后监听器会自动接收TF树的消息，并且缓存10秒。②循环部分tf::StampedTransformtransform;定义存放变换关系的变量。listener.waitForTransform("/turtle2","/turtle1",ros::Time(0),ros::Duration(3.0));listener.lookupTransform("/turtle2","/turtle1",ros::Time(0),transform);监听中两个常用的函数transformPoint（）和lookupTransform()，前者可以获得两个坐标系之间转换的关系，包括旋转与平移，后者可以实现从“/turtle2”到“/turtle1”的转换。catch(tf::TransformException&ex){ROS_ERROR("%s",ex.what());ros::Duration(1.0).sleep();continue;}监听两个坐标系之间的变换。由于tf会把监听的内容存放到一个缓存中，然后再读取相关的内