🤖️ 自主机器人“罗德尼”：第一部分
Phil Hopley 著
Conmajia 译
2019 年 1 月 16 日原文发表于 CodeProject（2019 年 1 月 15 日），中文版已获作者本人授权.
本文是 House Bot 机器人操作系统的第一部分.
简介
罗德尼是我设计的一个自主家庭机器人. 这是这个项目系列的第一篇文章. 在这部分我主要阐述概念，如何选择单板机，安装 ROS（robot operating system）以及编写第一部分用到的控制软件.
背景
早在 1970、1980 年代，我买了两本书：David L. Heiserman[1] 的《如何打造自编程机器人》和 Tod Loofbourrow[2] 的《如何打造计算机控制的机器人》. 当时我打算用我的 Z80 处理器板来制作机器人，可惜这个机器人一直没能诞生. 这么多年过去了，现在有很多类似树莓派（Raspberry Pi）和 Arduino 之类的袖珍单板电脑. 这些新玩意儿让制作复杂的家庭机器人变得特别简单.
在我那两本启蒙读物中，作者给机器人起名叫罗德尼（Rodney）和迈克（Mike），顺其自然，我给我的机器人起名叫罗德尼.
 |
 |
我在 CodeProject 上看到过两篇关于机器人的文章，给我启发很大.
第一篇文章是《人人都造机器人！》（Let's build a robot! [3]）介绍了机器人的基本概念和一些很棒的想法，不过我很怀疑靠这些概念怎么造机器人？这篇文章给我的最大启发是可以用一个小显示器当做机器人的脑袋. 最有用的还是文章里一个 Pi Robot 的博客链接，让我第一次认识了机器人操作系统（ROS）. 这实际上是机器人编程的标准，就像维基上对 ROS 的解释：
ROS（机器人操作系统）为软件开发者创建机器人程序提供各种库函数和工具. 它提供了硬件的抽象，设备驱动，库函数，可视化驱动，消息传递机制，分包管理等等内容. ROS 作为开源软件，通过 BSD 许可授权.
ROS 其实算不是一个真正的操作系统，更像是一个用于 Linux 上的中间件（middleware），尤其适合运行在 Ubuntu 上. 网上可以找到大量开源的 ROS 代码用于各种传感器，可以帮助你集中精力开发机器人程序. ROS 的维基也全是满满的干货，如果你还不熟悉 ROS，去维基看看就对了.
第二篇文章是《PiRex：远程控制的树莓派机器人》（PiRex – remote controlled Raspberry Pi based robot [4]）. 尽管涵盖的内容不如第一篇那么广泛，这篇文章完整阐述了一个机器人项目. 两篇文章都用到了树莓派，还是比较便宜的.
所以，罗德尼不用再像 80 年代我计划的那样先要自制一块处理器板，直接用上了树莓派3B，带 1GB 内存，相当给力.
ROS 和树莓派
这里我将解释如何在项目中使用 ROS 和它提供的工具来测试代码. 我没打算写一篇 ROS 的教程，网上可以找打一大堆，比我能写出来的好得多. 当然我会时不时提供一些 ROS 维基上的链接作为延伸阅读. 现在嘛，我先列出一个提纲和部分 ROS 的名词，休闲读者也可以快速浏览.
- 这是一个分布式系统，机器人代码可以在通过网络连接的多台设备上运行
- 一个节点（node）负责执行某一任务
- 节点以功能包（package）的形式组织，作为一套文件夹或者文件的集合
- 可以用多种编程语言来设计节点，我用的是 C++ 和 Python
- 节点互相之间用称为话题（topic）的单向流通信
- 话题属于即时消息（message），消息是话题的数据结构
- ROS 自带标准消息，也可以定义自己的消息结构
- 节点也可以通过服务器／客户端（server/client）的阻塞式协议，使用服务（service）进行通信
- 节点还可以通过动作（action）协议进行非阻塞式面向目标任务的通信
- 所有节点向系统中唯一的主机（master）注册，即使用分布式系统，主机也是唯一的
- 用 catkin 构建和编译代码
- 独立的节点可以用
rosrun命令调用，也可以用启动工具在一个命令行下启用多个节点 - 系统包含了一个参数服务器（parameter server），节点可以在运行过程中存取参数
- 系统还包含检查、硬件仿真等多种工具
既然决定了用树莓派作为处理器，ROS 作为软件系统，第一件要做的事情就是在树莓派上安装 ROS.
ROS 的下载、安装方法可以参考这个网页. 但是更轻松的方法是用我制作的预装了 ROS，可以在树莓派上运行的 Ubuntu 系统镜像. 你可以在 Ubiquity Robotics 网站上下载这个镜像. 镜像中预装的是 Kinetic 版的 ROS，内置了访问树莓派摄像头的 raspicam_node 包. 当然你可以用别的镜像，GitHub 上可以下载 Ubiquity 提交的功能包源码.
罗德尼用到的其他外设有：
- 7 寸触摸屏
- 摄像头模块
显示屏显示机器人的状态信息，web 内容，同时显示机器人的表情，算是给了它一张“脸”. 摄像头是机器人的“眼睛”，能进行人脸识别，让它先认出主人来.
这张是触摸屏的照片，树莓派和摄像头安在屏幕背面，用 3D 打印的零件组装起来. 零件的 .stl 文件我已经打包出来，点击下载.
（正面）
（反面）
鉴于 ROS 可以在分布式网络上运行，我把我的 Ubuntu PC 机也装上了 ROS，用来开发节点，运行 ROS 测试工具、仿真工具等等.
机器人的任务
设计项目最基本的是确定需求. 对于罗德尼，我打算指定一些想让它完成的功能. 《人人都造机器人！》里罗列了很多家庭机器人可以做的工作，比如：
帮我带个话
既然机器人可以认出家庭成员，那么让它给某人带个话就很自然了. 我可以说机器人，帮我告诉某某下午六点来车站接我.之后，即使他电话静音了或者正在嗨音乐，机器人也会跑到他的房间，找到他提醒他.
听起来不错，也许我的项目可以从这个任务入手. 当然应稍作改进，毕竟我设计的罗德尼可以通过网络浏览器来控制并设置任务.
带个话给某人是一个大任务，可以拆分成小的设计目标，各自分别完成：
- 使用摄像头观察环境，搜索并识别人脸，找到后显示消息（在脸上）
- 面部识别和语音合成，这样罗德尼可以真正地带个话
- 通过键盘和摇杆遥控机器人移动
- 辅助导航的激光雷达或者类似的传感器
- 自主移动
- 安排任务完成后提示
对机器人来说，一个看起来很简单的任务都能列出这么长的单子. 具体实现起来，也不是一两句话能说明白的. 接下来，我把带个话定为“任务 1”，分别设计实现上面各个目标.
任务 1，目标 1
要完成这个设计目标，需要：
- 用 RC 舵机控制机器人的脑袋（摄像头）平移和倾斜
- 读取树莓派摄像头拍摄的图片
- 检测识别人脸
- 依序控制这些动作
本文是第一篇文章，先关注移动机器人的脑袋. 很显然，需要两个舵机分别控制平移（pan）和倾斜（tilt）. 为了扩展，我还添加了一套额外的舵机（2 个）. 这些舵机都是通过 PWM 信号控制的，但是树莓派只有 1 个硬件 PWM 通道，所以要使用软件方式在 GPIO 口输出 PWM 信号. 另外需要注意避免跳舵，我用了一块单独的电路板进行控制.
PiBorg, the UltraBorg 上有一种模块，通过 I2C 总线把 4 路舵机和 4 路 HC-SR04 超声波传感器连到树莓派. 不过我上一个项目还剩了不少 Arduino Nano，我打算用它来做控制板.
ROS 社区有很多杰作可以应用，这样我能专心设计我的机器人程序，其他方面，尽量发扬“拿来主义”的精神. 使用 rosserial_arduino 包，可以把 Arduino 通过串口连接到 ROS，这个包的文档在这里阅读.
使用前，要在 ROS 目标机和 Arduino IDE 里安装 rosserial_arduino，如果用了自定义消息，那还要重新编译 Arduino 库. 这些操作在教程里都有讲解.
现在来编写控制各个舵机位置的 ROS 功能包. 这个包的节点将处理平移／倾斜的消息，把它转化成独立的位置消息传给 Arduino，由后者负责控制操作舵机. 第一条消息判断需要移动哪个关节，移动多少；第二条消息发送一个编号和角度值给 Arduino，启动对应的舵机. 这样划分功能后，Arduino 编程时就不必考虑调用它的 ROS，只负责操作舵机.
第一条消息我用 ROS 内置的 sensor_msgs/JointState 实现. ROS 里标准的位置单位是弧度，Arduino 接受的角度单位是度，所以在设计节点的时候还要先把弧度换算成度. JointState 里有不少字段暂时都用不上，虽然有点浪费，不过使用 ROS 内置消息的好处在于可以方便地使用它提供的各种配套工具.
第二条消息定义了待操作舵机编号和角度. 这里我用了自定义的消息，避免 Arduino 源码里不必要的资源浪费.
现在来编写用于这两条消息的功能包，以及用于 ROS 的 Arduino 源码.
控制舵机的功能包消息我命名为 servo_msgs. 创建好后，它将生成 C++ 和 Python 代码，同时重新编译 Arduino 库生成 .h 文件.
实现这个功能包的代码文件包存在 servo_msgs 文件夹下，包括了一份说明文件以及每个 ROS 功能包都需要的 CmakeList.txt 和 package.xml 文件. 这些文件的意义以及如何创建功能包，可以参考这篇教程.
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.
有几个子文件夹，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消息里关节的名称max和min用于限制关节移动幅度，单位是弧度flip_rotation下文解释
ROS 惯例是右手定则（right-hand rule），因此关节的值会围绕正轴的逆时针方向上增大. 现在在罗德尼的头部倾斜舵机安装的方式遵循左手定则，所以需要设置 flip_rotation 为真. pan_tilt_node 可以保证传给 Arduino 的舵机方向是正确的，这点不用担心.
cfg 文件夹下的 pan_tilt.cfg 文件用于动态配置服务器，这样可以随时对舵机进行微调（trim）. 这个文件实际上是 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"))
关于动态配置服务器详细内容，可以看这篇文章. 在这里，我添加了 4 个参数，用来分别配置 4 个舵机，默认值都是 0，转动范围 -45\(^\circ\) 到 45\(^\circ\).
launch 文件夹包含了全部启动文件. 其中的 pan_tilt_test.launch 是测试专用的. 它实际上是个 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>
关于启动文件的详细信息，可以看这篇文章. 在启动文件里，首先用 load 命令加载了配置文件：
<rosparam command="load" file="$(find pan_tilt)/config/config.yaml" />
接下来执行了 pan_tilt 功能包的 pan_tilt_node 节点，通过指定输出为 screen，让信息直接显示到运行程序的终端上.
<node pkg="pan_tilt" type="pan_tilt_node" name="pan_tilt_node" output="screen" />
最后运行 roserial 和 Arduino 进行通信. 我用的 Arduino Nano 是通过 USB 连接到 PC 的，所以指定设备名 /dev/ttyUSB0.
<node pkg="rosserial_python" type="serial_node.py" name="serial_node" output="screen" args="/dev/ttyUSB0" />
剩下的 include 和 src 文件夹里是功能包的 C++ 源码. pan_tilt_node.cpp 文件包含了 PanTiltNode 类的定义和程序的主函数.
主函数用 pan_tilt_node 初始化 ROS，生成节点实例，同时将回调函数绑定到动态配置服务器.
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;
}
PanTiltNode 类在构造函数里加载参数服务器的参数：
// 构造函数
PanTiltNode::PanTiltNode()
{
double max_radians;
double min_radians;
int temp;
/* 从参数服务器获取参数，如果获取失败，则使用默认值 */
// 指定舵机功能
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);
// 检查舵机安装方式是否符合右手定则
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);
/* 取值范围. 为了满足右手定则，这些值可能需要进行翻转. */
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;
}
// 关节名
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;
// 锁存已发布的节点
servo_array_pub_ = n_.advertise<servo_msgs::servo_array>("/servo", 10, true);
// 订阅话题
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：
// 移动关节的回调函数
void PanTiltNode::panTiltCB(const sensor_msgs::JointState& joint)
{
bool index0 = false;
bool index1 = false;
/* 在消息的列表里查找关节名. 位置（旋转）值均为正弧度值，符合右手定则，
* 需要根据舵机方向换算成角度值.
*/
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 平移
index0_pan_ = (int)signedRadianToServoDegrees(joint.position[i], pan_flip_rotation_[0]);
index0 = true;
}
else if(pan_joint_names_[1] == joint.name[i])
{
// Index 1 平移
index1_pan_ = (int)signedRadianToServoDegrees(joint.position[i], pan_flip_rotation_[1]);
index1 = true;
}
else if(tilt_joint_names_[0] == joint.name[i])
{
// Index 0 倾斜
index0_tilt_ = (int)signedRadianToServoDegrees(joint.position[i], tilt_flip_rotation_[0]);
index0 = true;
}
else if (tilt_joint_names_[1] == joint.name[i])
{
// Index 1 倾斜
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]);
// 发送平移位置
servo.index = (unsigned int)pan_servo_[index];
servo.angle = (unsigned int)pan;
servo_array_pub_.publish(servo);
// 发送偏移位置
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 标准单位和方向换算成适合舵机的数值.
// 将正弧度值换算成舵机使用的角度值. 0 弧度相当于 90 度.
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，每个设备一次.
// 这个回调会在动态配置参数变化的时候执行
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;
// 只有收到位置消息才执行
if(first_index0_msg_received_ == true)
{
// 用新微调值发送新消息
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 例程作为模板来写的，包含了平移／倾斜节点里用到的各元素，支持多个舵机.
setup 函数对节点进行了初始化，订阅了舵机话题. 4 台舵机分别连接到 Arduino 的 PWM 引脚 9、6、 5 和 10. loop 函数里，调用了 spinOnce，随后延迟 1 毫秒. spinOnce 实际上会执行 servo_cb 回调函数. 这个函数每次收到舵机消息时都会执行.
/*
* 基于 rosserial 舵机例程
* 最多可以控制 4 台舵机
* 节点订阅舵机话题，并作为 rodney_msgs::servo_array 消息运行.
* 消息包含两个元素，编号和角度.
* 编号范围：0-3
* 角度范围：0-180
*
* D5 -> PWM 输出口，舵机 2
* D6 -> PWM 输出口，舵机 1
* D9 -> PWM 输出口，舵机 0
* D10 -> PWM 输出口，舵机 3
*/
#if (ARDUINO >= 100)
#include <Arduino.h>
#else
#include <WProgram.h>
#endif
#include <Servo.h>
#include <ros.h>
#include <servo_msgs/servo_array.h>
/* 定义连接舵机的 PWM 端口 */
#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); //设置舵机 0 角度，范围 0-180
break;
case 1:
nh.logdebug("Servo 1 ");
servo1.write(cmd_msg.angle); //设置舵机 1 角度，范围 0-180
break;
case 2:
nh.logdebug("Servo 2 ");
servo2.write(cmd_msg.angle); //设置舵机 2 角度，范围 0-180
break;
case 3:
nh.logdebug("Servo 3 ");
servo3.write(cmd_msg.angle); //设置舵机 3 角度，范围 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); // 关联舵机输出引脚
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);
}
使用源码
上面的程序编译烧录到 Arduino 板子前，要先编译前面写的 ROS 功能包，并且重新编译用于 Arduino 的 ROS 库. 由于我用的 Linux 版 Arduino IDE，我将在两个平台上进行编译. 我决定在树莓派上运行节点，PC 机上运行测试工具. 当然，现在还没有用到专用的树莓派硬件，所以也可以在 PC 上运行节点. 阅读下面的代码时，要注意区分运行的平台（树莓派或 PC）. 这些代码都存在 rodney_ws（树莓派）和 test_ws（PC）文件夹下.
在 PC 上编译 ROS 功能包
ROS 使用的是 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 库
编译 ros_lib 库的命令行如下：
$ source ~/test_ws/devel/setup.bash
$ cd ~/Arduino/libraries
$ rm -rf ros_lib
$ rosrun rosserial_arduino make_libraries.py .
如果编译没有问题，~/Arduino/libraries/ros_lib/servo_msgs 文件夹下会生成 servo_array.h 头文件.
编译并烧录 Arduino
把 rodney_control 文件夹复制到 ~/Arduino/Projects 下. 运行 Arduino IDE，打开 rodney_control.ino 文件. 在工具→开发板
菜单里选择 Arduino 开发板型号（我这里用的是 Arduino Nano）. 在工具→处理器
菜单里选择处理器型号（ATmega328）.
用 USB 线把 Arduino Nano 连接到 PC 上，在工具→端口
菜单里选择对应的端口（/dev/ttyUSB0）.
点击上传
按钮，如果一切正常，源代码将编译并烧录到 Arduino 里.
Arduino 电路
制作罗德尼的时候，供电问题必须要考虑到. 在我的设计里，Arduino 从树莓派的 USB 口取电，舵机则用 4 节 5 号（AA）可充电电池供电. 图 3 是供电电路的示意图.
为了测试，我在面包板上搭建了上图的电路，并且只接了头部平移／倾斜设备.
在树莓派上编译 ROS 功能包
还是用类似的命令，创建 catkin 工作区并初始化：
$ mkdir -p ~/rodney_ws/src
$ cd ~/rodney_ws/
$ catkin_make
把 pan_tilt 和 servo_msgs 文件夹复制到 ~/rodney_ws/src 然后编译：
$ cd ~/rodney_ws/
$ catkin_make
如果无错，则编译完成.
小提示
在 PC 和树莓派上运行 ROS 代码和工具时，可能需要在多个命令行终端上执行同样的命令. 下一节我还是会写完整的命令，不过我可以使用我写的 .bash 文件简化命令输入.
首先编辑 .bashrc：
$ cd ~/ $ nano .bashrc
在文件最后添加 source /home/ubuntu/rodney_ws/devel/setup.bash，保存退出.
PC 在运行测试代码时需要知道 ROS 主机位置（端口），所以 PC 的 .bashrc 里我添加了下面的语句：
alias rodney='source ~/test_ws/devel/setup.bash; \ export ROS_MASTER_URI=http://ubiquityrobot:11311'
一个 rodney 就可以一次运行上面两个命令，再不用敲命令敲到手软啦.
运行代码
一切准备就绪，随时可以运行代码了. 用 USB 线把 Arduino 连到树莓派，用启动文件打开节点：
$ cd ~/rodney_ws/
$ source devel/setup.bash
$ roslaunch pan_tilt pan_tilt_test.launch
终端上会显示：
- 参数服务器的参数列表
- 节点列表，包括了
pan_tilt_node和serial_node - 主机地址
- 上面两个节点的启动过程
- 代码里的日志信息
这时就可以用 ROS 的工具来检查、测试系统了. 新开一个命令行终端（PC），输入命令：
$ cd ~/test_ws
$ source devel/setup.bash
如果节点是在同一设备上运行的，而工具程序在另一台设备上运行，那么需要指定主机的地址：
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
现在可以运行图形工具了：
$ rqt_graph
用这个工具可以看到节点的运行情况以及和 /servo 话题的连接情况. 图中可以看到 /pan_tilt_node/joints 话题.
现在在 PC 上打开一个终端，用 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.
平移／倾斜设备
平移／倾斜设备使用的日本双叶舵机，一个型号是 S3003，另一个是 S3305. S3305 自带金属固定组件，如图 8 所示.
当然我用的是 3D 打印的零件. 考虑到显示器和树莓派的重量对舵机轴向压力，我用了一个载荷组件来减轻这个问题. 这个组件相当于舵机的外骨骼，可以增强舵机承载的机械强度. 也可以通过把屏幕固定，只移动摄像头的方式代替现在的方案，不过这样看起来就不像一个机器人了. 图 9 展示了我设计的装配件效果.
兴趣所在
这篇文章里，我实现了用树莓派运行字节编写的 ROS 节点，用 Arduino 控制舵机. 下一篇文章，我将继续研究设计目标 1，添加一个封装在 ROS 节点中的 Python 人脸识别库，然后添加一个节点来控制罗德尼脑袋的移动.
如果说现在罗德尼还只是一副躯壳，那么它即将拥有灵魂！
历史
- 首次发表：2018/07/28
- 第二版：2018/07/31 修正了
package.xml的错误 - 第三版：2019/01/09 改用
sensor_msgs/JointState
许可
本文以及任何相关的源代码和文件都是根据 GNU通用公共许可证（GPLv3）授权的.
关于作者
Phil Hopley，来自英国🇬🇧，高级软件工程师，已退休，爱好徒步和划船，闲暇时会做点软硬件小玩意儿.
if(jQuery('#no-reward').text() == 'true') jQuery('.bottom-reward').addClass('hidden');
