Rodney - 迟来的自主机器人（第一部分）
4.98/5 (33投票s)
关于 ROS（机器人操作系统）家用机器人系列的第一部分
引言
罗德尼机器人项目是一个业余机器人项目，旨在设计和构建一个自主的家庭机器人。本文是描述该系列项目的第一篇。在这一部分中，我将介绍概念，选择单板计算机，安装 ROS（机器人操作系统）并编写控制软件的第一部分。
背景
早在20世纪70年代末80年代初，我购买了以下书籍：《如何构建自己的自编程机器人》作者David L. Heiserman和《如何构建计算机控制的机器人》作者Tod Loofbourrow。我的计划是基于Z80处理器构建自己的处理器板，然后围绕这块板构建机器人。它从未真正实现。这么多年后，随着像Raspberry Pi和Arduino这样现代的小型板的出现，让家庭机器人启动并运行的任务变得容易得多，尽管人们对它的能力期望也更高。
这两本书都给它们的机器人命名为“Rodney”和“Mike”。因此，为了纪念其中一本书，计划中的机器人被命名为Rodney。
本网站上还有两个机器人项目，我从中获得了一些启发。
第一篇文章让我们建造一个机器人！是一篇机器人概念文章，包含一些很棒的想法，尽管从文章中我不确定机器人是否真的建造成功了？使用一个小型显示器作为机器人头部是我从文章中获得的主要想法之一，但我在本文中发现最有用的事情之一是链接到Pi Robot博客。这让我接触到了机器人操作系统（ROS）。这是机器人编程的事实标准。正如ROS维基上所说：
ROS（机器人操作系统）提供库和工具，帮助软件开发人员创建机器人应用程序。它提供硬件抽象、设备驱动程序、库、可视化工具、消息传递、包管理等。ROS采用开源BSD许可。
它并不是一个真正的操作系统，更像是一个中间件，旨在用于 Linux 平台（它在 Ubuntu 下运行最佳）。有大量的开源 ROS 代码可用于传感器，您可以将这些库放入您的项目中，从而专注于机器人应用程序。ROS Wiki 充满了大量有用的信息，如果您不熟悉 ROS，请前往那里查看。
我从中获得灵感的第二篇文章是PiRex – 遥控树莓派机器人。尽管不像第一篇文章那么雄心勃勃，但本文是围绕一个已完成的机器人项目编写的。两篇文章都使用了相对便宜的树莓派。
因此，与80年代初不同，罗德尼将围绕一块带有1GB RAM的Raspberry Pi 3 Model B进行构建，而不是首先构建一个处理器板。
ROS 和 树莓派
在本文中，我将解释如何在项目中使用ROS，并描述如何使用ROS中可用的一些工具来测试我的代码。这不是一个ROS教程，网上有很多教程比我做得更好。我会不时地在ROS维基上提供相关教程的链接，但现在，为了帮助初次阅读本文的读者或偶然阅读的读者，这里有一些您可能会觉得有用的ROS术语。
- 它是一个分布式系统，机器人代码可以在通过网络通信的多台机器上运行。
- 一个 **节点** 是一个单一用途的可执行文件。
- 节点被组织成 **包**，这是一个用于文件夹和文件集合的术语。
- 节点可以使用多种语言编写。在这个项目中，我们将使用 C++ 和 Python。
- 节点之间通过 **话题（Topics）** 进行通信，话题是单向数据流。
- 话题是 **消息（Messages）** 的实例。消息是数据结构。
- ROS提供标准消息，您也可以创建用户定义消息。
- 节点之间也可以使用 **服务** 进行通信，这是一种服务器/客户端阻塞协议。
- 节点还可以使用 **动作（Actions）** 进行通信，这是一种非阻塞的面向目标的任务协议。
- 存在一个主节点，roscore，所有其他节点都向它注册。即使使用分布式系统，也只存在一个主节点。
- 它使用一个 catkin 构建系统。
- 单个节点可以使用 `rosrun` 命令运行，或者您可以使用启动工具从同一个命令终端启动多个节点。
- 它包含一个 **参数服务器**。节点可以在运行时存储和检索参数。
- 它包含用于检查系统的各种工具，甚至可以模拟机器人硬件。
这篇Code Project 文章对 ROS 有一个很好的概述。
因此，在决定使用 Raspberry Pi 3 作为主处理器并使用 ROS 后，第一步是在 Pi 上安装 ROS。
下载和安装 ROS 的说明可在此处获取，但为了简化操作，我将使用一个包含 ROS 的 Raspberry Pi Ubuntu 镜像。您可以从 Ubiquity Robotics 网站免费下载该镜像。此镜像中包含的 ROS 版本是 Kinetic 版本。该镜像还包括一些有用的 ROS 包，例如用于访问 Raspberry Pi 摄像头的包 raspicam_node。如果您更喜欢为您的 Raspberry Pi 使用不同的镜像并自行安装 ROS，您仍然可以通过从他们的 GitHub 站点下载代码来使用 Ubiquity 的包。
我还打算在罗德尼项目中使用其他 Raspberry Pi 外设：
- 7英寸触摸屏显示器
- 摄像头模块V2
该计划是使用显示器向用户传递状态信息、网络内容，并显示动画机器人面部。摄像头将是机器人的眼睛，最初用于面部识别。
以下图片展示了带有 Raspberry Pi 和摄像头安装在屏幕背面的 7 英寸显示器。摄像头使用 3D 打印支架安装。支架的 _stl_ 文件包含在本文附带的 3D 打印 zip 文件中。
由于 ROS 系统可以在分布式网络上运行，我还将 ROS 安装在 Ubuntu 桌面版上。这台台式电脑将用于开发系统的节点，运行一些可用于测试代码的 ROS 工具，以及运行仿真。
机器人任务
为了提出项目的需求，我将指定一些我希望罗德尼能够执行的“任务”。在文章“让我们建造一个机器人！”中，作者列出了他希望机器人能在家中完成的工作。其中一项工作是：
“给……带个消息——既然机器人将（拥有）识别家庭成员的能力，那么让它成为‘消息传递者和提醒者’怎么样？我可以说‘机器人，提醒（人名）晚上6点去车站接我’。然后，即使那个家庭成员的手机静音，或者正在听响亮的音乐，或者（插入任何不来车站接我的理由），机器人也可以在房子里漫游，找到那个人，并把消息告诉他们。”
这听起来像是一个很好的起点，也将成为我们的第一个任务。不过，我要稍微修改一下：如果您可以通过网络浏览器访问 Rodney 来控制和设置任务，那会怎样？
让我们将“传递消息给…”任务分解成几个可以单独完成的较小设计目标。此任务的设计目标将是：
- 设计目标1：能够使用摄像头环顾四周，搜索人脸，尝试识别看到的人，并为任何被识别的人显示消息
- 设计目标2：面部表情和语音合成。Rodney需要能够传达消息
- 设计目标3：通过远程键盘和/或游戏手柄控制移动
- 设计目标4：增加激光测距仪或类似的测距传感器以辅助导航
- 设计目标5：自主移动
- 设计目标6：任务分配和完成通知
这对于一个看似简单的机器人任务来说，需要完成的事情可真不少。
任务1，设计目标1
要实现此设计目标，我们需要：
- 使用 RC 舵机控制头部/摄像头进行平移/倾斜运动
- 从树莓派摄像头访问图像
- 检测和识别面部
- 控制这些动作的顺序
在本文的剩余部分，我将重点关注头部/摄像头的平移/倾斜控制。
为了控制头部/摄像头，我们需要一个平移和倾斜装置，这将需要两个遥控舵机。我还将包括第二个平移/倾斜装置以备将来扩展。因此，我们立即需要四个 PWM 输出以控制舵机。Raspberry Pi 只有一个硬件 PWM，尽管我们可以使用软件 PWM，但我将通过将舵机的控制权交给第二块板来避免这种开销。
我们可以使用专门的电路板，例如 PiBorg 的 UltraBorg。使用这块板，您可以通过 I2C 总线将最多四个舵机和四个 HC-SR04 超声波设备连接到 Raspberry Pi。然而，由于我手头有一些来自之前项目的 Arduino Nano，我将利用其中一个。
这也将是我们利用 ROS 社区已完成工作的众多例子中的第一个，它使我们能够专注于机器人应用程序。为了连接到将在 Arduino 上运行的类似 ROS 的节点，我们将使用一个包含用于通过串口与 Arduino 通信的节点以及用于 Arduino 草图的 Arduino 库的软件包。该软件包的文档可在 ROS Wiki 网站 rosserial_arduino 上查阅。
要使用此软件包，我们需要将其安装到 ROS 目标上，并将库安装到 Arduino IDE 环境中。如果使用任何用户定义的 ROS 消息（我们会的），我们还需要重新构建 Arduino 库。关于如何做到这一点以及更多内容，请参阅 rosserial arduino 教程。
为了控制构成云台设备的每个舵机位置，我们将编写一个ROS包，其节点将接收云台需求消息并将其转换为发送到Arduino的单独位置消息。第一条消息将识别要移动的关节和所需位置。发送到Arduino的第二条消息将包含一个索引值，指示要移动四个舵机中的哪一个，以及该舵机移动到的角度值。将此功能拆分意味着Arduino草图只需要了解舵机，而无需了解云台设备，因此它可以重复用于其他舵机应用程序。顺便说一下，在Arduino编程领域，在Arduino上运行的代码被称为草图，我将继续在这里使用这个术语。如果您不熟悉Arduino编程，Code Project网站上有很多关于Arduino的文章。
对于识别关节位置的第一条消息，我们将使用 ROS 预定义消息 sensor_msgs/JointState。此消息类型的文档可以在这里找到。现在，根据 ROS 标准，位置单位是弧度，因此我们的节点还需要将位置转换为 Arduino 所需的度数。该消息还包含我们不会使用的许多字段。使用此消息类型可能看起来有些多余，但遵循 ROS 标准并使用现有消息类型意味着我们可以在项目后期利用一些非常有用的 ROS 工具。
第二条消息将识别要移动的舵机以及以度为单位的角度，它将是一个用户定义的消息，因为我们不希望 Arduino 草图中出现任何不必要的开销。
我们可以将用户定义的消息定义包含在云台包中，但同样为了重用，我们将为消息定义创建一个单独的包。
因此，为了完成平移/倾斜功能，我们将编写两个 ROS 包和一个 ROS 风格的 Arduino 草图。
我们将第一个包命名为 `servo_msgs`，它将定义我们的用户自定义消息。构建时，它将生成供 C++ 代码使用的 `.h` 文件，并自动创建 Python 脚本。我们还将重新编译 Arduino 库以生成供我们的草图使用的 `.h` 文件。
构成此第一个包的文件位于 _servo_msgs_ 文件夹中。此文件夹的根目录包含一个记录该包的 readme 文件，以及 ROS 包中始终需要存在的两个文件。它们是 _CmakeList.txt_ 和 _package.xml_ 文件，有关这些文件的信息可以在创建 ROS 包教程中找到。
包中的 `msg` 文件夹包含我们消息的定义文件 `servo_array.msg`。
# index references the servo that the angle is for, e.g. 0, 1, 2 or 3
# angle is the angle to set the servo to
uint8 index
uint16 angle
你可以把它想象成一个 C 语言的结构体。这是将作为 ROS 话题发送给 Arduino 的消息。该消息包含两个元素，`index` 指示要移动哪个舵机，`angle` 是以度为单位的舵机移动角度。
这样就完成了我们第一个简单的 ROS 包，我们的第二个包是 `pan_tilt` 包。这个包位于 `pan_tilt` 文件夹中，并包含将构成 `pan_tilt_node` 的可执行代码。
这个包的根文件夹再次包含了文档文件，以及 _CmakeList.txt_ 和 _package.xml_ 文件。这个包包含了一些子文件夹，我将简要描述一下。_config_ 文件夹包含文件 _config.yaml_。这个文件将由启动文件（见下文）用于在参数服务器中设置给定参数。这将使我们能够在不重新编译代码的情况下配置系统。
# Configuration for pan/tilt devices
# In Rodney index0 is for the head and index 1 is spare
servo:
index0:
pan:
servo: 0
joint_name: 'head_pan'
tilt:
servo: 1
flip_rotation: true
max: 0.349066
min: -1.39626
joint_name: 'head_tilt'
index1:
pan:
servo: 2
tilt:
servo: 3
在此配置文件中，`index0` 给出了头部平移和倾斜设备的参数，`index1` 给出了第二个平移和倾斜设备的参数。参数如下：
servo标识哪个舵机负责该关节joint_name标识此关节在 `joint_state` 消息中将被调用的名称flip_rotation见下文max和min以弧度为单位给出，用于限制关节的行程
ROS 约定是遵守右手定则，因此关节的值将围绕正轴逆时针方向增加。现在在 Rodney 的构建中，头部倾斜舵机以一种遵循左手定则的方式安装。通过将参数 `flip_rotation` 设置为 `true`，我们的系统可以遵循该约定，但 _pan_tilt_node_ 可以确保将正确的方向值传递给 Arduino，以用于该特定舵机。
_cfg_ 文件夹包含文件 _pan_tilt.cfg_。此文件由动态重新配置服务器使用，以便我们可以在运行时调整舵机的微调。如您所见，该文件实际上是一个 Python 脚本。
#!/usr/bin/env python
PACKAGE = "pan_tilt"
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
gen.add("index0_pan_trim", int_t, 0, "Index 0 - Pan Trim", 0, -45, 45)
gen.add("index0_tilt_trim", int_t, 0, "Index 0 - Tilt Trim", 0, -45, 45)
gen.add("index1_pan_trim", int_t, 0, "Index 1 - Pan Trim", 0, -45, 45)
gen.add("index1_tilt_trim", int_t, 0, "Index 1 - Tilt Trim", 0, -45, 45)
exit(gen.generate(PACKAGE, "pan_tilt_node", "PanTilt"))
要完整理解动态重新配置服务器，请参阅 ROS Wiki 动态重新配置部分。目前，在我们的文件中，您可以看到我们添加了四个参数，每个舵机一个，并且每个参数的默认值为零，最小值为 -45，最大值为 45。
_launch_ 文件夹包含启动文件，它不仅使我们能够加载配置文件，还能启动构成系统的所有节点。在我们的文件夹中，有一个 _pan_tilt_test.launch_ 文件，用于测试 Rodney 系统的平移/倾斜部分。如您所见，这是一个 XML 格式的文件。
<?xml version="1.0" ?>
<launch>
<rosparam command="load" file="$(find pan_tilt)/config/config.yaml" />
<node pkg="pan_tilt" type="pan_tilt_node" name="pan_tilt_node" output="screen" />
<node pkg="rosserial_python" type="serial_node.py" name="serial_node"
output="screen" args="/dev/ttyUSB0" />
</launch>
要完整理解启动文件，请参阅 ROS Wiki 关于启动文件的部分。我们的启动文件首先查找并加载我们的配置文件。
<rosparam command="load" file="$(find pan_tilt)/config/config.yaml" />
下一个标签将导致我们的 _pan_tilt_node_ 被执行。请注意，`output="screen"` 会将任何日志消息定向到我们启动的终端。
<node pkg="pan_tilt" type="pan_tilt_node" name="pan_tilt_node" output="screen" />
文件中的最后一个标签将导致运行与 Arduino 通信的 rosserial 节点。您可以看到选择连接到 Arduino 的串口的参数，`args="/dev/ttyUSB0"`。
<node pkg="rosserial_python" type="serial_node.py" name="serial_node"
output="screen" args="/dev/ttyUSB0" />
剩余的文件夹（_include_ 和 _src_）包含该包的 C++ 代码。对于这个包，我们有一个 C++ 类 `PanTiltNode` 和一个包含在 _pan_tilt_node.cpp_ 文件中的主例程。
主例程通知 ROS 我们的节点，创建包含节点代码的类实例，将回调函数传递给动态重新配置服务器，并将控制权交给 ROS spin，后者将处理传入和传出的话题。
int main(int argc, char **argv)
{
ros::init(argc, argv, "pan_tilt_node");
PanTiltNode *pan_tiltnode = new PanTiltNode();
dynamic_reconfigure::Server<pan_tilt::PanTiltConfig> server;
dynamic_reconfigure::Server<pan_tilt::PanTiltConfig>::CallbackType f;
f = boost::bind(&PanTiltNode::reconfCallback, pan_tiltnode, _1, _2);
server.setCallback(f);
std::string node_name = ros::this_node::getName();
ROS_INFO("%s started", node_name.c_str());
ros::spin();
return 0;
}
我们类的构造函数从配置文件的参数服务器中加载参数。
// Constructor
PanTiltNode::PanTiltNode()
{
double max_radians;
double min_radians;
int temp;
/* Get any parameters from server which will not change after startup.
* Defaults used if parameter is not in the parameter server
*/
// Which servo is used for what
n_.param("/servo/index0/pan/servo", pan_servo_[0], 0);
n_.param("/servo/index0/tilt/servo", tilt_servo_[0], 1);
n_.param("/servo/index1/pan/servo", pan_servo_[1], 2);
n_.param("/servo/index1/tilt/servo", tilt_servo_[1], 3);
// Check for any servos mounted the opposite rotation of the right hand rule
n_.param("/servo/index0/pan/flip_rotation", pan_flip_rotation_[0], false);
n_.param("/servo/index0/tilt/flip_rotation", tilt_flip_rotation_[0], false);
n_.param("/servo/index1/pan/flip_rotation", pan_flip_rotation_[1], false);
n_.param("/servo/index1/tilt/flip_rotation", tilt_flip_rotation_[1], false);
/* Maximum and Minimum ranges. Values stored on parameter server in
* radians and RH rule as per ROS standard. These need converting
* to degrees and may need flipping.
*/
n_.param("/servo/index0/pan/max", max_radians, M_PI/2.0);
n_.param("/servo/index0/pan/min", min_radians, -(M_PI/2.0));
pan_max_[0] = (int)signedRadianToServoDegrees(max_radians, pan_flip_rotation_[0]);
pan_min_[0] = (int)signedRadianToServoDegrees(min_radians, pan_flip_rotation_[0]);
if(true == pan_flip_rotation_[0])
{
temp = pan_max_[0];
pan_max_[0] = pan_min_[0];
pan_min_[0] = temp;
}
n_.param("/servo/index0/tilt/max", max_radians, M_PI/2.0);
n_.param("/servo/index0/tilt/min", min_radians, -(M_PI/2.0));
tilt_max_[0] = (int)signedRadianToServoDegrees(max_radians, tilt_flip_rotation_[0]);
tilt_min_[0] = (int)signedRadianToServoDegrees(min_radians, tilt_flip_rotation_[0]);
if(true == tilt_flip_rotation_[0])
{
temp = tilt_max_[0];
tilt_max_[0] = tilt_min_[0];
tilt_min_[0] = temp;
}
n_.param("/servo/index1/pan/max", max_radians, M_PI/2.0);
n_.param("/servo/index1/pan/min", min_radians, -(M_PI/2.0));
pan_max_[1] = (int)signedRadianToServoDegrees(max_radians, pan_flip_rotation_[1]);
pan_min_[1] = (int)signedRadianToServoDegrees(min_radians, pan_flip_rotation_[1]);
if(true == pan_flip_rotation_[1])
{
temp = pan_max_[1];
pan_max_[1] = pan_min_[1];
pan_min_[1] = temp;
}
n_.param("/servo/index1/tilt/max", max_radians, M_PI/2.0);
n_.param("/servo/index1/tilt/min", min_radians, -(M_PI/2.0));
tilt_max_[1] = (int)signedRadianToServoDegrees(max_radians, tilt_flip_rotation_[1]);
tilt_min_[1] = (int)signedRadianToServoDegrees(min_radians, tilt_flip_rotation_[1]);
if(true == tilt_flip_rotation_[1])
{
temp = tilt_max_[1];
tilt_max_[1] = tilt_min_[1];
tilt_min_[1] = temp;
}
// Joint names
n_.param<std::string>("/servo/index0/pan/joint_name",
pan_joint_names_[0], "reserved_pan0");
n_.param<std::string>("/servo/index0/tilt/joint_name",
tilt_joint_names_[0], "reserved_tilt0");
n_.param<std::string>("/servo/index1/pan/joint_name",
pan_joint_names_[1], "reserved_pan1");
n_.param<std::string>("/servo/index1/tilt/joint_name",
tilt_joint_names_[1], "reserved_tilt1");
first_index0_msg_received_ = false;
first_index1_msg_received_ = false;
// Published topic is latched
servo_array_pub_ = n_.advertise<servo_msgs::servo_array>("/servo", 10, true);
// Subscribe to topic
joint_state_sub_ = n_.subscribe("/pan_tilt_node/joints",
10, &PanTiltNode::panTiltCB, this);
}
对 `param` 的调用将从服务器读取参数（如果可用），否则将使用默认值。
n_.param("/servo/index0/pan_servo", pan_servo_[0], 0);
构造函数的最后两行订阅了主题，并声明了我们的节点将发布哪些主题。订阅调用被传递了当主题到达时要调用的回调函数。
我们的回调函数名为 `panTiltCB`。
// Callback to move the joints
void PanTiltNode::panTiltCB(const sensor_msgs::JointState& joint)
{
bool index0 = false;
bool index1 = false;
/* Search the list of joint names in the message. Although we expect pan/tilt
* values for one device, a JointState message may contain data for one joint
* or all four joints. The position (rotation) values are signed radians and
* follow the right-hand rule. Values to be converted from signed radians to
* degrees and for the servo orientation. Pan/tilt values are also stored in
* case we change the trim.
*/
for (unsigned int i = 0; i < joint.name.size(); i++)
{
// Is it one of the pan or tilt joints
if(pan_joint_names_[0] == joint.name[i])
{
// Index 0 pan
index0_pan_ = (int)signedRadianToServoDegrees
(joint.position[i], pan_flip_rotation_[0]);
index0 = true;
}
else if(pan_joint_names_[1] == joint.name[i])
{
// Index 1 pan
index1_pan_ = (int)signedRadianToServoDegrees
(joint.position[i], pan_flip_rotation_[1]);
index1 = true;
}
else if(tilt_joint_names_[0] == joint.name[i])
{
// Index 0 tilt
index0_tilt_ = (int)signedRadianToServoDegrees
(joint.position[i], tilt_flip_rotation_[0]);
index0 = true;
}
else if (tilt_joint_names_[1] == joint.name[i])
{
// Index 1 tilt
index1_tilt_ = (int)signedRadianToServoDegrees
(joint.position[i], tilt_flip_rotation_[1]);
index1 = true;
}
}
if(index0 == true)
{
first_index0_msg_received_ = true;
movePanTilt(index0_pan_, index0_tilt_, index0_pan_trim_, index0_tilt_trim_, 0);
}
if(index1 == true)
{
first_index1_msg_received_ = true;
movePanTilt(index1_pan_, index1_tilt_, index1_pan_trim_, index0_tilt_trim_, 1);
}
}
回调函数遍历接收消息中的名称，查找已知的关节名称。如果找到名称，则使用 `signedRadianToServoDegrees` 辅助函数将关联的位置值从 ROS 标准和方向转换为表示舵机度数的值。
回调函数随后调用 `movePanTilt` 函数。此函数添加相关平移舵机的修剪偏移量，检查是否应限制范围，然后发布带有舵机索引和位置的两条消息。发布的这两条消息类型相同，一条用于相关平移舵机，另一条用于相关倾斜舵机。
void PanTiltNode::movePanTilt(int pan_value, int tilt_value,
int pan_trim, int tilt_trim, int index)
{
int pan;
int tilt;
servo_msgs::servo_array servo;
pan = pan_trim + pan_value;
tilt = tilt_trim + tilt_value;
pan = checkMaxMin(pan, pan_max_[index], pan_min_[index]);
tilt = checkMaxMin(tilt, tilt_max_[index], tilt_min_[index]);
// Send message for pan position
servo.index = (unsigned int)pan_servo_[index];
servo.angle = (unsigned int)pan;
servo_array_pub_.publish(servo);
// Send message for tilt position
servo.index = (unsigned int)tilt_servo_[index];
servo.angle = (unsigned int)tilt;
servo_array_pub_.publish(servo);
}
有两个辅助函数。第一个用于检查最大/最小范围。
int PanTiltNode::checkMaxMin(int current_value, int max, int min)
{
int value = current_value;
if (value > max)
{
value = max;
}
if (value < min)
{
value = min;
}
return (value);
}
第二个辅助函数用于将ROS标准单位和旋转方向转换为舵机所需的单位和方向。
// Converts a signed radian value to servo degrees. 0 radians is 90 degrees.
double PanTiltNode::signedRadianToServoDegrees(double rad, bool flip_rotation)
{
double retVal;
if(true == flip_rotation)
{
retVal = ((-rad/(2.0*M_PI))*360.0)+90.0;
}
else
{
retVal = ((rad/(2.0*M_PI))*360.0)+90.0;
}
return retVal;
}
动态参数服务器回调存储每个微调参数，然后对 `movePanTilt` 进行两次调用，每个平移/倾斜设备一次，使用最后一个位置值和最新的微调值。
// This callback is for when the dynamic configuration parameters change
void PanTiltNode::reconfCallback(pan_tilt::PanTiltConfig &config, uint32_t level)
{
index0_pan_trim_ = config.index0_pan_trim;
index0_tilt_trim_ = config.index0_tilt_trim;
index1_pan_trim_ = config.index1_pan_trim;
index1_tilt_trim_ = config.index1_tilt_trim;
// We don't want to send a message following a call here unless we have received
// a position message. Otherwise the trim value will be taken for an actual position.
if(first_index0_msg_received_ == true)
{
// Send new messages with new trim values
movePanTilt(index0_pan_, index0_tilt_, index0_pan_trim_, index0_tilt_trim_, 0);
}
if(first_index1_msg_received_ == true)
{
movePanTilt(index1_pan_, index1_tilt_, index1_pan_trim_, index1_tilt_trim_, 1);
}
}
_pan_tilt_node.h_ 文件包含我们 `PanTiltNode` 类的定义。
完成云台包后，我们需要做的最后一个编码任务是编写 Arduino 草图。该草图包含云台节点中使用的许多元素。我们的草图基于 rosserial 的舵机教程，但我们必须修改它以访问多个舵机，其中还包括订阅我们用户定义的消息。
每个 Arduino 草图都包含一个 setup 和 loop 过程。我们的 setup 过程初始化节点并订阅舵机话题。setup 过程的其余部分将引脚 9、6、5 和 10 连接到舵机的四个实例。
循环过程简单地调用 `spinOnce`，然后延迟 1 毫秒。对 `spinOnce` 的调用将处理话题的接收。
接收到舵机话题后，将调用回调函数 `servo_cb`。每次收到舵机话题消息时，此函数都会被调用，然后它会简单地调整索引 `servo` 的 PWM 输出。
/*
* Based on the rosserial Servo Control Example
* This version controls upto four RC Servos
* The node subscribes to the servo topic and acts on a rodney_msgs::servo_array message.
* This message contains two elements, index and angle. Index references the servos 0-3 and
* angle is the angle to set the servo to 0-180.
*
* D5 -> PWM servo indexed 2
* D6 -> PWM servo indexed 1
* D9 -> PWM servo indexed 0
* D10 -> PWM servo indexed 3
*/
#if (ARDUINO >= 100)
#include <Arduino.h>
#else
#include <WProgram.h>
#endif
#include <Servo.h>
#include <ros.h>
#include <servo_msgs/servo_array.h>
/* Define the PWM pins that the servos are connected to */
#define SERVO_0 9
#define SERVO_1 6
#define SERVO_2 5
#define SERVO_3 10
ros::NodeHandle nh;
Servo servo0;
Servo servo1;
Servo servo2;
Servo servo3;
void servo_cb( const servo_msgs::servo_array& cmd_msg)
{
/* Which servo to drive */
switch(cmd_msg.index)
{
case 0:
nh.logdebug("Servo 0 ");
servo0.write(cmd_msg.angle); //set servo 0 angle, should be from 0-180
break;
case 1:
nh.logdebug("Servo 1 ");
servo1.write(cmd_msg.angle); //set servo 1 angle, should be from 0-180
break;
case 2:
nh.logdebug("Servo 2 ");
servo2.write(cmd_msg.angle); //set servo 2 angle, should be from 0-180
break;
case 3:
nh.logdebug("Servo 3 ");
servo3.write(cmd_msg.angle); //set servo 3 angle, should be from 0-180
break;
default:
nh.logdebug("No Servo");
break;
}
}
ros::Subscriber<servo_msgs::servo_array> sub("servo", servo_cb);
void setup()
{
nh.initNode();
nh.subscribe(sub);
servo0.attach(SERVO_0); //attach it to the pin
servo1.attach(SERVO_1);
servo2.attach(SERVO_2);
servo3.attach(SERVO_3);
// Defaults
servo0.write(90);
servo1.write(120);
}
void loop(){
nh.spinOnce();
delay(1);
}
Using the Code
在编译草图并对 Arduino 编程之前，我们首先需要构建我们的 ROS 包并重新编译 ROS Arduino 库。我们需要这样做，以便我们的用户定义消息 `servo_array` 在 Arduino IDE 中可用。
由于我将使用 Linux 工作站运行 Arduino IDE，我将在工作站和 Raspberry Pi 上都构建我们的软件包。由于在此阶段我们没有使用任何专用的 Raspberry Pi 硬件，您可以完全在工作站上运行节点。我将在 Raspberry Pi 上运行节点，并在工作站上运行测试工具，如果您愿意，也可以在 Pi 上运行测试工具。请注意，为了区分 Pi 和工作站，在以下说明中，代码在 Pi 上的目录（工作区）名为“`rodney_ws`”，在工作站上名为“`test_ws`”。
在工作站上构建ROS包
ROS 使用 catkin 构建系统，所以首先我们将创建一个 catkin 工作空间并初始化该工作空间。在命令终端中，输入以下命令：
$ mkdir -p ~/test_ws/src
$ cd ~/test_ws/
$ catkin_make
将 `pan_tilt` 和 `servo_msgs` 这两个包文件夹复制到 `~/test_ws/src` 文件夹中，然后使用以下命令构建代码：
$ cd ~/test_ws/
$ catkin_make
检查构建是否无任何错误地完成。
构建 Arduino ROS 库
我在工作站上安装了 Arduino IDE，安装时它在我的 _home_ 目录中创建了一个 _Arduino_ 文件夹，其中包含一个名为“_libraries_”的子目录。请注意，在重新生成库时，您必须从“_libraries_”目录中删除 _ros_lib_ “`rm -rf ros_lib`”。
使用以下命令构建 `ros_lib` 库：
$ source ~/test_ws/devel/setup.bash
$ cd ~/Arduino/libraries
$ rm -rf ros_lib
$ rosrun rosserial_arduino make_libraries.py .
检查构建是否完成，没有错误，并检查 _servo_array.h_ 文件是否已在 _~/Arduino/libraries/ros_lib/servo_msgs_ 文件夹中创建。
构建舵机草图并对 Arduino 进行编程
将 _rodney_control_ 文件夹复制到 _~/Arduino/Projects_ 文件夹。启动 Arduino IDE 并打开 _rodney_control.ino_ 文件。从 **工具** -> **板卡** 菜单中，选择您正在使用的 Arduino 板卡。在我的例子中，它是 Nano。从 `工具` -> `处理器` 菜单中，选择处理器。在我的例子中，它是 ATmega328P (旧引导加载程序)。
构建草图并检查没有错误。
要对 Arduino 进行编程，请将设备连接到工作站的 USB 端口。在 IDE 中，从 **工具** -> **端口** 菜单中，选择 Arduino 连接到的串口。在我的例子中，它是 _/dev/ttyUSB0_。
接下来，将草图上传到 Arduino 并检查没有报告任何错误。
Arduino 电路
当我们构建罗德尼时，我们需要考虑电源问题。目前，我将使用 Raspberry Pi 的 USB 端口为 Arduino 供电，舵机将由 4 节 AA 可充电电池供电。下面是一个测试电路，显示了舵机连接和舵机的电源。
现在，为了测试软件，我将在面包板上构建电路，只连接头部平移和倾斜装置的舵机。
在树莓派上构建 ROS 包
创建一个 catkin 工作空间并初始化该工作空间。在命令终端中，输入以下命令：
$ mkdir -p ~/rodney_ws/src
$ cd ~/rodney_ws/
$ catkin_make
将 `pan_tilt` 和 `servo_msgs` 这两个包文件夹复制到 `~/rodney_ws/src` 文件夹中，然后使用以下命令构建代码：
$ cd ~/rodney_ws/
$ catkin_make
检查构建是否无任何错误地完成。
提示
在工作站和树莓派上运行 ROS 代码和工具时，在多个终端中可能会重复输入大量命令。在下一节中，我将包含完整的命令，但这里有一些技巧可以为您节省所有这些输入。
为了避免在 Raspberry Pi 上输入 “`source devel/setup.bash`”，我已将其添加到 Raspberry Pi 的 _.bashrc_ 文件中。
$ cd ~/
$ nano .bashrc
然后将 "source /home/ubuntu/rodney_ws/devel/setup.bash" 添加到文件末尾，保存并退出。
在工作站上运行测试代码和工具时，它需要知道 ROS 主节点在哪里，所以我将以下内容添加到了工作站的 _.bashrc_ 文件中。
alias rodney='source ~/test_ws/devel/setup.bash; \
export ROS_MASTER_URI=http://ubiquityrobot:11311'
然后只需在终端输入 "rodney"，这两个命令就会运行，省去了大量的输入。
运行代码
现在我们已经准备好运行代码了。将 Arduino 连接到 Pi 的 USB 端口，使用启动文件通过以下命令启动节点。如果系统中没有主节点运行，启动命令也会启动主节点 `roscore`。
$ cd ~/rodney_ws/
$ source devel/setup.bash
$ roslaunch pan_tilt pan_tilt_test.launch
在终端中，您应该看到：
- 现在参数服务器中的参数列表
- 节点列表，应显示 `pan_tilt_node` 和 `serial_node`。
- 主机的地址
- 两个节点的启动
- 我们代码的日志信息
现在我们可以使用一些 ROS 工具来检查、交互和测试系统。
要测试预期节点是否正在运行并通过话题连接，请在工作站上打开一个命令终端并键入以下命令：
$ cd ~/test_ws
$ source devel/setup.bash
如果您在一个设备（例如 Raspberry Pi）上启动了节点，并希望在第二个设备上运行工具，则需要告诉第二个设备在哪里找到主设备。在同一终端中键入：
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
现在在同一个终端中，启动图形工具：
$ rqt_graph
从图中，您可以看到两个节点正在运行，并通过 _/servo_ 话题连接。您还可以看到 _/pan_tilt_node/joints_ 话题。
现在我们将在工作站上打开第二个终端，并使用 `rostopic` 发送消息来移动平移/倾斜设备。在新终端中，输入以下命令，如果主节点在与启动节点不同的设备上运行，请不要忘记提供主节点的位置。
$ cd ~/test_ws
$ source devel/setup.bash
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ rostopic pub -1 /pan_tilt_node/joints sensor_msgs/JointState
'{header: {seq: 0, stamp: {secs: 0, nsecs: 0}, frame_id: ""},
name: [ "head_pan","tilt_pan"], position: [0,0.349066], velocity: [], effort: []}'
最后一条命令将导致 rostopic 发布一个 _/pan_tilt_node/joints_ 话题的实例，消息类型为 _sensor_msgs/JointState_，其中平移位置为 0 弧度，倾斜位置为 0.349066 弧度。如果一切正常，舵机将移动到给定位置。请注意，在该项目的当前阶段，舵机直接移动到新位置。在下一篇文章中，我们将添加一个节点，以更受控的方式移动头部。
输入 `rostopic` 命令可能会有点冗长。或者，您可以使用 rqt GUI。在终端中输入：
$ rosrun rqt_gui rqt_gui
这将启动一个窗口，您可以在其中选择 **消息发布器**，选择要发布的消息和消息字段内容。
由于云台设备的机械配件，它可能会偏离中心几度。您可以通过以下步骤微调舵机：
将两个舵机的位置都设置为中间位置。
$ rostopic pub -1 /pan_tilt_node/joints sensor_msgs/JointState
'{header: {seq: 0, stamp: {secs: 0, nsecs: 0}, frame_id: ""},
name: [ "head_pan","tilt_pan"], position: [0,0], velocity: [], effort: []}'
在新终端中，使用以下命令启动 `rqt_reconfigure`，如果主节点在不同的设备上运行，请不要忘记提供主节点的位置。
$ cd ~/test_ws
$ source devel/setup.bash
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ rosrun rqt_reconfigure rqt_reconfigure
这将弹出一个用户界面，如下图所示。修剪参数可以通过界面动态调整。
一旦您对修剪值满意，您可以编辑 _pan_tilt.cfg_，将新的修剪值作为默认值。然后，下次启动节点时，将使用这些修剪值。
要终止节点，只需在终端中按 **Ctrl-c** 即可。
头部平移倾斜装置
对于云台设备，我使用了两个 Futaba 舵机，一个是 S3003，另一个是 S3305。S3305 包含金属齿轮，并安装在平移位置。您可以购买各种尺寸舵机的云台设备，但我选择 3D 打印自己的版本，零件的 stl 文件可在 zip 文件中找到。我曾担心显示器和 Raspberry Pi 的组合重量会对平移舵机轴产生侧向扭矩，所以我使用了承重舵机块来缓解这个问题。这个单元充当舵机外骨骼，增强了舵机可以承受的机械载荷。这增加了机器人的成本，因此另一种选择是将摄像头安装在一个较小的云台设备上，并将屏幕固定在位置。以下图片显示了承重舵机块和云台装置。
关注点
在本文中，我们成功地将 ROS 社区中的一个 ROS 节点集成到我们的机器人系统中，并编写了我们自己的 ROS 节点。我们让 ROS 运行在主板 Raspberry Pi 上，并且还将部分功能卸载到了 Arduino Nano 上。
在下一篇文章中，我将继续在设计目标1上努力，通过将一个 Python 面部识别库包装在一个 ROS 节点中，并添加一个控制头部运动的节点。
历史
- 2018年7月28日：首次发布
- 2014年7月31日：版本2：`pan_tilt` _package.xml_ 文件包含一个格式不正确的电子邮件地址，导致启动节点时出现问题
- 2019年1月9日：版本3：现在使用 `sensor_msgs/JointState` 类型作为云台节点的输入话题，并遵循 ROS 标准度量单位和坐标约定