第3章 编写ROS程序-2

1、发布者程序

在本节中，我们将看到如何发送随机生成的速度指令到一个turtlesim海龟，使它漫无目的地巡游。这个程序的源文件称为pubvel，这个程序展示了从代码中发布消息涉及的所有要素。

其代码如下：

pubvel和hello程序主要的区别都是由于发布消息的需求导致的。

包含消息类型声明     每一个 ROS 话题都与一个消息类型相关联，每一个消息类型都有一个相对应 C++头文件。你需要在你的程序中为每一个用到的消息类型包含这个头文件，代码如下所示：

#include <package_name/type_name.h>

这里应该要注意的是，头文件中的功能包名是定义消息类型的包的名称，而不一定是你自己的包的名称。

在 pubvel 程序中，我们想发布一条类型为 geometry_msgs/Twist 的消息 （名为 geometry_msgs 的包所拥有的类型为 Twist 的消息） ，我们应该这样：

#include <geometry_msgs/Twist.h>

这个头文件的目的是定义一个 C++类，此类和给定的消息类型含有相同的数据类型成员。

这个类定义在以包命名的域名空间中。

这样命名的实际影响是当引用 C++代码中的消息类时，你将会使用双分号（::）来区分开包名和类型名，双分号也称为范围解析运算符。

在我们的 pubvel 例程中，头文件定义了一个名为geometry_msgs::Twist 的类。

创建发布者对象     发布消息的实际工作是由类名为ros::Publisher的一个对象来完成的。类似下面这行的代码创建了我们需要的对象：

ros::Publisher pub = node_handle.advertise<message_type>( topic_name, queue_size);

解析：

---node_handle 是 ros::NodeHandle 类的一个对象，是你在程序的开始处创建的。我们将调用这个对象的 advertise 方法。

---在尖括号中的 message_type 部分，其正式名称为模板参数，是我们要发布的消息的数据类型。这个应该是上面讨论过的头 文 件 中 定 义 的 类 名 。 在 例 程 中 ， 我 们 使 用geometry_msgs::Twist 类。

---topic_name 是一个字符串，它包含了我们想发布的话题的名称。 它应该和 rostopic list 或者 rqt_graph 中展示的话题名称一致，但通常没有前斜杠（/） 。我们丢掉前斜杠使话题名为一个相对名称；第 5 章解释了相对名称的机制和目的。在此例程中，话题名为turtle1/cmd_vel。

---advertise 最后的参数是一个整数，表示这个发布者发布的消息序列的大小。在大多数情况下，一个相对比较大的值，比
方说 1000，是合适的。

如果你想从同一个节点发布关于多个话题的消息，你需要为每个话题创建一个独立的 ros::Publisher 对象。

创建并填充消息对象     当我们创建 ros::Publisher 对象时已经引用了消息类。对于消息类型的每个域，这个类都有一个可公共访问的数据成员。

当然， 大多数消息类型含有float64 类型之外的域。 幸运的是，从ROS域类型到C++数据类型的映射正如你所期望的方式工作。一个可能未被注意到的事实是，数组类型（在rosmsg中用方括号表示）在C++代码中是通过STL向量实现的。

发布消息    在所有的前期工作完成后，使用 ros::Publisher 对象的publish 方法可以很简单地发布消息。例如下面所示：
　　　　pub.publish(msg);

这个方法将所给的消息添加到发布者的输出消息队列中，从这里，它会尽快被发送到相同话题的订阅者那里。

定义输出格式    表3.4中的ROS_INFO_STREAM行尽管和发布速度命令不是直接相关的，但还是值得一看的。这是关于宏ROS_INFO_STREAM 可以做什么的一个更加完整的例证， 因为它演示了在输出中除了插入字符串还可以插入其他数据的能力。

2、消息发布循环

程序在这个循环中使用了两个附加的构造函数。

节点是否停止工作的检查     pubvel 的 while 循环的条件是： ros::ok()

---你可能对节点使用了 rosnode kill 命令。

---你可能给程序发送了一个终止信号（Ctrl-C）。

---你可能在程序的某个位置调用了 ros::shutdown() ，这个函数是在你的代码中发送节点工作已经完成信号的一个很有用的方法。

---你可能以相同的名字启动了其他节点，经常是因为你启动了一个相同程序的新实例。

控制消息发布频率     pubvel的最后一个新知识点是它使用了ros::Rate对象：

ros::Rate rate(2);

这个对象控制循环运行速度，其构造函数中的参数以赫兹（Hz）为单位，即每秒钟的循环数。这个例子创建了旨在规范每秒钟执行两个迭代循环的速率对象。邻近每次循环迭代的结尾，我们调用此对象的 sleep 方法： rate.sleep();

你可以使用 rostopic hz 命令来确认这种调控是否工作正常。
对于 pubvel 程序，输出结果类似于：
average rate: 2.000

min: 0.500s max: 0.500s std dev: 0.00006s window: 10

我们看到消息以每秒 2 条的速率发布，且时间上偏差非常小。

3、编译pubvel

适当修改 CMakeLists.txt 和 package.xml， 然后用 catkin_make 来编译你的工作区。然而，它和 hello 例程有一个很重要的区别。

声明消息类型依赖库     因为pubvel使用了来自geometry_msgs包的消息类型，我们必须声明对这个包的依赖关系，这和 3.2.2 节中讨论的 roscpp 依赖库的形式相同。具体而言，除了 roscpp，我们 必 须 修 改 CMakeLists.txt 文 件 的 find_package 行 来 声 明
geometry_msgs： find_package(catkin REQUIRED COMPONENTS roscpp geometry_msgs)

注意： 我们是修改已有的 find_package 行， 而不是新建一行。

在 package.xml 文件中，我们添加新的依赖项：
<build_depend>geometry_msgs</build_depend>
<run_depend>geometry_msgs</run_depend>

别忘了，还有跟hello.cpp程序里类似的修改，一样要添加进去。

回到工作空间编译catkin_make

4、执行 pubvel