🤖️ 自主机器人“罗德尼”:第一部分

Phil Hopley 著
Conmajia 译
2019 年 1 月 16 日

原文发表于 CodeProject(2019 年 1 月 15 日),中文版已获作者本人授权.

本文是 House Bot 机器人操作系统的第一部分.

全文约 6000 字,建议阅读时间 15 分钟.

源码 61.7 KB

3D 打印文件 70.9 KB

简介

罗德尼是我设计的一个自主家庭机器人. 这是这个项目系列的第一篇文章. 在这部分我主要阐述概念,如何选择单板机,安装 ROS(robot operating system)以及编写第一部分用到的控制软件.

背景

早在 1970、1980 年代,我买了两本书:David L. Heiserman[1] 的《如何打造自编程机器人》和 Tod Loofbourrow[2] 的《如何打造计算机控制的机器人》. 当时我打算用我的 Z80 处理器板来制作机器人,可惜这个机器人一直没能诞生. 这么多年过去了,现在有很多类似树莓派(Raspberry Pi)和 Arduino 之类的袖珍单板电脑. 这些新玩意儿让制作复杂的家庭机器人变得特别简单.

在我那两本启蒙读物中,作者给机器人起名叫罗德尼(Rodney)和迈克(Mike),顺其自然,我给我的机器人起名叫罗德尼.

图 1 勾起我兴趣的两本机器人制作书籍

我在 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 文件我已经打包出来,点击下载.

(正面)

(反面)

图 2 罗德尼的脑袋(正面和反面)

鉴于 ROS 可以在分布式网络上运行,我把我的 Ubuntu PC 机也装上了 ROS,用来开发节点,运行 ROS 测试工具、仿真工具等等.

机器人的任务

设计项目最基本的是确定需求. 对于罗德尼,我打算指定一些想让它完成的功能. 《人人都造机器人!》里罗列了很多家庭机器人可以做的工作,比如:

帮我带个话
既然机器人可以认出家庭成员,那么让它给某人带个话就很自然了. 我可以说机器人,帮我告诉某某下午六点来车站接我. 之后,即使他电话静音了或者正在嗨音乐,机器人也会跑到他的房间,找到他提醒他.

听起来不错,也许我的项目可以从这个任务入手. 当然应稍作改进,毕竟我设计的罗德尼可以通过网络浏览器来控制并设置任务.

带个话给某人是一个大任务,可以拆分成小的设计目标,各自分别完成:

  1. 使用摄像头观察环境,搜索并识别人脸,找到后显示消息(在脸上)
  2. 面部识别和语音合成,这样罗德尼可以真正地带个话
  3. 通过键盘和摇杆遥控机器人移动
  4. 辅助导航的激光雷达或者类似的传感器
  5. 自主移动
  6. 安排任务完成后提示

对机器人来说,一个看起来很简单的任务都能列出这么长的单子. 具体实现起来,也不是一两句话能说明白的. 接下来,我把带个话定为“任务 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.txtpackage.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 消息里关节的名称
  • maxmin 用于限制关节移动幅度,单位是弧度
  • 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" />

剩下的 includesrc 文件夹里是功能包的 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_tiltservo_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 是供电电路的示意图.

图 3 罗德尼供电电路图

为了测试,我在面包板上搭建了上图的电路,并且只接了头部平移/倾斜设备.

图 4 罗德尼供电测试电路

在树莓派上编译 ROS 功能包

还是用类似的命令,创建 catkin 工作区并初始化:

$ mkdir -p ~/rodney_ws/src
$ cd ~/rodney_ws/
$ catkin_make

pan_tiltservo_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_nodeserial_node
  • 主机地址
  • 上面两个节点的启动过程
  • 代码里的日志信息

这时就可以用 ROS 的工具来检查、测试系统了. 新开一个命令行终端(PC),输入命令:

$ cd ~/test_ws
$ source devel/setup.bash

如果节点是在同一设备上运行的,而工具程序在另一台设备上运行,那么需要指定主机的地址:

$ export ROS_MASTER_URI=http://ubiquityrobot:11311

现在可以运行图形工具了:

$ rqt_graph

图 5 ROS 图形工具

用这个工具可以看到节点的运行情况以及和 /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

这个命令会运行一个图形界面,可以选择消息发布者,发布消息和内容.

图 6 rpt 图形界面

在装配各零件的时候,很可能有一定的机械误差,所以平移/倾斜会偏离中点一定角度,这时候可以对它进行微调,把两个舵机设置到中心位置:

$ 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

这个命令会打开类似下面的窗口,微调参数可以通过这个界面调整:

图 7 微调参数设置界面

调整到满意之后,就可以用得到的值更新 pan_tilt.cfg 配置文件里的默认值了,这样下次节点会使用这些校正过的值启动.

要关闭节点,在终端里按 Ctrl-C.

平移/倾斜设备

平移/倾斜设备使用的日本双叶舵机,一个型号是 S3003,另一个是 S3305. S3305 自带金属固定组件,如图 8 所示.

图 8 舵机和固定组件

当然我用的是 3D 打印的零件. 考虑到显示器和树莓派的重量对舵机轴向压力,我用了一个载荷组件来减轻这个问题. 这个组件相当于舵机的外骨骼,可以增强舵机承载的机械强度. 也可以通过把屏幕固定,只移动摄像头的方式代替现在的方案,不过这样看起来就不像一个机器了. 图 9 展示了我设计的装配件效果.

图 9 自制装配件

兴趣所在

这篇文章里,我实现了用树莓派运行字节编写的 ROS 节点,用 Arduino 控制舵机. 下一篇文章,我将继续研究设计目标 1,添加一个封装在 ROS 节点中的 Python 人脸识别库,然后添加一个节点来控制罗德尼脑袋的移动.

如果说现在罗德尼还只是一副躯壳,那么它即将拥有灵魂!

图 10 罗德尼认出我了!

历史

  • 首次发表:2018/07/28
  • 第二版:2018/07/31 修正了 package.xml 的错误
  • 第三版:2019/01/09 改用 sensor_msgs/JointState

许可

本文以及任何相关的源代码和文件都是根据 GNU通用公共许可证(GPLv3)授权的.

关于作者

img

Phil Hopley,来自英国🇬🇧,高级软件工程师,已退休,爱好徒步和划船,闲暇时会做点软硬件小玩意儿.


  1. Heiserman 也是《Apple II C语言编程》一书作者. ↩︎

  2. Loofbourrow 是 ViralGains 公司 CEO. ↩︎

  3. 本文获得了 CodeProject 2016 年 7 月号 Everything Else 奖状第 2 名. ↩︎

  4. 本文获得了 CodeProject 2018 年 3 月号 Everything Else 奖状第 2 名. ↩︎

posted @ 2019-01-17 23:55  Conmajia  阅读(565)  评论(0编辑  收藏