我使用的是ubuntu20.04下的ROS Noetic版本，是ROS 1 的最后一个长期支持（LTS）版本，将于2025年5月停止维护

一，Linux系统基本操作

ctrl+alt+t快速打开终端

1，pwd命令

查看当前终端所在路径

使用方式：直接在终端输入pwd回车即可

2，cd命令

切换路径

使用方式：cd+要切换到的路径(例如要切换到home路径下，则：cd /home/，然后回车即可)。也可以通过cd yf进入到home的下一级目录中，还可以通过cd ..可以切换到上一级目录

3，mkdir命令

mkdir可以在当前目录下创建文件夹 ，比如在进入home下的yf目录后，在终端输入mkdir test回车即可在yf文件夹内创建出名为test的文件夹

4，ls命令

查看当前路径下的所有文件夹，如下图所示

5，touch命令

 在此路径的文件夹内创建一个文件，类似于windows的txt文件

使用方法：touch test_file回车即可在当前目录文件夹下创建名为test_file文本文件

6，mv命令

将文件从原路径转移到另一个指定路径(原路径文件会被剪切)

使用方法：mv test_file /home/yf.即将test_file文件转移到/home/yf目录下

7，cp命令

将文件从原路径拷贝到另一个指定路径(原路径文件会保存)

使用方法：cp test_file /home/yf/.即将test_file文件转移到/home/yf目录下不进行重新命名 

cp test_file /home/yf/test_file2.即将test_file文件转移到/home/yf目录下且重新命名为test_file2

8，rm命令

删除文件

使用方法：rm test_file 删除test_file文件。

rm -r test_folder/ 删除test_folder文件夹

9，sudo命令

用来提升当前用户权限。用于安装或更新软件

10，** --help

终端命令帮助

比如rm --help.sudo --help. cd --help等

二，运行C++/Python代码

在终端中运行C++/Python代码

1，C++代码在终端运行

g++ 文件名 -o 输出的可执行文件名：此步用于编译代码文件得到可执行文件(终端输入形式如下图1)，如下图2，C++_for就是C++for.cpp编译得到的可执行文件.得到可执行文件后再用./c++_for来执行文件获得代码结果

2，Python在终端运行

python .py文件 ：如下图所示即可运行py文件得到代码结果

三，认识ROS

 ROS，即Robot Operating System机器人操作系统，主要分为通信机制，开发工具，应用功能和生态系统四大模部分。目标是提高机器人开发中的软件复用率，可以把别人开发好的代码功能进行复用，在此基础上再做进一步的扩展和完善，加速机器人开发

1，通信机制

通信机制上ROS提供了非常重要的松耦合分布式通信框架。整个ROS里的所有功能框架可以抽象成一个节点图，如下图。里面一系列的椭圆图，每一个椭圆代表一个节点，比如说图像采集，图像处理，图像驱动，SLAM导航等相关算法，每一个具体的功能都是一个节点，节点之间通过箭头做连接，箭头就代表节点之间的通信数据的流向，方框代表进程。所有的功能抽象出来就是下图所示的计算图。后续会介绍各个节点之间的通讯如何通过代码来实现

松耦合分布式通信是一种分布式系统中组件之间交互的方式，系统中的各个组件（如节点、进程等）之间的依赖关系较弱。每个组件都相对独立地运行，它们之间通过定义良好的接口和通信协议进行交互，但并不紧密依赖于其他组件的内部实现细节。例如，图中的各个节点（如 /gazebo、/twist_marker_server 等）虽然通过 /clock、/rosout 等主题进行通信，但它们各自的功能相对独立，一个节点的故障或更新不会直接导致其他节点无法正常工作。由于组件之间的低耦合性，当系统需要扩展功能或处理能力时，可以相对容易地添加新的组件或节点，而不需要对现有组件进行大规模的修改。

2，开发工具

 为了提高机器人开发效率，ROS提供了很多工具。包括命令行工具，通过终端输入一系列命令来完成一系列功能快速调试机器人，看到机器人的一些数据；还有可视化工具，比如Rviz看到机器人建图效果；还有利用Gazebo建造机器人的仿真环境

3，应用功能

ROS当中的应用功能非常丰富，在ROS社区里可以找到许多功能包。比如下图的导航Navigation，SLAM建图，Movelt完成机械臂运动规划。使用ROS功能包时，最重要的是关心它的输入和输出，其次去关心它里面的核心算法

只要想去做机器人的相关事情，都会在谷歌或者github里面搜ROS相关的关键词，可能都会找到与要做的事情功能相似甚至一样的功能包。

4，生态系统

软件源里面放了ROS该版本下的所有编译好的安装文件，后面运行的很多功能包直接是通过apt-get install来安装的，就像用手机安装一个app一样

ROS wiki是记录ROS相关信息最完整的资料论坛；但是它的整理上面会比较零散，所以找某个资料会比较麻烦，最好还是用谷歌直接搜

ROS Answers像知乎一样，提问问题会有很多人来回答

ROS和ROS Wiki里有大量的功能包和算法的说明。ROS Robots里有ROS官方支持的一些机器人列表。 

四，ROS核心概念

ROS里的通信机制是一个松耦合分布式软件框架。下图中的Node就是节点，每个节点是在机器人系统当中完成一个具体功能的一个进程，每个节点之间会有一系列数据的传输。所有节点之间的位置也不是固定的，比如有些节点可以在ComputerA当中运行，有些节点可以在ComputerB当中运行，节点之间通过一系列数据的传输方式来完成通讯，每个节点的编程语言也不是固定的。我们可以分布式的完成各个节点的开发，最终只要有统一的通讯标准就可以完成整个系统的搭建了。

1，节点与节点管理器

节点--执行单元 

• 执行具体任务的进程，独立运行的可执行文件(类似于Windows的.exe可执行文件)。一个节点内可实现的内容可多可少
• 不同节点可使用不同的编程语言，可分布式运行在不同的主机
• 节点在系统中的名称是唯一的

节点管理器（ROS Master）--控制中心

• 为节点提供命名和注册服务
• 跟踪和记录话题/服务通信。辅助节点相互查找，建立连接
• 提供参数服务器，节点使用此服务区存储和检索运行时的参数

例如下图，节点先找ROSMaster来注册，告诉ROSMaster它要干嘛，然后ROSMaster就会记下每个节点的状态。摄像头采集的信息处理为CameraNode，它采集的信息要交给ImageProcessingNode处理，这两者之间的联系就由ROSMaster来建立。图像处理节点处理完数据后就会传到Laptop的图像显示节点，以此来让操作者看到处理情况

2，话题通信

节点之间要通信就会涉及到通信方式，ROS为节点之间的通信设计了两种核心通讯方式，一种叫做话题，一种叫做服务

话题(Topic)--异步通信机制

发布/订阅模型

• 节点间用来传输数据的重要总线
• 使用发布/订阅模型，数据由发布者传输到订阅者，同一个话题的订阅者和发布者可以不唯一

话题通讯模型比较简单，如下图，里面会分为发布者(Publisher)和订阅者(Subscriber)。左端为发布者，发布者发布数据，右端订阅者订阅数据。话题这种通讯方式是单向数据的传输。针对这个传输通道定义一个名字就叫做话题topic。这个通道中传输的数据都是有自己的数据结构定义的，这个数据结构定义我们称之为消息

消息(Message)--话题数据 

消息用来描述我们传输话题数据里面话题的数据类型的，比如说图像是由RGB组成的有多少个像素，每个像素里的RGB是几位的等，这个详细的定义是消息，图像数据管道叫做是话题

• 具有一定的类型和数据结构，包括ROS提供的标准类型和用户自定义类型
• 使用编程语言无关的.msg文件定义，编译过程中生成对应的代码文件

消息里的很多内容在ROS中已经预定义好了结构，比如图像，雷达等等，我们也可以自己去定义这样的接口，使用.msg来定义

在下图中可以看到具体的含义。Camera Node作为驱动节点驱动摄像头后，数据会由此驱动节点获取到，然后此驱动节点作为发布者将数据发布出去，发出的管道在下图叫做/image_data(话题)，image_data这个管道中传输的数据内容称为消息Message，由Image Processing Node和Image Display Node订阅。

3，服务通信 

话题通信是一种异步通信，没有办法知道发布者和订阅者单向传输的时效性，发布出去订阅者可能很久才接收到。很多情况下，我们希望数据发送出去后对方可以给是否接收到的回复

服务(Service)--同步通信机制

请求/应答模型

• 使用客户端/服务器(C/S)模型，客户端发送请求数据，服务器完成处理后返回应答数据
• 使用编程语言无关的.srv文件定义请求和问答数据结构，编译过程中生成对应的代码文件

话题和服务的区别 

4，参数

参数在ROS Master里面有维护一个参数服务器，它会保存一部分的参数作为全局共享字典，所有节点都可以通过网络来访问这些共享字典，共享字典里的数据类型是可以多变的(可以有int float string等等)。他里面底层的通讯机制是RPC，并不是话题和服务。

参数(Parameter)--全局共享字典 

• 可通过网络访问的共享，多变量字典
• 节点使用此服务器来存储和检索运行时的参数
• 适合存储静态，非二进制的配置参数，不适合存储动态配置的数据。存储一些不会频繁改变的参数

5，文件系统

功能包 

ROS软件中的基本单元，包含节点源码，配置文件，数据定义等

功能包清单

记录功能中的基本信息，包含作者信息，许可信息，依赖选项，编译标志等

元功能包

组织多个用于同一目的功能包

五，ROS命令行工具的使用 

下表列出了主要命令行工具的使用方式

1，小海龟案例

（1）roscore

roscore用来启动ROS Master。运行ROS系统时必须首先运行roscore

（2）rosrun

rosrun是用来运行某个功能包里面的某个节点的指令，后面跟两个参数，一个是功能包名(turtlesim是ROS安装好之后学习用的功能包)， 另一个是节点名。可以双tab键查看功能包里的工具(turtlesim_node是仿真器节点)(turtle_teleop_key是键盘控制节点)

（3）rqt_graph

ROS里有一系列以rqt开头的工具，都是一些基于qt的可视化工具，可以很直观的看到很多信息

rqt_graph是用来显示系统计算图的工具，通过此图可以很快了解整个系统的全貌。如下图

此图清晰的列出了ROS当中的几个节点，两个节点之间存在一个话题/turtle1/cmd_vel做通讯，话题里的具体数据内容就是键盘输入

（4）rosnode

rosnode用来显示系统当中所有节点信息的指令，直接在终端输入rosnode回车，可以看到指令提示，后面可以接ping list info等等。

1，rosnode list

可以显示系统中所有的节点。rosout是在ROS环境里默认只要启动roscore就会启动的一个话题，这个话题主要采集ROS中所有节点的日志信息，用来提交给上面的界面做显示的

 

 2，rosnode info [节点名] 

来查看某个节点具体的信息。比如说它正在发送哪些话题，订阅哪些话题，以及它提供的一些服务Services

 3，rosnode ping [节点名]

rosnode ping 是 ROS（Robot Operating System）中的一个命令行工具，用于测试 ROS 节点之间的连接性和通信延迟。

rosnode ping /my_node

上述命令将尝试与名为 /my_node 的节点进行通信，并输出相关信息，包括节点是否可达、往返通信延迟等。

• rosnode ping 会向指定的节点发送请求，检查该节点是否处于活动状态且能够响应请求。如果节点正常运行且网络通信正常，它将返回成功信息。
• 该命令可以测量从发送请求到收到节点响应的时间延迟，以毫秒为单位显示。这对于评估节点间的通信性能和系统的实时性非常有用。

4，rosnode kill  [节点名]

5，rosnode cleanup

rosnode cleanup 是 ROS（Robot Operating System）中的一个命令行工具，主要用于清理 ROS 系统中的不可达节点信息。在 ROS 系统运行过程中，由于各种原因（如节点意外崩溃、网络问题等）可能会导致节点的注册信息在 ROS 主节点的节点注册表中仍然保留，但实际上该节点已经不再运行或无法访问。rosnode cleanup 可以帮助清除这些 “僵尸” 节点的注册信息，使 ROS 主节点的节点注册表保持清洁。

当你执行 rosnode cleanup 时，它会执行以下操作：

1. 它会向 ROS 主节点发送请求，获取当前注册的所有节点列表。
2. 然后它会尝试与列表中的每个节点进行通信，以检查该节点是否仍然处于活动状态。
3. 对于无法响应的节点，它会将其从 ROS 主节点的节点注册表中删除。

 当执行 rosnode cleanup 时，它通常会输出一些信息，告诉你哪些节点被检查，哪些节点被删除。例如：

[rosnode] killing on /dead_node
[rosnode] killing on /another_dead_node

（5）rostopic

 1，rostopic list

可以打印出当前系统的所有话题列表

2，rostopic pub

可以通过此指令给某一个话题发送数据，让海龟动起来

rostopic pub /turtle1/cmd_vel 按双tab，如下图

liner表示线速度，angular表示角速度。直接在下方x y z上修改即可

 回车后海龟会按照1.0m/s速度向前运动，但运动一段时间后停下来，这是因为pub命令只会发布一次

可以在pub后面加一个-r 10来循环发布 。-r表示频率，即以多少频率发布，10表示一秒发布10次

3，rostopic delay [话题名称]

rostopic delay 是 ROS（Robot Operating System）中的一个命令行工具，主要用于测量从消息发布到接收之间的延迟时间。

当你运行 rostopic delay /my_topic 时，可能会看到类似如下的输出：

subscribed to [/my_topic]
average delay: 0.010min: 0.005s max: 0.020s std dev: 0.003s window: 5

4，rostopic echo [话题名称]

rostopic echo 是 ROS（Robot Operating System）中的一个命令行工具，主要用于在终端显示特定 ROS 话题的消息内容。

rostopic echo 会订阅指定的话题，并将该话题上接收到的消息内容输出到终端，这样你可以直接看到消息的具体内容，包括消息中的各种字段及其值。这对于查看和验证话题消息的内容非常有用，尤其是在开发和调试过程中。

（6）rosmsg show

可以查看现在发布话题消息的数据结构是怎样的

（7）rosservice

1，rosservice list

可以看到当前在仿真器里面给我们提供的所有服务内容，这里面的服务的服务端都是海龟仿真器，终端都是作为客户端去请求某个服务的。

2，rosservice call

请求某个服务

上图的/spawn是产生的意思。如果想要在海龟仿真器再产生一只海龟，可以输入rosservice call /spawn，然后按两下tab键自动补全服务类型和数据内容，x,y表示产生新的海龟的x和y坐标(仿真器左下角为(0,0)点),，theta为海龟诞生时的角度，name为产生海龟的名字。

输入完成后回车发送这个服务，仿真器会接收这个服务的请求，然后返回一个数据，如下图，多了一行的name:就是返回的数据

（8）rosbag

rosbag是一个话题记录的工具，它可以用来记录当前系统里的所有话题数据并且保存下来，重新启动roscore和海龟仿真器，然后输入rosbag play cmd_record.bag就可以将海龟运动复现出来。方便调试

1，rosbag record 记录话题数据

语法：

rosbag record [选项] [话题名称 1] [话题名称 2]...

 -O用来指定将记录的数据输出到的文件名称。如下，下述代码将把记录的话题/topic1和/topic数据保存到名为 my_bag_file.bag 的文件中。

rosbag record -O my_bag_file.bag /topic1 /topic2

这会将系统中所有话题的数据都记录下来，但需要注意文件大小可能会迅速增长，因此在使用时要谨慎。 

rosbag record -a

 -a -O连起来用如下

rosbag record -a -O my_bag_file.bag 

2，rosbag info查看记录的文件信息

rosbag info my_bag_file.bag

这个命令将显示 rosbag 文件的基本信息，包括文件大小、持续时间、开始时间、结束时间、包含的话题及其消息数量等。

3，rosbag play回放记录的文件 

语法：

rosbag play [选项] [rosbag 文件名]

rosbag play my_bag_file.bag

 可以再加-r来指定回放的速率。如下，以二倍速回方

rosbag play -r 2.0 my_bag_file.bag

也可以加-s来指定从哪个时间点开始回放。如下，将从10s后开始回放

rosbag play -s 10.0 my_bag_file.bag

以下是一个完整的示例，假设你要记录 /odom（机器人的里程计）和 /scan（激光扫描）话题的数据：

rosbag record -O robot_data.bag /odom /scan

运行该命令后，rosbag 会开始记录 /odom 和 /scan 话题的数据，你可以通过 Ctrl+C 停止记录。

使用以下命令查看刚刚生成的 rosbag 文件的信息：

rosbag info robot_data.bag

回放 rosbag 文件 

rosbag play robot_data.bag

六，创建工作空间和功能包

1，工作空间介绍

工作空间(workplace)是一个存放工程开发相关文件的文件夹。 所有的源码，配置文件和可执行文件都放在工作空间文件夹中。

工作空间里主要会分为四个文件夹：

• src:代码空间(Source Space)。用来放置功能包，功能包里的所有代码，配置文件，launch文件都在此空间里
• build:编译空间(Build Space)。放置编译过程中产生的中间文件，一般用不到
• devel:开发空间(Development Space)。放置编译生成的可执行文件，库，脚本等等
• install:安装空间(Install Space)。用install指令安装成功后的结果放在install安装空间里。安装空间和开发空间里的内容是有一定重复的，在ROS2中针对这一点做了修正，只保留了install这个空间。ROS1里还是有开发空间的，安装空间一般用不到

2，创建工作空间

 

编译工作空间就是C++里的编译，每次修改都需要重新编译。每次编译完成都要设置环境变量，除非在.bashrc文件中写入指令，下面会介绍

（1）创建工作空间

1. mkdir -p ~/catkin_ws/src ：mkdir创建文件夹，文件夹名字叫做catkin_ws，再创建catkin_ws文件夹下的src文件夹(src名称固定)。

当使用 mkdir -p 命令时，它允许你创建一个多层级的目录结构。如果父目录不存在，mkdir -p 会自动创建所需的父目录，而不会像 mkdir 命令的普通用法那样，在父目录不存在时抛出错误。假设你想创建一个目录 /path/to/new/directory，其中 /path/to/new 可能并不存在。使用普通的mkdir时就会报错，而是用-p，这个命令会先检查 /path/to/new 是否存在，如果不存在则创建它，然后再创建 directory 子目录。

1. cd ~/catkin_ws/src：进入src文件目录下
2. catkin_init_workspace：将当前文件夹变成ROS工作空间的属性 。初始化变成ROS的workspace。回车之后会产生下图所示CMakeLists.txt文件

（2）编译工作空间

1. cd ~/catkin_ws/ ：回到工作空间根目录下
2. catkin_make：编译 (ROS里catkin编译工具所提供的编译器指令，通过此指令会编译src文件下所有功能包的源码，编译结果放到devel/文件夹和install/文件夹下)。如下图，只自动产生了build和devel文件夹，并没有产生install文件夹。

要产生install文件夹需要输入catkin_make install指令，如下

（3）设置环境变量

source devel/setup.bash：编译成功后设置环境变量。因为很多可执行文件都是在工作空间里放置的，为了让系统找得到这些可执行文件，都需要去设置环境变量

为了不每次编译完成后都设置一遍环境变量，可以在.bashrc文件最后一行中添加source devel/setup.bash。具体步骤如下

打开主文件夹

按ctrl+h打开隐藏文件

双击打开.bashrc文件。在最后一行粘贴source devel/setup.bash

修改路径！

source /home/用户名/catkin_ws/devel/setup.bash。保存退出，重启终端即可 

（4）检查环境变量

echo $ROS_PACKAGE_PATH

3，创建功能包

注意：同一个工作空间下，不允许存在同名功能包。不同工作空间下，允许存在同名功能包

（1）创建功能包

创建代码时一定要创建功能包，功能包是放置ROS源码的最小单元，所有源码必须全部放到功能包里，不能直接放到src文件下。

catkin_create_pkg<package_name>[depend1][depend2][depend3] .....：package_name是要创建的功能包名字，depend是指依赖，是功能包在编译的时候需要依赖ROS里的哪些其他的功能包

例如：catkin_create_pkg test_pkg std_msgs rospy roscpp ：test_pkg功能包需要依赖ROS里的std_mags(里面有ROS定义的标准消息结构，比如int bool等) rospy roscpp功能包

1. cd src：一定在src文件下创建功能包
2. catkin_create_pkg test_pkg std_msgs rospy roscpp

src里面如下所示

test_pkg如下。src是用来放置功能包的代码文件的，include放置的是头文件，比如C++里的头文件可以放到include/test_pkg里面。CMakeLists.txt和package.xml是每个功能包必须存在的两个文件，标志着这个文件是一个功能包。

这两个文件的作用：package.xml文件里的内容通过xml语言去描述的与功能包相关的信息，比如说功能包的名字，版本号，维护者的信息，功能包的依赖信息(如下第二张图片绿色字体)。如果这些依赖在刚才没有全部添加好的话，也可以在xml文件中直接手动添加。CMakeLists.txt文件是用来描述功能包里面的编译规则，使用的语法是CMake，CMake也是一个基于gcc的编译器，它在编译规则上的设置会更加友好，我们可以通过类似编程语言的设置一样去设置功能包里代码的编译规则。

（2）编译功能包

目前已将有一个功能包了，虽然里面并没有实际的内容。这里的编译和编译工作空间一样

1. cd ~/catkin_ws
2. catkin_make
3. source ~catkin_ws/devel/setup.bash/。编译成功之后，如果要运行功能包里的某一个程序，需要先设置一下工作空间的环境变量。只有设置了这个环境变量之后，系统才能找到这个工作空间和里面的功能包
4. echo $ROS_PACKAGE_PATH

编译结果如下。可以看到编译了test_pkg这个功能包 

检查环境变量

 

 七，发布者Publisher的编程实现

在之前海龟仿真器中，主要通过键盘或者命令行形式来发布指令让海龟动起来。这节来介绍如何通过程序来实现发布者并且发布海龟速度指令让海龟动起来。

通过程序发布一个Message，Message的数据结构是Twist，里面会分成线速度角速度，通过topic(/turtle1/cmd_vel)将数据传输给Subscriber。

1，创建功能包

cd ~/catkin_ws/src

catkin_create_pkg learning_topic roscpp rospy std_msgs geometry_msgs turtlesim 

geometry_msgs为Twist消息所在包。turtlesim ，很多数据定义在turtlesim中作定义。

2，创建发布者代码(C++)

1.  初始化ROS节点
2. 向ROS Master注册节点信息。包括发布的话题名和话题中的消息类型
3. 创建消息数据
4. 按照一定频率循环发布信息

在 learning_topic功能包的src文件创建velocity_publisher.cpp文件

//该例程将发布turtle1/cmd_vel话题，消息类型geometry_msgs::Twist
//消息类型geometry_msgs::Twist用来控制海龟完成运动#include <ros/ros.h>
#include <geometry_msgs/Twist.h>//argc这是一个整数，表示命令行参数的数量。它包含了程序名称本身以及传递给程序的其他参数的总数。
//例如，如果你终端运行./my_ros_node arg1 arg2，那么 argc 的值为 3（程序名 + 2 个参数）。
//argv这是一个指向字符指针数组的指针，其中包含了程序名称以及传递给程序的实际参数。
//继续上面的例子，argv[0] 是程序名称（如 ./my_ros_node），argv[1] 是 arg1，argv[2] 是 arg2。
int main(int argc, char **argv)
{//ROS节点初始化。"velocity_publisher"为节点的名字，节点名不允许重复//ros::init 函数使用 argc 和 argv 来初始化 ROS 节点，并且接受第三个参数作为节点名称（这里是 //"node_name"）。//argc 和 argv 会传递给 ROS 系统，让 ROS 能够处理一些命令行选项，例如：//--ros-args：用于指定 ROS 相关的参数，例如 --ros-args --remap __ns:=/my_namespace 可以 //将节点的命名空间重映射到 /my_namespace。//__name:=new_node_name：可以用来在命令行修改节点名称。ros::init(argc, argv, "velocity_publisher");//创建节点句柄。主要用来管理ROS相关的API资源，比如后面创建发布者，创建一系列API调用//都是需要用节点句柄来做调用。即管理节点资源ros::NodeHandle n;//创建一个Publisher,发布的话题名为/turtle1/cmd_vel(即往/turtle1/cmd_vel话题去发布消息)//消息类型为geometry_msgs::Twist，turtle_vel_pub为发布者，队列长度10。//如果Publisher发布的很快，底层来不及响应，数据可以先存到队列里来//然后再根据实际发接收数据的能力 从队列往外拿。如果队列爆了，ROS会将最先入列的数据抛除//以保证队列里的数据都是最新的。这个队列就相当于缓存ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",                                 10);//通过上述数据完成了发布者创建，确定了发布者发布的数据类型以及给谁发//设置循环的频率。类似于终端的-r 10。//在 ROS中，ros::Rate loop_rate(10); 是一个常用的代码片段，用于控制循环的频率。//ros::Rate 是一个类，它被用来指定一个期望的循环频率。loop_rate(10) 是 ros::Rate 类的构造 //函数调用，这里的 10 表示期望的频率为 10Hz，即每秒执行 10 次。ros::Rate loop_rate(10);int count = 0;//while循环就干封装数据并且发布出去这件事/* ros::ok()用于判断ros节点是否运行异常,ros::ok() 是一个全局函数，它用于检查整个 ROS 系统的状态。当以下情况发生时，ros::ok() 会返回 false：1,收到了终止信号（如用户按下 Ctrl+C）。2,调用了 ros::shutdown() 函数，这通常是程序主动关闭 ROS 系统的操作。3,所有的 ros::NodeHandle 对象都已经被销毁，并且 ros::shutdown() 被调用。4,主节点与 ROS 主节点之间的连接丢失。*/while(ros::ok()){//初始化geometry_msgs::Twist类型的消息geometry_msgs::Twist vel_msg;//创建Twist类对象，对象里包含线速度和角速度vel_msg.linear.x = 0.5;vel.msg.angular.z = 0.2;//设置速度//发布消息turtle_vel_pub.publish(vel.msg);//ROS_INFO类似于printf，来做日志输出ROS_INFO("Publish turtle velocity command[%0.2f m/s, %0.2f rad/s]",vel_msg.linear.x,vel_msg.angular.z);//按照循环频率延时。loop_rate.sleep();//如果循环一次不过0.1s，此语句会自动补全到0.1s}    return 0;
}

3，配置发布者代码编译规则

修改CMakeLists.txt文件

将add_executable(velocity_publisher src/velocity_publisher.cpp)

target_link_libraries(velocity_publisher${catkin_LIBRARIES})两句话复制到 CMakeLists.txt文件Build之下。在Build之下有对这两句代码的解释

add_executable用来描述要把哪一个程序文件给编译成哪一个可执行文件。即将src/velocity_publisher.cpp编译为velocity_publisher可执行文件

target_link_libraries用来帮助把可执行文件跟ROS相关库做链接的，比如调用的C++，Python的一些接口。

4，编译并运行发布者

1. cd ~/catkin_ws
2. catkin_make
3. source devel/setup.bash
4. rescore
5. rosrun turtlesim turtlesim_node运行海龟仿生器
6. rosrun learning_topic velocity_publisher 运行程序

在工作空间里的devel/lib/learning_topic/velocity_publisher即为编译得到的结果

5，Python版本

rosrun learning_topic velocity_publisher.py来运行

注意运行前必须检查py文件的执行权限为对勾：右键py文件打开属性

允许作为程序执行文件打对勾即可

 

注意，一定先声明解释器#!/usr/bin/env python3

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# 该例程将发布turtle1/cmd_vel话题，消息类型geometry_msgs::Twistimport rospy
from geometry_msgs.msg import Twistdef velocity_publisher():# ROS节点初始化。#当 anonymous=True 时，ROS 会自动为节点名称添加一个随机数后缀，确保节点名称的唯一性。这在你 #可能启动多个相同类型的节点时非常有用，避免了名称冲突。#初始化一个名为 velocity_publisher 的节点，并使用 anonymous=True 确保节点名称的唯一性。如 #果有多个相同名称的节点同时运行，ROS 会自动为其添加后缀，如 velocity_publisher_12345 （其中 #12345 是随机数）。rospy.init_node('velocity_publisher', anonymous=True)# 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队 #列长度10turtle_vel_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)#设置循环的频率rate = rospy.Rate(10) while not rospy.is_shutdown():# 初始化geometry_msgs::Twist类型的消息vel_msg = Twist()vel_msg.linear.x = 0.5vel_msg.angular.z = 0.2# 发布消息turtle_vel_pub.publish(vel_msg)rospy.loginfo("Publsh turtle velocity command[%0.2f m/s, %0.2f rad/s]", vel_msg.linear.x, vel_msg.angular.z)# 按照循环频率延时rate.sleep()if __name__ == '__main__':try:velocity_publisher()#rospy.ROSInterruptException 是 ROS 中一个特殊的异常，通常在 ROS 系统终止时抛出，例如使用 #Ctrl+C 终止节点或 roscore 关闭时。#pass 是一个占位符，表示不执行任何操作。当捕获到 rospy.ROSInterruptException 异常时，代码 #不会执行任何额外操作，程序会正常终止。except rospy.ROSInterruptException:pass

八，订阅者Subscriber的编程实现

实现一个订阅者来订阅海龟的位姿信息。Publisher为海龟仿真器，Subscriber是我们这一节要实现的程序。两者之间要传递的Message是海龟的pose信息，话题是/turtle1/pose。

 1，创建订阅者代码(C++)

1. 初始化ROS节点
2. 订阅需要的话题
3. 循环等待话题消息，接收到消息后进入回调函数
4. 在回调函数中完成消息处理

/*** 该例程将订阅/turtle1/pose话题，消息类型turtlesim::Pose*/#include <ros/ros.h>
#include "turtlesim/Pose.h"// 接收到订阅的消息后，会进入消息回调函数
//输入参数是消息的指针
//poseCallback 函数的输入参数 const turtlesim::Pose::ConstPtr& msg 是一个指向 //turtlesim::Pose 消息的常量指针，用于接收并处理 /turtle1/pose 话题上的消息
void poseCallback(const turtlesim::Pose::ConstPtr& msg)
{// 将接收到的消息打印出来。得到海龟的当前位置信息ROS_INFO("Turtle pose: x:%0.6f, y:%0.6f", msg->x, msg->y);
}int main(int argc, char **argv)
{// 初始化ROS节点ros::init(argc, argv, "pose_subscriber");// 创建节点句柄ros::NodeHandle n;// 创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数poseCallback//Subscriber一端也要创建一个缓冲区，即队列，队列不断保存从Publisher传来的数据//Subscriber根据自己的处理能力一个一个的处理//订阅者并不知道发布者的数据何时进来。当订阅者一旦发现有数据进来之后，就会跳到回调函数//来进行处理。类似于中断。ros::Subscriber pose_sub = n.subscribe("/turtle1/pose", 10, poseCallback);// 循环等待回调函数。spin当中会不断查看队列，看队列中有没有消息进来//有进来的话就会调用回调函数，没有的话就会不断死循环//ros::spin() 会不断检查是否有新的消息到达，如果有新消息，它会调用相应的回调函数。//它会处理各种 ROS 事件，包括消息到达、服务调用等，确保程序能够对 ROS 系统中的各种事件做出响 //应。当你有一个或多个订阅者需要持续接收消息时，ros::spin() 可以确保你的节点保持运行并处理这 //些消息。ros::spin()会阻塞程序，持续等待并处理各种事件，直到节点关闭（如按下 Ctrl+C 或 //ros::ok() 为 false）。ros::spinOnce()只处理一次事件，不会阻塞程序。通常与 ros::Rate 一 //起使用，在需要控制节点的执行频率时很有用ros::spin();return 0;
}

 2，配置订阅者代码编译规则

修改CMakeLists.txt文件

将add_executable(pose_subscriber src/pose_subscriber.cpp)

target_link_libraries(pose_subscriber${catkin_LIBRARIES})两句话复制到 CMakeLists.txt文件Build之下

3，编译并运行发布者 

1. cd ~/catkin_ws
2. catkin_make
3. source devel/setup.bash
4. rescore
5. rosrun turtlesim turtlesim_node运行海龟仿生器
6. rosrun learning_topic velocity_publisher 运行发布者程序
7. rosrun learning_topic pose_subscriber 运行订阅者程序

4，Python版本

rosrun learning_topic pose_subscriber.py来运行

# 该例程将订阅/turtle1/pose话题，消息类型turtlesim::Poseimport rospy
from turtlesim.msg import Posedef poseCallback(msg):rospy.loginfo("Turtle pose: x:%0.6f, y:%0.6f", msg.x, msg.y)def pose_subscriber():# ROS节点初始化rospy.init_node('pose_subscriber', anonymous=True)# 创建一个Subscriber，订阅名为/turtle1/pose的topic，使用 Pose 消息类型，注册回调函数 # poseCallbackrospy.Subscriber("/turtle1/pose", Pose, poseCallback)# 循环等待回调函数rospy.spin()if __name__ == '__main__':pose_subscriber()

九，话题消息的定义和使用

在七八两节学习中，不管是在发布指令的Twist消息还是去定义海龟位姿的Pose消息，这个Twist和Pose都是在ROS中定义好的，可以直接拿来使用。如果在开发中，ROS定义的消息已经无法满足使用的要求，这时我们就可以自己来定义消息的类型。

这一节课就完成消息的自定义并且完成Publisher和Subscriber的使用。这一节完成的传输是传输一个Person的个人信息(包括名字，年龄，性别)，Publisher来发布这个Person信息，Subscriber来接收这个Person信息，/person_info作为话题名(Topic)，我们会把Person个人信息在learning_topic里做定义，所以消息就是learning_topic里的Person消息

1，自定义话题消息

（1）创建.msg文件 

打开/catkin_ws/src/learning_topic功能包，在功能包里面创建文件夹叫做msg(与消息有关的定义全都放到msg文件夹里，方便管理)

打开msg文件夹，在里面创建Person.msg文件 

打开Person.mag文件。并在里面输入消息定义的数据类型完成数据接口的定义，ROS会根据string uint8这些定义来变成不同的C++和Python的程序。这里我们对Person作描述，包含string名字字符串，sex性别，age年龄，同时我们会针对性别，分为男女和未知三种，定义一些宏定义分别对应数字0 1 2，然后将其放到.msg文件，这个文件是和语言无关的，string和uint8这些表示我们要在不同的程序里面扩展成对应该种程序的表示方法。

完成数据接口定义后，接下来要针对数据接口的定义来设置编译规则 

（2）在package.xml中添加功能包依赖

添加动态生成程序的功能包的依赖：

<build_depend>为编译依赖，<exec_depend>为执行依赖。编译依赖一个动态产生message的功能包message_generation，运行依赖一个动态message_runtime

点开package.xml文件

在最下面对应位置添加上述两行代码 添加依赖

（3）在CMakeLists.txt添加编译选项

打开 CMakeLists.txt

首先在最上面find_package里面添加功能包message_generation

 第二步在CMakeLists.txt里面添加一个把.msg文件编译成对应的不同的程序文件的配置项

在如下对应位置添加。第一个是add_message_files，它会把我们刚才定义的Person.msg作为定义的交接接口，编译的时候编译器就会发现这个定义的接口，针对它去做编译。第二个是，这个就是编译Person.msg文件时需要依赖哪些ROS已有的包，std_msgs这个包就包含刚才定义的string和uint8

第三步需要在catkin_package里面创建一个message运行的依赖message_runtime。

在下图对应位置将catkin_package里面把CATKIN_DEPENDS(编译时的依赖)一行解除注释，并将message_runtime加入进去

（4）编译生成语言相关文件

回到catkin_ws文件下打开终端，输入catkin_make开始编译。

编译完成后，可以在/catkin_ws/devel/include/learning_topic里看到Person.h文件，这个就是根据.mag文件去编译得到的C++头文件

以下为打开后里面的内容 。里面的内容根据简单的几行定义代码扩展出一系列针对C++类的定义

2，创建发布者代码(C++)

将发布者和订阅者代码都拷贝到/catkin_ws/src/learning_topic/src文件下

/*** 该例程将发布/person_info话题，自定义消息类型learning_topic::Person*/#include <ros/ros.h>
#include "learning_topic/Person.h"int main(int argc, char **argv)
{// ROS节点初始化ros::init(argc, argv, "person_publisher");// 创建节点句柄ros::NodeHandle n;// 创建一个Publisher，发布名为/person_info的topic，消息类型为learning_topic::Person，队列 //长度10。Publisher用来发布我们自己创建的learning_topic::Person消息接口的//这个消息接口需要通过#include "learning_topic/Person.h"去调用//Publisher发布的数据往/person_info这个话题去发的ros::Publisher person_info_pub = n.advertise<learning_topic::Person>("/person_info", 10);// 设置循环的频率ros::Rate loop_rate(1);int count = 0;while (ros::ok()){// 初始化learning_topic::Person类型的消息learning_topic::Person person_msg;person_msg.name = "Tom";person_msg.age  = 18;person_msg.sex  = learning_topic::Person::male;// 发布消息person_info_pub.publish(person_msg);ROS_INFO("Publish Person Info: name:%s  age:%d  sex:%d", person_msg.name.c_str(), person_msg.age, person_msg.sex);//ROS_INFO 函数是 ROS 中的日志输出函数，它的原型通常接受 const char* 类型的参数，这是 //C 风格的字符串。C 风格的字符串以 '\0' 结尾，存储在字符数组中。//为了将 std::string 类型的 person_msg.name 转换为 const char* 类型，我们使用 //c_str() 方法。std::string 的 c_str() 方法会返回一个指向内部存储的字符数组的指针，该 //字符数组包含了与 std::string 相同的内容，并且以 '\0' 结尾，从而可以直接作为 const //char* 使用。// 按照循环频率延时loop_rate.sleep();}return 0;
}

3，创建订阅者代码(C++) 

/*** 该例程将订阅/person_info话题，自定义消息类型learning_topic::Person*/#include <ros/ros.h>
#include "learning_topic/Person.h"// 接收到订阅的消息后，会进入消息回调函数
void personInfoCallback(const learning_topic::Person::ConstPtr& msg)
{// 将接收到的消息打印出来ROS_INFO("Subcribe Person Info: name:%s  age:%d  sex:%d", msg->name.c_str(), msg->age, msg->sex);
}int main(int argc, char **argv)
{// 初始化ROS节点ros::init(argc, argv, "person_subscriber");// 创建节点句柄ros::NodeHandle n;// 创建一个Subscriber，订阅/person_info话题，一旦有消息就进入回调函数personInfoCallbackros::Subscriber person_info_sub = n.subscribe("/person_info", 10, personInfoCallback);// 循环等待回调函数ros::spin();return 0;
}

 4，配置代码编译规则

add_executable(person_publisher src/person_publisher.cpp)

target_link_libraries(person_publisher ${catkin_LIBRARIES})

add_dependencies(person_publisher ${PROJECT_NAME}_generate_messages_cpp)

add_executable(person_subscriber src/person_subscriber.cpp)

target_link_libraries(person_subscriber ${catkin_LIBRARIES})

add_dependencies(person_subscriber ${PROJECT_NAME}_generate_messages_cpp)

将上述6行代码添加到指定位置。与七八节的配置代码编译规则一样。 这里因为我们有一些代码是动态生成的，所以我们需要让我们的person_publisher和person_subscriber可执行文件与动态生成的一些程序产生依赖关系。add_dependencies就是用来动态的与刚才生成的头文件Person.h做链接的。

5，编译并运行发布者和订阅者

1. cd ~/catkin_ws
2. catkin_make
3. source devel/setup.bash
4. roscore
5. rosrun learning_topic person_subscriber
6.  rosrun learning_topic person_publisher(subscriber与publisher先运行谁都可以)

可以看到发布者和订阅者开始通讯了 

如果此时将roscore关闭，两者仍然在通讯。这是因为roscore即ROS Master在一开始给两者建立联系后，ROS Master退出后，两者通讯不会断开。

6，Python版本

person_publisher.py

#!/usr/bin/env python3
# 该例程将发布/person_info话题，自定义消息类型learning_topic::Personimport rospy
from learning_topic.msg import Persondef velocity_publisher():# ROS节点初始化rospy.init_node('person_publisher', anonymous=True)# 创建一个Publisher，发布名为/person_info的topic，消息类型为learning_topic::Person，队列长度10person_info_pub = rospy.Publisher('/person_info', Person, queue_size=10)#设置循环的频率rate = rospy.Rate(10) while not rospy.is_shutdown():# 初始化learning_topic::Person类型的消息person_msg = Person()person_msg.name = "Tom";person_msg.age  = 18;person_msg.sex  = Person.male;# 发布消息person_info_pub.publish(person_msg)rospy.loginfo("Publsh person message[%s, %d, %d]", person_msg.name, person_msg.age, person_msg.sex)# 按照循环频率延时rate.sleep()if __name__ == '__main__':try:velocity_publisher()except rospy.ROSInterruptException:pass

person_subscriber.py

#!/usr/bin/env python3
# 该例程将订阅/person_info话题，自定义消息类型learning_topic::Personimport rospy
from learning_topic.msg import Persondef personInfoCallback(msg):rospy.loginfo("Subcribe Person Info: name:%s  age:%d  sex:%d", msg.name, msg.age, msg.sex)def person_subscriber():# ROS节点初始化rospy.init_node('person_subscriber', anonymous=True)# 创建一个Subscriber，订阅名为/person_info的topic，注册回调函数personInfoCallbackrospy.Subscriber("/person_info", Person, personInfoCallback)# 循环等待回调函数rospy.spin()if __name__ == '__main__':person_subscriber()

十，客户端Client的编程实现

在之前的海龟仿真器中，我们通过命令行在海龟仿真器中生成了一只新的海龟。这节学习如何通过程序的形式发布服务请求来实现客户端去让海龟仿真器生成一只新海龟

Server端是海龟仿真器，Client端是本节要实现的程序，在Client端我们会来实现一个客户端请求的节点，它会发布一个产生海龟的Request请求，发给Server端，Server端收到请求之后会产生一只新海龟并且回馈一个Response给Client。中间的Service名字是/spawn，Service里的消息结构是turtlesim::Spawn。

 

1，创建功能包

1.  cd ~/catkin_ws/src
2. catkin_create_pkg learning_service roscpp rospy std_msgs geometry_msgs turtlesim std_srvs

2，创建客户端代码(C++)

将下方代码放到/learning_sevice/src下面来

1. 初始化ROS节点
2. 创建一个Client实例
3. 发布服务请求数据
4. 等待Server处理之后的应答结果 

/*** 该例程将请求/spawn服务，服务数据类型turtlesim::Spawn*/#include <ros/ros.h>
#include <turtlesim/Spawn.h>int main(int argc, char** argv)
{// 初始化ROS节点ros::init(argc, argv, "turtle_spawn");// 创建节点句柄ros::NodeHandle node;// 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的service//当调用 waitForService("/spawn") 时，它会阻塞当前程序的执行，直到 /spawn 服务可用。//当你编写的 ROS 节点需要调用 /spawn 服务，但不确定该服务是否已经启动时，使用 //waitForService 函数可以确保在服务可用之前，程序不会尝试调用该服务而导致错误。	//waitForService 函数会阻塞程序执行，直到服务可用或超时。如果在指定的超时时间内服务没有可 //用，程序将继续执行，但会返回 false。默认情况下，超时时间较长，可以通过函数的第二个参数设置 //超时时间，例如 ros::service::waitForService("/spawn", ros::Duration(10.0)) 会等待10s ros::service::waitForService("/spawn");//创建客户端，客户端用来给/spawn服务发送请求的，请求的数据类型为turtlesim::Spawnros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");// 初始化turtlesim::Spawn的请求数据turtlesim::Spawn srv;srv.request.x = 2.0;srv.request.y = 2.0;srv.request.name = "turtle2";// 请求服务调用ROS_INFO("Call service to spwan turtle[x:%0.6f, y:%0.6f, name:%s]", srv.request.x, srv.request.y, srv.request.name.c_str());//在topic通讯里是publicsh和subscribe,在service里面请求客户端则是calladd_turtle.call(srv);//阻塞型函数，将Request发出去并且一直等待服务器给Response// 显示服务调用结果ROS_INFO("Spwan turtle successfully [name:%s]", srv.response.name.c_str());return 0;
};

3，配置客户端代码编译规则

和之前一样

add_executable(turtle_spawn src/turtle_spawn.cpp)

target_link_libraries(turtle_spawn ${catkin_LIBRARIES})

将上述两行代码拷贝到/learning_service/CMakeLists.txt的对应位置

 4，编译并运行客户端

1. cd ~/catkin_ws
2. catkin_make
3. source devel/setup.bash
4. roscore
5. rosrun turtlesim turtlesim_node 启动海龟仿真器
6. rosrun learning_service turtle_spawn

编译完成后的/catkin_ws/devel/lib/learning_service/turtle_spawn即为编译完成生成的可执行文件。

运行结果如下

5，Python版本

将Python放在新建的文件夹scripts(也可以为别的名)里也可以，放在src文件里也可以

#!/usr/bin/env python3
# 该例程将请求/spawn服务，服务数据类型turtlesim::Spawnimport sys
import rospy
from turtlesim.srv import Spawndef turtle_spawn():# ROS节点初始化rospy.init_node('turtle_spawn')# 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的servicerospy.wait_for_service('/spawn')try:add_turtle = rospy.ServiceProxy('/spawn', Spawn)# 请求服务调用，输入请求数据request = add_turtle(2.0, 2.0, 0.0, "turtle2")return request.name# rospy.ServiceException 是 ROS Python 库 rospy 中定义的一个异常类型。当使用 # rospy.ServiceProxy 调用服务时，如果发生错误，通常会抛出 rospy.ServiceException 异常。# as e 部分将捕获到的异常对象存储在变量 e 中，以便后续可以对其进行处理或打印异常信息。当发生 # 这种异常时，会执行 print "Service call failed: %s"%e，将异常信息打印出来，让用户知道服务 # 调用失败以及失败的原因。except rospy.ServiceException as e:print ("Service call failed: %s"%e)if __name__ == "__main__":#服务调用并显示调用结果print ("Spwan turtle successfully [name:%s]" %(turtle_spawn()))

十一，服务端Server的编程实现

这一节我们实现Server端的功能，Server端的功能主要是给海龟发送速度指令(通过topic发送)，通过Service要实现的功能是Client端发布Request来控制Server是否给海龟发送指令。Client一端类似于海龟的运动与停止的开关，发送一个Request海龟就运动，再发送一个Request海龟就停止，Server一端来接收开关指令并且完成海龟运动指令的发送。这里Service的名字是/turtle_command是我们自定义的，Service的数据类型定义是用ROS当中针对服务的标准定义Trigger，相当于是一个触发信号。

1，创建服务器代码(C++)

/*** 该例程将执行/turtle_command服务，服务数据类型std_srvs/Trigger*/#include <ros/ros.h>
#include <geometry_msgs/Twist.h>//给海龟用topic发布指令时用
#include <std_srvs/Trigger.h>//服务数据类型头文件bool pubCommand = false;//作为标志位，标志海龟是运动还是停止。默认为停止// service回调函数，输入参数req(Trigger的Request是空内容)，输出参数res
bool commandCallback(std_srvs::Trigger::Request  &req,std_srvs::Trigger::Response &res)
{pubCommand = !pubCommand;//对标志位处理// 显示请求数据ROS_INFO("Publish turtle velocity command [%s]", pubCommand==true?"Yes":"No");// 设置反馈数据。给Client端Response。success和message是对应Trigger的定义的，可以看下方介绍res.success = true;res.message = "Change turtle command state!";return true;
}int main(int argc, char **argv)
{// ROS节点初始化ros::init(argc, argv, "turtle_command_server");// 创建节点句柄ros::NodeHandle node;// 创建一个名为/turtle_command的service，注册回调函数commandCallback//在回调函数里通过topic发送指令让海龟动起来ros::ServiceServer command_service = node.advertiseService("/turtle_command", commandCallback);// 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队 // 列长度10ros::Publisher turtle_vel_pub = node.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);// 循环等待回调函数ROS_INFO("Ready to receive turtle command.");// 设置循环的频率ros::Rate loop_rate(10);while(ros::ok()){// 查看一次回调函数队列，即查看是否有服务消息进来，一旦有服务消息进来就会进入回调函数ros::spinOnce();// 如果标志位为true，则发布速度指令if(pubCommand){geometry_msgs::Twist vel_msg;vel_msg.linear.x = 0.5;vel_msg.angular.z = 0.2;turtle_vel_pub.publish(vel_msg);}//按照循环频率延时loop_rate.sleep();}return 0;
}

Trigger定义：可以用命令行rossrv show std_srv/Trigger(rossrv和rosmsg类似)查看

以下即为Service的Trigger定义，Service定义方式下一节会详细介绍。Service会涉及到Request与Response两个部分的数据，会通过三个横线做区隔，如下，三个横线之上为Request内容，之下为Response内容。这里Trigger的Request内容是空的，这在ROS里是允许的。

 2，配置服务器代码编译规则

add_executable(turtle_command_server src/turtle_command_server.cpp)

target_link_libraries(turtle_command_server ${catkin_LIBRARIES})

3，编译并运行服务器

1. cd ~/catkin_ws
2. catkin_make
3. source devel/setup.bash
4. roscore
5. rosrun turtlesim turtlesim_node 启动海龟仿真器
6. rosrun learning_service turtle_command_server
7. rosservice call /turtle_command 通过终端发送service请求

输入rosservice call /turtle_command后可以看到海龟动起来了。 

再输入rosservice call /turtle_command 后，海龟就停下来了

4，Python版本 

rosrun learning_service turtle_command_server.py来运行即可

#!/usr/bin/env python3
# 该例程将执行/turtle_command服务，服务数据类型std_srvs/Triggerimport rospy
import threading,time
from geometry_msgs.msg import Twist
from std_srvs.srv import Trigger, TriggerResponsepubCommand = False;
turtle_vel_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)# 在C++程序里面调用了ros::spinOnce();这个函数，会查询一次队列看队列里是否有数据，如果有的话就会
# 进入回调函数，如果没有的话就会跳出继续执行while其他代码。但是spinOnce()在ROS的Python接口里
# 是没有实现的，Python里只有一个spin()是不断循环的。所以在Python里面我们需要通过多线程的
# 机制来实现类似spinOnce()和if判断的功能。所以Python程序会多加一个command_thread()函数
def command_thread():	while True:if pubCommand:vel_msg = Twist()vel_msg.linear.x = 0.5vel_msg.angular.z = 0.2turtle_vel_pub.publish(vel_msg)time.sleep(0.1)def commandCallback(req):global pubCommandpubCommand = bool(1-pubCommand)# 显示请求数据rospy.loginfo("Publish turtle velocity command![%d]", pubCommand)# 反馈数据return TriggerResponse(1, "Change turtle command state!")def turtle_command_server():# ROS节点初始化rospy.init_node('turtle_command_server')# 创建一个名为/turtle_command的server，注册回调函数commandCallbacks = rospy.Service('/turtle_command', Trigger, commandCallback)# 循环等待回调函数print ("Ready to receive turtle command.")# 创建一个新线程启动函数command_thread()threading.Thread(target = command_thread).start()rospy.spin()  # 死循环。不断查看是否有Request进来。一旦有就会进入回调函数if __name__ == "__main__":turtle_command_server()

十二，服务数据的定义和使用

前面我们学习到了/spawn和trigger两种已经在ROS中定义好的服务数据类型。很多场景下面ROS已经定义好的数据结构并不能满足我们的需求。本节学习根据自己的需求来定义服务数据类型并且使用它

 Client一端发布显示某个人信息的请求并且将此人信息通过自定义的Service数据给发送出去，在Server一端就会收到Request同时里面包含这个人的名字年龄等信息，然后通过日志显示出来，同时通过Response反馈显示结果。Service名字是自定义的/show_person，服务数据类型是learning_service::Person

1，自定义服务数据

（1）定义srv文件

服务Service与消息message不同的是服务是有Response的，这就意味着我们要在.srv文件中定义Request数据类型和Response数据类型两种。这两种数据类型之间通过---隔开。---以上为Request数据类型，以下为Response数据类型。编译时会通过srv文件里的定义来产生对应的头文件

在catkin_ws/src/learning_service目录下创建srv文件夹

在srv里面创建文件Person.srv

打开Person.srv文件，然后将要定义的数据类型放进去 

 接下来就会涉及到编译的相关步骤。编译跟我们之前的message是一样的。如下

（2）在package.xml中添加功能包依赖

打开package.xml文件

  在对应位置添加配置

（3） 在CMakeLists.txt添加编译选项

添加操作和话题消息的定义和使用完全一样 

（4）编译生成语言相关文件

回到/catkin_ws目录下打开终端输入catkin_make进行编译

在catkin_ws/devel/include/learning_service下查看编译结果。可以看到里面是有三个头文件。刚才在Person.srv中定义的数据类型三个横线上方的Request数据类型会放到PersonRequest.h这个头文件，三横线下方的Response数据类型会放到PersonResponse.h头文件下，同时会生成一个整体的Person.h包含PersonRequest.h和PersonResponse.h，所以用的时候一般只用Person.h

2，创建客户端代码(C++)

将创建服务器Server和创建客户端Client的代码都放到catkin_ws/src/learning_service/src文件下

/*** 该例程将请求/show_person服务，服务数据类型learning_service::Person*/#include <ros/ros.h>
#include "learning_service/Person.h"int main(int argc, char** argv)
{// 初始化ROS节点ros::init(argc, argv, "person_client");// 创建节点句柄ros::NodeHandle node;// 发现/show_person服务后，创建一个服务客户端，连接名为/show_person的serviceros::service::waitForService("/show_person");ros::ServiceClient person_client = node.serviceClient<learning_service::Person>("/show_person");// 初始化learning_service::Person的请求数据learning_service::Person srv;//Person这个名字需要和定义的Person.srv文件名完全一致srv.request.name = "Tom";srv.request.age  = 20;srv.request.sex  = learning_service::Person::Request::male;// 请求服务调用ROS_INFO("Call service to show person[name:%s, age:%d, sex:%d]", srv.request.name.c_str(), srv.request.age, srv.request.sex);person_client.call(srv);//发送服务请求并等待Server反馈结果// 显示服务调用结果ROS_INFO("Show person result : %s", srv.response.result.c_str());return 0;
};

3，创建服务器代码(C++)

/*** 该例程将执行/show_person服务，服务数据类型learning_service::Person*/#include <ros/ros.h>
#include "learning_service/Person.h"// service回调函数，输入参数req，输出参数res
bool personCallback(learning_service::Person::Request  &req,learning_service::Person::Response &res)
{// 显示请求数据ROS_INFO("Person: name:%s  age:%d  sex:%d", req.name.c_str(), req.age, req.sex);// 设置反馈数据res.result = "OK";return true;
}int main(int argc, char **argv)
{// ROS节点初始化ros::init(argc, argv, "person_server");// 创建节点句柄ros::NodeHandle n;// 创建一个名为/show_person的service，注册回调函数personCallbackros::ServiceServer person_service = n.advertiseService("/show_person", personCallback);// 循环等待回调函数ROS_INFO("Ready to show person informtion.");ros::spin();//不断等待Request信息，一旦有Request信息就会进入回调函数return 0;
} 

4，配置代码编译规则

add_executable(person_server src/person_server.cpp)

target_link_libraries(person_server ${catkin_LIBRARIES})

add_dependencies(person_server ${PROJECT_NAME}_gencpp)

add_executable(person_client src/person_client.cpp)

target_link_libraries(person_client ${catkin_LIBRARIES})

add_dependencies(person_client ${PROJECT_NAME}_gencpp)

在catkin_ws/src/learning_service/src/CMakeLists.txt里面 指定位置加入上述编译规则

5，编译并运行发布者和订阅者

1. cd ~/catkin_ws
2. catkin_make
3. source devel/setup.bash
4. roscore
5. rosrun learning_service person_server
6. rosrun learning_service person_client

 可以看到如下运行结果

注意：每次运行程序之前，尤其是运行新的程序之前，记得将roscore给关掉。因为roscore里面的参数服务器会有很多的参数，如果运行程序很多，有些参数名字可能会一样，这就会导致冲突造成一些奇怪的问题。所以建议每次运行程序之前将roscore重启一遍 

6，Python版本

• person_server.py

# 该例程将执行/show_person服务，服务数据类型learning_service::Personimport rospy
from learning_service.srv import Person, PersonResponsedef personCallback(req):# 显示请求数据rospy.loginfo("Person: name:%s  age:%d  sex:%d", req.name, req.age, req.sex)# 反馈数据return PersonResponse("OK")def person_server():# ROS节点初始化rospy.init_node('person_server')# 创建一个名为/show_person的server，注册回调函数personCallbacks = rospy.Service('/show_person', Person, personCallback)# 循环等待回调函数print "Ready to show person informtion."rospy.spin()if __name__ == "__main__":person_server()

• person_client.py 

# 该例程将请求/show_person服务，服务数据类型learning_service::Personimport sys
import rospy
from learning_service.srv import Person, PersonRequestdef person_client():# ROS节点初始化rospy.init_node('person_client')# 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的servicerospy.wait_for_service('/show_person')try:person_client = rospy.ServiceProxy('/show_person', Person)# 请求服务调用，输入请求数据response = person_client("Tom", 20, PersonRequest.male)return response.resultexcept rospy.ServiceException as e:print ("Service call failed: %s"%e)if __name__ == "__main__":#服务调用并显示调用结果print ("Show person result : %s" %(person_client()))

十三，参数的使用与编程方法

在ROS Master当中有一个参数服务器Parameter Server，它是一个全局字典用来保存各个节点之间的配置参数，里面的配置参数是各个节点都可以全局访问的。比如在A节点里面访问robot_name，就会获取到my_robot这个值，它只要给ROS Master发送查询请求就会返回my_robot这样一个结果。同理Node B Node C都是一样的，甚至Node A和Node C不在一个PC端都可以访问全局字典(只要在同一个ROS环境下即可)

1，创建功能包

1. cd ~/catkin_ws/src
2. catkin_create_pkg learning_parameter roscpp rospy std_srvs 

 

2，parameter参数命令行使用 

以海龟为例看一下rosparam命令行的使用方式。rosparam可以完成大部分参数的查询，设置，保存到文件，读取文件等等。

在ROS里，如果参数比较多的话，一般会用到YAML参数文件，如下。左边是变量名，右边是变量对应的值，将所有参数都列出来，然后可以一次性加载到ROS的parameter里。

终端中输入rosparam回车，可以看到后面可以跟一系列的参数如下 

 

（1）rosparam list

查看例程里有多少参数 。如果是/turtlesim/background_b的话，在写程序时记得将turtlesim补全，写为ros::param::get("/turtlesim/background_r", red);而不是ros::param::get("/background_r", red);

background_b g r指的是海龟仿真器的背景颜色，默认为蓝色，rosdistro是ROS的发行版版本，rosversion是ROS的版本号，run_id是进程的ID号 

（2）rosparam get 参数名

 查看某个参数的值

（3）rosparam set 参数名 要修改成的参数值

 修改后再给仿真器发送请求rosservice call /clear "{}"，仿真器背景颜色就会修改

（4）rosparam dump 文件名

保存参数到文件

 

输入后回车，param.yaml文件就保存在了当前路径下，并且保存了当前路径下的参数值

打开param.yaml文件即可看到保存的参数。可以在文件中修改参数，修改完成保存关闭，再使用rosparam load命令即可。

（5）rosparam load 文件名

从文件中读取参数，加载文件等

 

回车后即可将在文件中修改的参数生效 

（6）rosparam delete 参数名

3，在程序中获得/修改参数

将其拷贝到/catkin_ws/src/learning_parameter/src文件下

/*** 该例程设置/读取海龟例程中的参数*/
#include <string>
#include <ros/ros.h>
#include <std_srvs/Empty.h>
//Empty 服务消息的请求和响应都不包含任何参数。它的定义非常简单，没有数据成员，仅仅是一个空的服务
//消息结构。int main(int argc, char **argv)
{int red, green, blue;// ROS节点初始化ros::init(argc, argv, "parameter_config");// 创建节点句柄ros::NodeHandle node;// 读取背景颜色参数//原型：ros::param::get(const std::string& key, T& value)//value是用于存储获取到的参数值的变量。ros::param::get("/background_r", red);ros::param::get("/background_g", green);ros::param::get("/background_b", blue);ROS_INFO("Get Backgroud Color[%d, %d, %d]", red, green, blue);// 设置背景颜色参数ros::param::set("/background_r", 255);ros::param::set("/background_g", 255);ros::param::set("/background_b", 255);ROS_INFO("Set Backgroud Color[255, 255, 255]");// 读取背景颜色参数ros::param::get("/background_r", red);ros::param::get("/background_g", green);ros::param::get("/background_b", blue);ROS_INFO("Re-get Background Color[%d, %d, %d]", red, green, blue);// 调用服务，刷新背景颜色ros::service::waitForService("/clear");ros::ServiceClient clear_background = node.serviceClient<std_srvs::Empty>("/clear");std_srvs::Empty srv;clear_background.call(srv);sleep(1);return 0;
}

4，配置代码编译规则

add_executable(parameter_config src/parameter_config.cpp)

target_link_libraries(parameter_config ${catkin_LIBRARIES})

5，编译并运行发布者

1. cd ~/catkin_ws
2. catkin_make
3. source devel/setup.bash
4. roscore
5. rosrun turtlesim turtlesim_node
6. rosrun learning_parameter parameter_config

注意：get 和set并不是ROS里唯一的获取参数或者设置参数的方式，也有其他方式。可以参考链接:http://wiki.ros.org/Parameter%20Server

6，Python版本

# 该例程设置/读取海龟例程中的参数import sys
import rospy
from std_srvs.srv import Emptydef parameter_config():# ROS节点初始化rospy.init_node('parameter_config', anonymous=True)# 读取背景颜色参数red   = rospy.get_param('/background_r')green = rospy.get_param('/background_g')blue  = rospy.get_param('/background_b')rospy.loginfo("Get Backgroud Color[%d, %d, %d]", red, green, blue)# 设置背景颜色参数rospy.set_param("/background_r", 255);rospy.set_param("/background_g", 255);rospy.set_param("/background_b", 255);rospy.loginfo("Set Backgroud Color[255, 255, 255]");# 读取背景颜色参数red   = rospy.get_param('/background_r')green = rospy.get_param('/background_g')blue  = rospy.get_param('/background_b')rospy.loginfo("Get Backgroud Color[%d, %d, %d]", red, green, blue)# 发现/spawn服务后，创建一个服务客户端，连接名为/spawn的servicerospy.wait_for_service('/clear')try:clear_background = rospy.ServiceProxy('/clear', Empty)# 请求服务调用，输入请求数据response = clear_background()return responseexcept rospy.ServiceException as e:print ("Service call failed: %s"%e)if __name__ == "__main__":parameter_config()

十四，ROS中的坐标系管理系统

1，TF工具包介绍

在ROS中针对坐标变换的工具TF的使用方法

在一个机器人中会有许多许多繁杂的坐标系，我们如何描述任意两个坐标系的关系就很麻烦，如果用纯理论来算可能要做几十次矩阵运算。TF功能包可以管理所有的坐标系，用这个功能包就可以直接通过查询的方式来得到两个坐标系之间的关系，所有的底层数学计算都封装到功能包里面帮我们完成了，这就是TF功能包最重要的一个功能。

TF功能包是有一个时间属性的，它默认会记录10s钟之内的机器人所有坐标系之间的位置关系，比如可以查询5s钟之前机器人的头部坐标系相对于全局坐标系的关系。机械臂抓取物体，物体相对于机械臂的终端坐标系，相对于机器人地盘坐标系位置关系都可以通过TF功能包来直接获取

 TF坐标变换通过广播TF变换和监听TF变换来实现，并不是topic或者service的方式。在ROS系统里面，只要启动ROS Master和TF，就会在后台维护一颗TF Tree，所有的坐标系都是通过树形结构保存在TF树当中，任意一个节点想要查询任意两个坐标系之间的变换关系时都可以通过树去查询得到

2，TF功能举例说明

比如说下图的移动机器人，红色的为底盘。建模时会给地盘机器人的中心点做一个坐标系(比如下图命名的base_link)，在机器人地盘上还装有激光雷达，在激光雷达的中心点也建立一个坐标系(比如下图的base_laser)，所有检测到的激光雷达的数据是在激光雷达坐标系下的一个位置关系。base_link和base_laser这两个坐标系之间肯定是有位置关系的，我们可以通过机器人的安装位置来确定(比如下图x方向差10cm,y方向差20cm，平移关系，没有旋转关系)，当激光雷达检测到障碍物的距离，比如(0.3,0.0,0.0)，0.3m如何去变换得到在base_link下的数据就要通过TF来完成。变换即可得到base_link(0.4, 0.0, 0.2)

 

3，坐标数据查询工具

通过海龟案例来进一步讲解TF在ROS中的作用

要实现的效果是在海龟仿真器里生成两只海龟，一只海龟通过键盘控制，另一只海龟可以自动跟随键盘控制的海龟移动 

1. sudo apt-get install ros-noetic-turtle-tf 安装TF功能包
2. roslaunch turtle_tf turtle_tf_demo.launch    roslaunch用来启动ROS当中的launch文件，launch文件可以理解为一个脚本，里面可以把许多节点的启动都写进去，一次性启动多个节点。如果这一步出现大片红字，应该是python版本的问题，输入sudo apt install python-is-python3就可以了
3. rosrun turtlesim turtle_teleop_key 运行海龟键盘控制节点
4. rosrun tf view_frames    tf功能包里提供的一个工具，可以直接可视化的看到整个系统当中的所有坐标系之间关系。经常用此方法来查看tf是不是连通的。此步如果出现typeerror问题，输入sudo nano /opt/ros/noetic/lib/view_frames 将m=r.search(vstr)改为m = r.search(vstr.decode('utf-8'))即可

（1）tf工具1： view_frames

输入 rosrun tf view_frames后可以看到最后一行提示生成了frames.pdf文件，默认放在当前终端的默认路径下

双击打开后如下，这就是当前系统中的tf坐标之间的位置关系。在当前的海龟仿真器当中共有三个坐标系，world,turtle1和turtle2，world是全局坐标系，表示整个仿真器的坐标原点(不会改变) 

（2）tf工具2：tf_ehco(命令行工具)

rosrun tf tf_echo turtle1 turtle2

可以直接去查询在树当中任意两个坐标系之间的关系 。并且位置信息会不断刷新

描述了turtle2坐标系如何通过变换变成到turtle1坐标系。里面的数据包含Translation和Rotation两部分，Translation就是平移，坐标系在xyz三个方向上的平移；Rotation就是旋转，分为三行，三行的数据内容是等价的，只不过是通过三种方式描述，Quaternion为四元数xyzw，RPY是分别围绕xyz轴的旋转，radian是通过弧度单位来描述，degree是通过角度来描述

 

（3）工具3：rviz（可视化工具） 

Rviz是一个三维监视平台，在ROS中用的非常多，后续会详细介绍

rosrun rviz rviz -d 'rospack find turtle_tf' /rviz/turtle_rviz.rviz

打开如下界面

将左上角Fixed Frame改成world

 

右下角add添加TF显示位置关系

效果如下。可以看到移动效果。turtle2和turtle1之间连着一根线，这根线就是向量，turtle2沿着向量走不断靠近turtle1，向量的坐标数据就是ehco工具打印出来的Translation。

turtle1不断移动，向量也不断变换，向量的长度直接算出来之后除以设置的时间就会得到速度，速度通过海龟的Twist消息发给turtle2，turtle2就可以移动了

十五，tf坐标系广播和监听的编程实现

1，创建功能包

1. cd ~/catkin_ws/src
2. catkin_create_pkg learning_tf roscpp rospy tf turtlesim

2，创建tf广播器代码(C++)

在之前运行起来的海龟例程，可以看到两只海龟和world之间有个位置关系，这个位置关系我们要通过tf来广播出来 

1. 定义tf广播器(TransformBroadcaster)
2. 创建坐标变换值
3. 发布坐标变换(sendTransform)

将下述代码放到/catkin_ws/src/learning_tf/src当中去 

/*** 该例程产生tf数据，并计算、发布turtle2的速度指令*/
/*tf/transform_broadcaster.h 头文件中定义了 tf::TransformBroadcaster 类，主要用于广播
（broadcast）变换信息。当你在一个 ROS 系统中，需要向 TF 系统发送某个坐标系相对于另一个坐标系的
变换信息时，就会使用 tf::TransformBroadcaster
假设你有一个移动机器人，它有一个底盘坐标系（base_link）和一个安装在底盘上的激光雷达坐标系
（laser_link）。为了让其他 ROS 节点知道激光雷达相对于底盘的位置和姿态，你可以使用 
tf::TransformBroadcaster 来广播从 base_link 到 laser_link 的变换。
当一个传感器（如摄像头）可以移动（例如安装在一个可旋转的云台上），并且其位置和姿态随时间变化时，你可以使用 tf::TransformBroadcaster 来持续更新该传感器坐标系相对于其父坐标系（例如机器人的手臂或底盘）的变换。*/
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>std::string turtle_name;void poseCallback(const turtlesim::Pose::ConstPtr& msg)//传入参数：海龟的位置，xy坐标和角度
{/* 创建tf的广播器。当使用 static 关键字时，tf::TransformBroadcaster 对象 br 的生命周期会延长到整个程序的运行期间。这意味着该对象只会被创建一次，而不是每次调用 poseCallback 函数时都创建一个新的 tf::TransformBroadcaster 对象。在 ROS 系统中，特别是在回调函数中，如果没有 static 关键字，每次消息到达并调用 poseCallback 函数时，都会创建一个新的 tf::TransformBroadcaster 对象。这会导致额外的资源消耗，包括内存分配和初始化成本，并且可能会引起性能问题，尤其是当消息到达频率较高时。*/static tf::TransformBroadcaster br;// 初始化tf数据。填充坐标系之间的映射关系//以turtle1为例，turtle1与world两个坐标系之间的位置关系包括平移和旋转，这两个数据组成//tf::Transform 是 tf 库中的一个类，用于表示三维空间中的变换，包括平移和旋转。//setOrigin 是 tf::Transform 类的一个方法，用于设置该变换的平移部分。//tf::Vector3 是 tf 库中的一个类，用于表示三维空间中的向量tf::Transform transform;transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );//设置平移参数，包括xyz的平移//该语句的整体功能是将 tf::Transform 对象 transform 的平移部分设置为一个新的位置，这个位置 //是由 msg->x、msg->y 和 0.0 组成的三维向量。tf::Quaternion q;//旋转。四元数Quaternionq.setRPY(0, 0, msg->theta);//设置旋转参数，围绕xyz轴的旋转transform.setRotation(q);//至此transform保存了平移和旋转的关系// 广播world与海龟坐标系之间的tf数据br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));//把位置关系广播出去，之后ROS后台tf树就会把这两个坐标系的关系给插入到树当中
//ros::Time::now()是tf里的时间戳，tf是有时间概念的默认保存10s，ros::Time::now()是当前时间
//"world" turtle_name是指广播的world和turtle_name之间的坐标关系
/*br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name)); 这一句是将海龟的当前位姿信息转换为 tf 变换，并将其广播到 tf 系统中，使其他 ROS 节点可以利用该变换信息，例如进行坐标转换、定位、导航等操作。
tf::StampedTransform 是 tf 库中的一个类，用于存储带有时间戳的变换信息。它包含了一个 tf::Transform 对象（表示平移和旋转）和一个时间戳，以及源坐标系和目标坐标系的名称。
在这个例子中，transform 是一个 tf::Transform 对象，它包含了具体的平移和旋转信息，由 transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) ) 和 tf::Quaternion q; q.setRPY(0, 0, msg->theta); transform.setRotation(q); 生成。
br.sendTransform 是 tf::TransformBroadcaster 类的一个方法，用于将 tf::StampedTransform 对象广播到 tf 系统中。当调用这个方法时，它将包含的变换信息（包括平移、旋转、时间戳以及源和目标坐标系）发送到 ROS 的 tf 系统中，使得其他 ROS 节点可以访问和使用这个变换信息。*/
}int main(int argc, char** argv)
{// 初始化ROS节点ros::init(argc, argv, "my_tf_broadcaster");// 输入参数作为海龟的名字/*if 判断用于检查程序启动时输入的命令行参数数量是否正确。在这个程序中，预期是有两个参数，第一 个参数是程序本身的名称（由操作系统自动提供），第二个参数应该是海龟的名字。所以 argc 的期望结果是 2。当 argc!= 2 时，意味着用户没有提供正确数量的参数。例如，如果用户仅仅运行程序而没有在命令行中输入海龟的名字作为参数，或者输入了过多或过少的参数，该 if 语句的条件将被满足。
当你在命令行中运行一个程序时，操作系统会将程序名称和用户输入的其他参数一起传递给程序。例如，假设你运行一个名为 my_program 的程序，你可能会这样输入：./my_program arg1 arg2。
在这个例子中，argc 的值将是 3。这是因为有三个元素被传递：
程序的名称：操作系统自动将程序名称作为第一个参数传递，对于上述例子，这个名称可能是 ./my_program。
第一个用户输入的参数：arg1。
第二个用户输入的参数：arg2。
如果你在命令行运行程序 ./my_tf_broadcaster turtle1，那么 argc 会是 2，argv[0] 是 ./my_tf_broadcaster ，argv[1] 是 turtle1。如果 argc 不是 2，程序会输出错误信息并终止，因为它没有接收到所需的海龟名称。所以将argv[1]赋给turtle_name。这样做可以用这一个函数获得turtle1或者turtle2和world的位置关系。但是任意的一个ROS节点不能重名，如果这个程序执行两遍获得了turtle1和turtle2相对于world的坐标位置，这就会导致节点名冲突，后面会通过重映射让节点名字不同，这样就可以执行两遍*/if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;}turtle_name = argv[1];// 订阅海龟的位姿话题ros::NodeHandle node;//通过句柄创建了一个Subscriber话题订阅者，订阅/* turtle_name+"/pose" 是要订阅的话题名称。这里使用 turtle_name 作为前缀，意味着话题名称会根据程序启动时提供的海龟名称来确定，例如，如果 turtle_name 是 "turtle1"，那么要订阅的话题就是 "turtle1/pose"。*/ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, poseCallback);// 循环等待回调函数ros::spin();return 0;
};

3，创建tf监听器代码(C++)

/*** 该例程监听tf数据，并计算、发布turtle2的速度指令*/#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h> //给海龟发送速度指令
#include <turtlesim/Spawn.h> //产生一只海龟int main(int argc, char** argv)
{// 初始化ROS节点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 中，ros::Rate rate(10.0); 和 ros::Rate loop_rate(10); 这两种用法在功能上基本相同，但有一些细微的区别：ros::Rate rate(10.0);：
这里使用浮点数 10.0 作为参数，明确表示期望的循环频率为 10.0Hz。
这种方式更具通用性，因为它可以设置非整数的频率，例如 ros::Rate rate(5.5); 表示 5.5Hz 的频率。
ros::Rate loop_rate(10);：
这里使用整数 10 作为参数，在这种情况下，它也表示 10Hz 的频率。
对于一些简单的情况，使用整数可能更简洁直观，特别是当频率是整数时。*/ros::Rate rate(10.0);while (ros::ok()){// 获取turtle1与turtle2坐标系之间的tf数据tf::StampedTransform transform;//保存平移和旋转的关系try{   //通过tf的listener来监听获取两个坐标系之间的关系listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));//等待。如果系统中存在turtle1和turtle2两个坐标系这段代码才会跳过去//ros::Duration(3.0)指如果等待超过3s，就会提示错误。ros::Time(0)指查询当前时间//如果存在就通过下述listener方法查询这两个坐标系之间的关系，结果保存在transformlistener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);//try...catch与Python的try...expect语法类似，是 C++ 中的异常处理机制。它允许程序尝试执行一 //段可能会抛出异常的代码（在 try 块中），并在异常发生时进行相应的处理（在 catch 块中）。}catch (tf::TransformException &ex)//当 try 块中的操作抛出 tf::TransformException 异常时，会进入 catch 块。{    //打印异常信息，使用 ROS_ERROR 函数将异常信息输出到 ROS 的错误日志中，便于调试和问题排查。ROS_ERROR("%s",ex.what());//让节点休眠 1 秒，以避免频繁的异常检查，减少资源消耗。ros::Duration(1.0).sleep();//直接跳转到 while 循环的下一次迭代，而不是继续执行后续代码，防止在出现异常时执行错误的操作。continue;}// 根据turtle1与turtle2坐标系之间的位置关系，发布turtle2的速度控制指令geometry_msgs::Twist vel_msg;//transform.getOrigin().x() 和 transform.getOrigin().y() 从 tf::StampedTransform  //对象 transform 中获取 turtle1 相对于 turtle2 的位置的 x 和 y 坐标。//atan2 是一个数学函数，它计算 y/x 的反正切值，范围在 -pi 到 pi 之间//通过 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x())，根据 //turtle1 相对于 turtle2 的位置计算 turtle2 的角速度。这里的 4.0 是一个比例因子，可根//据需要调整，用于调整旋转速度。vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),transform.getOrigin().x());//pow(transform.getOrigin().x(), 2) 和 pow(transform.getOrigin().y(), 2) 分别计算 //x 和 y 坐标的平方。//sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2)) 计算//turtle1 相对于 turtle2 位置的欧几里得距离（即直线距离）。//0.5 * sqrt(pow(transform.getOrigin().x(), 2) + //pow(transform.getOrigin().y(),2)) 计算 turtle2 的线速度，这里的 0.5 是一个比例因 //子，可根据需要调整，用于调整直线运动速度。vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +pow(transform.getOrigin().y(), 2));turtle_vel.publish(vel_msg);rate.sleep();}return 0;
};

 4，配置代码编译规则

add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)

target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})

add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)

target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})

5，编译并运行

这里运行不用launch，而是将它打散成很多节点单独来运行。同时将一个程序同样名字的节点通过重映射给变成两个名字

1. cd ~/catkin_ws
2. catkin_make
3. source devel/setup.bash
4. roscore
5. rosrun turtlesim turtlesim_node  启动海龟仿真器
6. rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1  /turtle1就是给main函数输入的参数。__name:=turtle1_tf_broadcaster是ROS的重映射机制，重新命名rosrun learning_tf turtle_tf_broadcaster：运行 learning_tf 软件包中的 turtle_tf_broadcaster 可执行文件。__name:=turtle1_tf_broadcaster：将运行的节点重命名为 turtle1_tf_broadcaster。/turtle1：这是传递给节点的参数，会在节点内部被处理，例如在上述代码中，turtle_name = argv[1]; 会将 /turtle1 作为 turtle_name，用于订阅 /turtle1/pose 话题，当你运行这个命令时，/turtle1 会被传递给 turtle_tf_broadcaster 节点的 main 函数中的 argv 参数。
7. rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2
8. rosrun learning_tf turtle_tf_listener 查询turtle1和turtle2两个坐标系之间的关系，并且给turtle2发布速度指令
9. rosrun turtlesim turtle_teleop_key 通过键盘控制海龟1

6，Python版本

 turtle_tf_listener.py

# 该例程将请求/show_person服务，服务数据类型learning_service::Personimport roslib
roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srvif __name__ == '__main__':rospy.init_node('turtle_tf_listener')listener = tf.TransformListener()rospy.wait_for_service('spawn')spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)spawner(4, 2, 0, 'turtle2')turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)rate = rospy.Rate(10.0)while not rospy.is_shutdown():try:(trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):continueangular = 4 * math.atan2(trans[1], trans[0])linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)cmd = geometry_msgs.msg.Twist()cmd.linear.x = linearcmd.angular.z = angularturtle_vel.publish(cmd)rate.sleep()

turtle_tf_broadcaster.py

# 该例程将请求/show_person服务，服务数据类型learning_service::Personimport roslib
roslib.load_manifest('learning_tf')
import rospyimport tf
import turtlesim.msgdef handle_turtle_pose(msg, turtlename):br = tf.TransformBroadcaster()br.sendTransform((msg.x, msg.y, 0),tf.transformations.quaternion_from_euler(0, 0, msg.theta),rospy.Time.now(),turtlename,"world")if __name__ == '__main__':rospy.init_node('turtle_tf_broadcaster')turtlename = rospy.get_param('~turtle')rospy.Subscriber('/%s/pose' % turtlename,turtlesim.msg.Pose,handle_turtle_pose,turtlename)rospy.spin()

十六，launch启动文件的使用方法

1，launch文件介绍

 在上一节中我们使用了很多终端和命令来完成两只海龟的跟随实验，在ROS中可以通过launch文件快速启动指令和节点。一个launch文件就可以启动机器人里面所有的功能了

Launch文件是通过XML文件实现多节点的配置和启动，可自动启动ROS Master。可以检测当前系统中是否有ROS Master启动，如果有的话就不会再启动，如果没有就会自动启动ROS Master

下图即为launch文件的内容，每一个node都是一个节点，都相当于一个rosrun命令

 2，launch文件常用语法

更多更详细的语法使用:http://wiki.ros.org/roslaunch/XML

（1）<launch>

launch文件里的所有内容都是通过xml这种标签来做描述的，所有的launch文件都必须包含一个根标签(根元素)<launch>。下图是一个最简单的launch文件，<launch>是启动，</launch>是结束，这两个launch之间包含的内容是节点启动的内容

（2）<node> 

<node>标签是启动节点 

<node pkg="package-name" type="executable-name" name="node-name"/>

• pkg:节点所在的功能包名称
• type:节点的可执行文件名称
• name:节点运行时的名称。这会取代代码文件里定义的node名字
• 以下属性为可选属性
• output:控制节点是否要将日志信息打印到终端
• respawn:如果节点未启动是否要重启
• required:是否要求某个节点必须要启动
• ns:namespace，可以给每个节点去做命名空间，避免命名冲突
• args:给每个节点输入参数来使用

（3）<param>/<rosparam>

设置ROS系统运行时的参数，存储在参数服务器里。前面我们学过可以用命令行或者程序来设置一些参数到参数服务器里面，通过launch文件也是可以加载参数的。rosparam标签可以将将整个参数文件全部load到ROS的参数服务器里来；param标签可以用来保存某一个参数

（4）<arg>

<arg>表示在launch文件内部的局部变量，仅限于launch文件使用 

如下图所示，用$(arg arg-name)来使用，将arg-name的arg-value赋给foo的value

（5） <remap>重映射

将ROS里原来命名的某一个资源重新换一个名字，换完名字后原来的名字就不存在了

（6）<include>嵌套

3，Launch示例

（1）示例1

 simple.launch是最简单的launch文件，只启动两个节点。out="screen"可以在终端中显示日志信息，如果不想打印可以直接删掉output

<launch><node pkg="learning_topic" type="person_subscriber" name="talker" output="screen" /><node pkg="learning_topic" type="person_publisher" name="listener" output="screen" /> 
</launch>

在catkin_ws/src路径下新建功能包learning_launch。catkin_create_pkg learning_launch。这个功能包不需要其他的依赖，因为launch文件本身是一个系统文件，基本都是链接其他库里的功能的。

里面是没有src和include文件的 

一般在功能包创建launch文件时会先创建launch文件夹，这样比较方便管理功能包的资源的

将simple.launch拷贝到launch文件夹中

 打开simple.launch，修改文本显示的语法，改为XML

回到工作空间根目录catkin_ws下，输入catkin_make编译

在终端下输入roslaunch learning_launch simple.launch即可启动simple文件 

（2）示例2

同理运行turtlesim_parameter_config.launch

turtlesim_parameter_config.launch文件配置了一些parameter  

<launch><param name="/turtle_number"   value="2"/><node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node"><param name="turtle_name1"   value="Tom"/><param name="turtle_name2"   value="Jerry"/><rosparam file="$(find learning_launch)/config/param.yaml" command="load"/></node><node pkg="turtlesim" type="turtle_teleop_key" name="turtle_teleop_key" output="screen"/></launch>

<rosparam file="$(find learning_launch)/config/param.yaml" command="load"/>：find是系统指令，会查找learning_launch功能包，找到功能包后会找功能包下config文件夹下的param.yaml文件。$(find learning_launch)是一个搜索功能包的指令会直接输出完整的路径。

    <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node">
        <param name="turtle_name1"   value="Tom"/>
        <param name="turtle_name2"   value="Jerry"/>将param写到node节点下与写到全局下的区别就是，写到node节点下输出参数是会在其前面加上节点名字的前缀作为命名空间，如下图。

下图是param.yaml文件的内容，参考上图可以看到给C和D加了group命名空间 。还可以在group后面再加命名空间。这都是为了防止资源冲突

 （3）示例3

完成launch文件启动广播和监听

 <launch><!-- Turtlesim Node--><node pkg="turtlesim" type="turtlesim_node" name="sim"/><node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/><node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" /><node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" /><node pkg="learning_tf" type="turtle_tf_listener" name="listener" /></launch>

python版本

<launch><!-- Turtlesim Node--><node pkg="turtlesim" type="turtlesim_node" name="sim"/><node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/><node name="turtle1_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py"><param name="turtle" type="string" value="turtle1" /></node><node name="turtle2_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py"><param name="turtle" type="string" value="turtle2" /> </node><node pkg="learning_tf" type="turtle_tf_listener.py" name="listener" /></launch>

（4）示例4

<include>和<remap>使用示例

 turtlesim_remap.launch

<launch><include file="$(find learning_launch)/launch/simple.launch" /><node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node"><remap from="/turtle1/cmd_vel" to="/cmd_vel"/></node></launch>

可以看到/turtle1/cmd_vel变成了/cmd_vel 

十七，常见可视化工具的使用

1，Qt工具箱

可以使用一系列rqt开头的命令行来启动

除此之外，还有下图所示工具 

 

（1）工具1：rqt_console 

以海龟为例

1. roscore
2. rosrun turtlesim turtlesim_node 启动海龟仿真器
3. rqt_graph之前介绍过
4. rqt_console ，打开界面如下

共分为三个窗口，最上面的窗口用来显示ROS系统里的日志的输出信息 ，包括一些info，warning，error等等

运行rosrun turtlesim turtle_teleop_key让海龟动起来。海龟撞墙之后会发布警告。发布的警告和终端里的一样，只不过这个工具会把error warning等进行整合并且可以在里面进行筛选

 （2）工具2：rqt_plot 

可以用来绘制数据曲线。打开后如下所示

可以在Topic中订阅具体的某个数据，通过数据会自动绘制曲线。

比如订阅海龟位置信息。输入/会自动匹配ROS当中的话题然后列出来。

这里选择/turtle1/pose，然后回车。可以看到曲线更新了

（3）工具3：rqt_image_view

这个是用来显示摄像头图像的。默认是没有图像的，需要插入摄像头并驱动起来，摄像头驱动之后在左上角选择图像信息的话题，然后此工具就可以把图像渲染出来

 

（4）工具4：rqt

这个工具是一个比较综合的工具，打开后如下。里面集成了ROS里面所有的rqt工具，在菜单栏的Plugins里就会找到一系列的工具

 

2，Rviz

（1）Rviz简介

在机器人开发当中我们都会涉及到很多数据的显示，比如看到的机器人模型，它的坐标系，运动规划的效果，导航地图路径，图像，点云等等。Rviz也是一个综合的机器人显示平台，ROS给我们提供了很多默认的显示插件，比如图像，点云，机器人模型等，这些在ROS里都已经标准定义好了，我们只要按照ROS的数据结构发布，都可以在窗口里做显示。如果在里面发现某个数据(比如气体液体等等)没有定义，我们也可以基于它的平台自己来开发一个插件做显示。     

Rviz是一款三维可视化工具，可以很好的兼容基于ROS软件框架的机器人平台

• 在rviz中，可以使用可扩展标记语言XML对机器人,周围物体等任何实物进行尺寸，质量，位置，材质，关节等属性的描述，并且在界面中呈现出来
• 同时，rviz还可以通过图形化的方式，实时显示机器人传感器的信息，机器人的运动状态，周围环境的变化等信息
• 总而言之，rviz通过机器人模型参数，机器人发布的传感器信息等数据，为用户进行所有可监测信息的图形化显示。用户和开发者也可以在rviz控制界面下，通过按钮，滑动条，数值等方式控制机器人的行为

（2）Rviz界面介绍

0区域显示模型，图像，点云等。3区域是用来显示视角的，我们可以通过区域上面的选项来调整观看模型的视角，俯视图，仰视图，左视图等等。1区域为工具栏，可以选择rviz给我们提供的工具，比如第一个interact就是类似指针的工具可以拖拽显示区，还有设置机器人导航点，目标点等工具。4为时间显示区，用来显示ROS启动的时间以及系统本身默认的时间。2区域用的最多，为显示项列表，比如想去显示摄像机图像，就要通过左下角Add来添加图像显示插件，再去选择对应的图像的话题，确定之后就可以在0区域显示图像；机器人模型，点云，地图同理都是在Add添加对应显示项。

（3）Rviz使用基本流程

1. roscore
2. rosrun rviz rviz 回车后启动rviz 

比如要显示图像，点击左下角Add。并找到Image，点击ok

即可出现以下界面

打开Image插件列表。在Image Topic中设置插件要订阅的话题(图像数据在ROS里面是以话题形式发布的)

 

如果我们有摄像头驱动的话，在Image Topic位置就会弹出对应的话题名，我们就可以选择它。然后就会在红框位置显示摄像头实时图像

 3，Gazebo

（1）Gazebo简介

Gazebo是一款功能强大的三维物理仿真平台

• 具有强大的物理引擎
• 高质量的图形渲染
• 方便的编程与图形接口
• 开源免费

我们可以在里面仿真一个机器人，包括摄像头，雷达，电机等等 

 

（2）Gazebo界面介绍

1区域为基本的配置区，可以调整一些光线，模型的位置等。2区域是模型列表，World里面列了这个仿真环境里有哪些模型机器人，大地，太阳，障碍物等等；Insert可以插入模型库当中的一些模型

（3）Gazebo使用基本流程

 roslaunch gazebo_ros

可以看到里面有一系列仿真环境，默认的是empty_world.launch空环境

选择一项运行即可 

后续文章会对Rviz和Gazebo详细使用流程

完结