自主机器人“罗德尼”：第一部分
Posted conmajia
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了自主机器人“罗德尼”：第一部分相关的知识，希望对你有一定的参考价值。
Phil Hopley 著
Conmajia 译
2019 年 1 月 16 日原文发表于 CodeProject（2019 年 1 月 15 日）.
本文是 House Bot 机器人操作系统的第一部分.
全文约 6000 字，建议阅读时间 15 分钟.
简介
罗德尼是我设计的一个自主家庭机器人. 这是这个项目系列的第一篇文章. 在这部分我主要阐述概念，如何选择单板机，安装 ROS（robot operating system）以及编写第一部分用到的控制软件.
背景
早在 1970、1980 年代，我买了两本书：David L. Heiserman1 的《如何打造自编程机器人》和 Tod Loofbourrow2 的《如何打造计算机控制的机器人》. 当时我打算用我的 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），节点可以在运行过程中存取参数
- 系统还包含检查、硬件仿真等多种工具
这里有一篇 Intel 发表的关于 ROS 的概述文章，感兴趣的读者可以参考.
既然决定了用树莓派作为处理器，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: 3index0 和 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如果主机节点没有运行，那么启动命令会同时启动主机节点 roscore.
终端上会显示：
- 参数服务器的参数列表
- 节点列表，包括了
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，来自英国????，高级软件工程师，已退休，爱好徒步和划船，闲暇时会做点软硬件小玩意儿.
以上是关于自主机器人“罗德尼”：第一部分的主要内容，如果未能解决你的问题，请参考以下文章
少儿编程 电子学会机器人等级考试一级真题解析判断题2018-12
Visual simultaneous localization and mapping: a survey 论文解析（全）