Python 机器人学习手册：6~10-阿里云开发者社区

六、将执行器和传感器连接到机器人控制器

在上一章中，我们讨论了构建机器人所需的硬件组件的选择。机器人中的重要组件是执行器和传感器。致动器为机器人提供移动性，而传感器则提供有关机器人环境的信息。在本章中，我们将集中讨论我们将在该机器人中使用的不同类型的执行器和传感器，以及如何将它们与 Tiva C LaunchPad 进行接口，Tiva C LaunchPad 是德州仪器（TI）的 32 位 ARM 微控制器板，在 80MHz。我们将从讨论执行器开始。我们首先要讨论的执行器是带有编码器的直流齿轮电动机。直流齿轮电动机使用直流电工作，并通过齿轮减速来降低轴速并增加最终轴的扭矩。这类电机非常经济，可以满足我们的机器人设计要求。我们将在机器人原型中使用该电机。

在本章的第一部分中，我们将讨论机器人驱动系统的设计。我们的机器人的驱动系统是差速驱动器，由两个带编码器的直流减速电机和一个电机驱动器组成。电机驱动器由 Tiva C LaunchPad 控制。我们将研究电动机驱动器和正交编码器与 Tiva C Launchpad 的接口。之后，我们将介绍一些最新的执行器，这些执行器可以用编码器代替现有的直流减速电机。如果所需的机器人需要更多的负载和精度，我们必须切换到此类执行器。最后，我们将介绍一些通常用于机器人的不同传感器。

在本章中，我们将介绍：

将直流齿轮电动机与 Tiva C LaunchPad 连接
正交编码器与 Tiva C LaunchPad 的接口
接口代码的说明
连接 Dynamixel 执行器
连接超声波传感器和红外接近传感器
接口惯性测量单元（IMU）

技术要求

您将需要在 Ubuntu 16.04 LTS 中设置必要的机器人硬件组件和 Energia IDE。

直流减速电机与 Tiva C LaunchPad 的接口

在上一章中，我们选择了直流减速电动机，该电动机带有 Pololu 的编码器和德州仪器（TI）的嵌入式板，称为 Tiva C LaunchPad。我们需要以下组件将电动机与 LaunchPad 连接：

两台 Pololu 金属齿轮电动机，尺寸为 37Dx73Lmm，每转编码器计数为 64
Pololu 车轮，90x10mm 和一个匹配的轮毂
Pololu VNH2SP30 双电机驱动器支架，MD03A
12V 密封铅酸/锂离子电池
3.3V 至 5V 的逻辑电平转换器；请访问这个页面。
Tiva C LaunchPad 及其兼容的接口线

下图显示了使用 Pololu H 桥的两个电机的接口电路：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-GdoGFEBb-1681873784538)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00077.jpeg)]

电机接口电路

要与 Launchpad 交互，我们必须在这两个电机之间连接一个电平转换器板。电机驱动器的工作电压为 5V，而 Launchpad 的工作电压为 3.3V，因此我们必须连接一个电平转换器，如下图所示：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-dTRDxKP2-1681873784539)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00078.gif)]

电平转换器电路

两个齿轮直流电动机分别连接到电动机驱动器的 OUT1A，OUT1B 和 OUT2A，OUT2B。 VIN(+) 和 GND(-) 是电动机的电源电压。这些直流电动机可以在 12V 电源下工作，因此我们提供 12V 作为输入电压。电机驱动器将支持 5.5V 至 16V 的输入电压。

电机驱动器的控制信号/输入引脚在驱动器的左侧。第一个引脚是 1DIAG/EN；在大多数情况下，我们将该引脚断开。这些引脚在驱动板上本身被外部拉高。该引脚的主要用途是启用或禁用 H 桥芯片。它还用于监视 H 桥 IC 的故障状态。引脚 1INA 和 1INB 用来控制电动机的旋转方向。 1PWM 引脚会将电动机切换为 ON 和 OFF 状态。我们使用 PWM 引脚实现速度控制。 CS 引脚将感测输出电流。每安培输出电流将输出 0.13V。 VIN 和 GND 引脚提供与我们为电机提供的输入电压相同的输入电压。我们不在这里使用这些引脚。 +5V(IN) 和 GND 引脚是电机驱动器 IC 的电源。电机驱动器和电机的电源不同。

下表显示了输入和输出组合的真值表：

INA	INB	DIAGA/ENA	DIAGB/ENB	OUTA	输出	CS	操作模式
1	1	1	1	H	H	高 Imp	到 VCC 的断路器
1	0	1	1	H	L	`Isense = Iout / K`	顺时针（CW）
0	1	1	1	L	H	`Isense = Iout / K`	逆时针（CCW）
0	0	1	1	L	L	高 Imp	到 GND 的断路器

值 DIAG/EN 引脚始终为高电平，因为这些引脚在驱动器板本身中被外部拉高。使用上述信号组合，我们可以在任何方向上移动机器人，并且通过调节 PWM 信号，我们也可以调节电动机的速度。这是使用 H 桥电路控制 DC 电动机的基本逻辑。

在将电机连接到 Launchpad 时，我们可能需要一个电平转换器。这是因为启动板的输出引脚只能提供 3.3V 电压，而电机驱动器需要 5V 才能触发；因此，我们必须将 3.3V 连接到 5V 逻辑电平转换器才能开始工作。

这两个电动机以差动驱动机构工作。下一节讨论差速驱动器及其操作。

差动轮式机器人

我们设计的机器人是差速轮/驱动机器人。在差动轮式机器人中，运动基于放置在机器人身体两侧的两个独立驱动的轮。它可以通过改变车轮的相对旋转速度来改变方向，因此不需要额外的转向运动。为了平衡机器人，可以添加自由转动的车轮或脚轮。下图显示了差动驱动器的典型表示：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-H4N7auKq-1681873784540)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00079.gif)]

差动轮式机器人

如果两个电机的方向相同，则机器人将向前或向后移动。如果一台电动机的速度高于另一台电动机，则机器人会转向较慢的电动机侧；否则，机器人会转向较慢的电动机侧。因此，要向左转，请停止左电动机并移动右电动机。下图显示了如何连接机器人中的两个电机。两个电机安装在基板的相对侧，我们在机器人的前后分别放置了两个脚轮以平衡：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-sVrR3Uel-1681873784540)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00080.gif)]

机器人底座的顶视图

接下来，我们可以根据真值表数据使用启动板对电机控制器进行编程。编程使用称为 Energia 的 IDE 完成。我们正在使用 C++ 语言对 Launchpad 进行编程，与 Arduino 开发板非常相似。

安装 Energy IDE

我们可以从以下链接下载最新版本的 Energia。

我们将主要在 64 位 Ubuntu 16.04 LTS 上讨论安装过程。我们将使用的 Energia 版本是 0101E0018：

从前面的链接下载适用于 Linux 的 64 位 Energia。
将 Energia 压缩文件解压缩到用户的 Home 文件夹中。
以下链接给出了设置 Tiva C Launchpad 板的说明
您必须从以下链接下载71-ti-permissions.rules文件
规则文件将授予用户读取和写入启动板的权限。您必须将文件另存为71-ti-permissions.rules并从当前路径执行以下命令，以将规则文件复制到系统文件夹中以获得许可：

$ sudo mv 71-ti-permissions.rules /etc/udev/rules.d/

复制文件后，执行以下命令激活规则：

$ sudo service udev restart

您现在可以将 Tiva C Launchpad 插入 PC 并在 Linux 终端中执行dmesg命令以查看 Linux 内核日志。如果已创建，则在消息末尾将显示一个串行端口设备，如以下屏幕快照所示：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-yVf9AAdT-1681873784540)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00081.jpeg)]

机器人底座的顶视图

如果可以看到串行端口设备，请使用文件夹中的以下命令启动 Energia：

$./energia

以下屏幕截图显示了 Energia IDE：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-bNppjica-1681873784541)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00082.jpeg)]

Energia IDE

现在，我们必须在 IDE 中选择板 tm4c123 来编译特定于该板的代码。为此，我们必须安装此板的包。您可以选择选项“工具 | 板子 | 板子管理器”安装包。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-V5GgVHJ2-1681873784541)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00083.jpeg)]

Energia 板子管理器

安装包后，可以通过导航到“工具 | tm4c123（80MHz）的启动板（Tiva C）”来选择板子，如以下屏幕截图所示：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-7j8StWCe-1681873784541)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00084.jpeg)]

能量板选择

然后，通过导航到“工具 | 串口/dev/ttyACM0”来选择串行端口，如以下屏幕截图所示：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-yPtQKVHb-1681873784542)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00085.jpeg)]

Energia 串口选择

使用“上载”按钮编译并上载代码。上载按钮将完成这两个过程。以下屏幕截图说明了成功的上传：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-aRdc7OgB-1681873784542)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00086.jpeg)]

Energia 串口选择

访问以下链接以在 Linux，macOS X 和 Windows 上安装 Energia：

对于 Linux 请参考这个页面
对于 macOS X，请参考这个页面
对于 Windows 请参考这个页面

电机接口代码

Energia 中的以下代码可用于测试差动驱动配置中的两个电动机。此代码可使机器人向前移动 5 秒钟，向后移动 5 秒钟。然后，它将机器人向左移动 5 秒钟，向右移动 5 秒钟。每次移动后，机器人将停止 1 秒钟。

在代码的开头，我们为两个电机的 INA，INB 和 PWM 定义了引脚，如下所示：

///Left Motor  Pins 
#define INA_1 12 
#define INB_1 13 
#define PWM_1 PC_6 
///Right Motor Pins 
#define INA_2 5 
#define INB_2 6 
#define PWM_2 PC_5

有关启动板的引脚，请参见这个页面。

以下代码显示了使机器人向前，向后，向左和向右移动的五个函数。第五个函数是停止机器人。我们将使用digitalWrite()函数将数字值写入引脚。 digitalWrite()的第一个参数是引脚号，第二个参数是要写入引脚的值。该值可以是HIGH或LOW。我们将使用analogWrite()函数将 PWM 值写入引脚。该函数的第一个参数是引脚号，第二个参数是 PWM 值。此值的范围是 0-255。在高 PWM 下，电机驱动器将快速切换并具有更高的速度。在低 PWM 下，电机驱动器内部的开关将变慢，因此电机也将变慢。目前，我们正在全速运行：

void move_forward() 
{ 
    //Setting CW rotation to and Left Motor  and CCW to Right Motor  
    //Left Motor  
    digitalWrite(INA_1,HIGH); 
    digitalWrite(INB_1,LOW); 
    analogWrite(PWM_1,255); 
    //Right Motor  
    digitalWrite(INA_2,LOW); 
    digitalWrite(INB_2,HIGH); 
    analogWrite(PWM_2,255);   
} 
/////////////////////////////////////////////////////// 
void move_left() 
{ 
    //Left Motor  
    digitalWrite(INA_1,HIGH); 
    digitalWrite(INB_1,HIGH); 
    analogWrite(PWM_1,0); 
    //Right Motor  
    digitalWrite(INA_2,LOW); 
    digitalWrite(INB_2,HIGH); 
    analogWrite(PWM_2,255);   
} 
////////////////////////////////////////////////////// 
void move_right() 
{ 
      //Left Motor  
      digitalWrite(INA_1,HIGH); 
      digitalWrite(INB_1,LOW); 
      analogWrite(PWM_1,255); 
      //Right Motor  
      digitalWrite(INA_2,HIGH); 
      digitalWrite(INB_2,HIGH); 
      analogWrite(PWM_2,0);   
} 
//////////////////////////////////////////////////////// 
void stop() 
{ 
    //Left Motor  
    digitalWrite(INA_1,HIGH); 
    digitalWrite(INB_1,HIGH); 
    analogWrite(PWM_1,0); 
    //Right Motor  
    digitalWrite(INA_2,HIGH); 
    digitalWrite(INB_2,HIGH); 
    analogWrite(PWM_2,0);   
} 
///////////////////////////////////////////////// 
void move_backward() 
{ 
    //Left Motor  
    digitalWrite(INA_1,LOW); 
    digitalWrite(INB_1,HIGH); 
    analogWrite(PWM_1,255); 
    //Right Motor  
    digitalWrite(INA_2,HIGH); 
    digitalWrite(INB_2,LOW); 
    analogWrite(PWM_2,255);   
}

我们首先将两个电机的INA和INB引脚设置为OUTPUT模式，以便我们可以将HIGH或LOW值写入这些引脚。 pinMode()函数用于设置 I/O 引脚的模式。 pinMode()的第一个参数是引脚号，第二个参数是模式。我们可以将引脚设置为输入或输出。要将引脚设置为输出，请给OUTPUT自变量作为第二个自变量。要将其设置为输入，请给INPUT作为第二个参数，如以下代码所示。无需将 PWM 引脚设置为输出，因为analogWrite()无需设置pinMode()就写入 PWM 信号：

void setup() 
{ 
   //Setting Left Motor pin as OUTPUT 
    pinMode(INA_1,OUTPUT);  
    pinMode(INB_1,OUTPUT); 
    pinMode(PWM_1,OUTPUT); 
   //Setting Right Motor pin as OUTPUT 
    pinMode(INA_2,OUTPUT); 
    pinMode(INB_2,OUTPUT); 
    pinMode(PWM_2,OUTPUT);   
}

以下代码段是代码的主要循环。它将在5秒内调用每个函数，例如move forward()，move_backward()，move_left()和move_right(),。调用每个函数后，机器人会在1秒内停止：

void loop() 
{ 
  //Move forward for 5 sec 
move_forward(); 
delay(5000); 
  //Stop for 1 sec 
stop(); 
delay(1000); 
  //Move backward for 5 sec 
move_backward(); 
delay(5000); 
  //Stop for 1 sec 
stop(); 
delay(1000); 
  //Move left for 5 sec 
move_left(); 
delay(5000); 
  //Stop for 1 sec 
stop(); 
delay(1000); 
  //Move right for 5 sec 
move_right(); 
delay(5000);   
  //Stop for 1 sec 
stop(); 
delay(1000); 
}

正交编码器与 Tiva C Launchpad 的接口

车轮编码器是连接到电动机的传感器，用于感应车轮的旋转数。如果我们知道转数，就可以计算出车轮的速度和位移。

对于该机器人，我们选择了带有内置编码器的电机。该编码器是正交类型，可以同时感测电动机的方向和速度。编码器使用不同类型的传感器（例如光学传感器和霍尔传感器）来检测这些参数。该编码器使用霍尔效应来感应旋转。正交编码器具有两个通道，即通道 A 和通道 B。每个通道将生成具有 90 度相移的数字信号。下图显示了典型正交编码器的波形：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-bNNyQ3SV-1681873784542)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00087.gif)]

正交编码器波形

如果电动机沿顺时针方向旋转，则通道 A 将领先通道 B；如果电动机逆时针旋转，则通道 B 将领先通道 A。该读数对于检测电动机的旋转方向很有用。下一节讨论如何将编码器输出转换为有用的测量值，例如位移和速度。

处理编码器数据

编码器数据是两相脉冲，相位相差 90 度。使用此数据，我们可以找到旋转方向以及电动机旋转了多少圈，从而找到了位移和速度。

指定编码器分辨率的一些术语是每转脉冲（PPR）或每转线（LPR）和每转计数（CPR）。 PPR 指定在电动机最终轴旋转一圈期间将有多少个电脉冲（0 到 1 过渡）。一些制造商使用名称 CPR 而不是 PPR，因为每个脉冲将包含两个边沿（上升和下降），并且存在两个具有 90 度相移的脉冲通道（A 和 B）。边的总数将是 PPR 数的四倍。大多数正交接收器使用所谓的 4X 解码来计数来自编码器 A 和 B 通道的所有边沿，与原始 PPR 值相比，产生 4X 分辨率。

在我们的电动机中，Pololu 指定电动机轴的 CPR 为 64，这对应于变速箱输出轴的 8,400 CPR。实际上，当电动机的最终轴完成一圈旋转时，我们将从变速箱输出轴上获得 8400 个计数。下图显示了如何从编码器脉冲中计算计数：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-wD7IkIUB-1681873784542)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00088.gif)]

带计数值的编码器脉冲

在此编码器规范中，给出了每转计数。它是由编码器通道边缘过渡计算的。编码器通道的一个脉冲对应四个计数。因此，要在我们的电机中获得 8,400 个计数，PPR 将为8,400 / 4 = 2,100。根据上图，我们将能够计算出一转的计数数量，但是我们还需要感知运动的方向。这是因为不管机器人向前还是向后移动，我们得到的计数都是相同的；因此，感测方向对于解码信号很重要。下图显示了如何解码编码器脉冲：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-VoAx2eyJ-1681873784543)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00089.gif)]

通过编码器脉冲检测电机方向

如果观察代码模式，我们可以理解它遵循 2 位格雷代码。格雷码是数字的编码，因此相邻数字的一位数字相差 1。格雷码通常在旋转编码器中用于高效编码。

我们可以通过状态转换来预测电动机的旋转方向。状态转换表如下：

状态	顺时针转换	逆时针转换
`0,0`	`0,1`至`0,0`	`1,0`至`0,0`
`1,0`	`0,0`至`1,0`	`1,1`至`1,0`
`1,1`	`1,0`至`1,1`	`0,1`至`1,1`
`0,1`	`1,1`至`0,1`	`0,0`至`0,1`

如果在状态转换图中表示它，将更加方便：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-bwBx0dMJ-1681873784543)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00090.gif)]

编码器状态转换图

收到此格雷码后，我们可以使用微控制器处理脉冲。电机的通道引脚必须连接到微控制器的中断引脚。因此，当通道具有边沿跳变时，它将在引脚上产生中断或触发，并且如果该引脚上有任何中断到达，则将在微控制器程序内执行中断服务程序或简单的功能。它可以读取两个引脚的当前状态。根据引脚的当前状态和先前的值，我们可以确定旋转方向，并可以决定是否要增加或减少计数。这是编码器处理的基本逻辑。

获得计数后，我们可以使用角度 = (计数值 / CPR) * 360来计算旋转角度（以度为单位）。在这里，如果用 8400 代替 CPR，则等式变为角度 = 0.04285 * 计数值；也就是说，要旋转 1 度，必须接收 24 个计数，或者必须产生 6 个编码的通道脉冲。

下图显示了一个电机编码器与 Tiva C LaunchPad 的接口电路：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-NQT1Jg0e-1681873784543)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00091.gif)]

将编码器连接到启动板

在上图中，您可以找到电动机引脚 CH A 和 CH B，它们是电动机编码器的输出。这些引脚连接到 Tiva C Launchpad 的 PB2 和 PB7 引脚。 ENC VCC 和 ENC GND 引脚是编码器的电源引脚，因此我们必须为这些引脚提供 +5V 和 GND。下一组引脚用于为电动机供电。 MOTOR VCC 和 MOTOR GND 分别标记为 OUTA 和 OUTB，它们直接进入电机驱动器以控制电机速度。

编码器输出脉冲的最大电压电平在 0V 至 5V 之间。在这种情况下，我们可以将编码器直接与 Launchpad 相连，因为它可以接收高达 5V 的输入，或者我们可以使用 3.3V 至 5V 的电平转换器，就像我们之前用于电机驱动器接口的一样。

在下一部分中，我们将在 Energia 中上载代码以测试正交编码器信号。我们需要检查是否从编码器获得了正确的计数。

正交编码器接口代码

该代码将通过串行端口打印左右电机编码器的计数。这两个编码器采用 2X 解码方案，因此我们将获得 4,200 CPR。在代码的第一部分中，我们定义了两个编码器的两个通道输出的引脚，并声明了两个编码器的计数变量。编码器变量在变量数据类型之前使用volatile关键字。 volatile的主要用途是带有volatile关键字的变量将存储在 RAM 存储器中，而普通变量位于 CPU 寄存器中。编码器的值将很快变化，因此使用普通变量将不准确。为了获得准确率，我们将volatile用于编码器变量，如下所示：

//Encoder pins definition 
// Left encoder 
#define Left_Encoder_PinA 31 
#define Left_Encoder_PinB 32 
volatile long Left_Encoder_Ticks = 0; 
//Variable to read current state of left encoder pin 
volatile bool LeftEncoderBSet; 
//Right Encoder 
#define Right_Encoder_PinA 33 
#define Right_Encoder_PinB 34 
volatile long Right_Encoder_Ticks = 0; 
//Variable to read current state of right encoder pin 
volatile bool RightEncoderBSet;

以下代码段是setup()函数的定义。在 Energia 中，setup()是一个内置函数，用于初始化以及一次性执行变量和函数。在setup()内部，我们以115200的波特率初始化串行数据通信，并调用用户定义的SetupEncoders()函数来初始化编码器的引脚。串行数据通信主要用于通过串行端子检查编码器计数：

void setup() 
{ 
    //Init Serial port with 115200 buad rate 
  Serial.begin(115200);   
  SetupEncoders(); 
}

SetupEncoders()的定义在下面的代码中给出。要接收编码器脉冲，我们需要在启动板中使用两个引脚作为输入。将编码器引脚配置为 Launchpad 作为输入，并激活其上拉电阻。 attachInterrupt ()函数会将编码器引脚之一配置为中断。 attachInterrupt ()函数具有三个参数。第一个参数是引脚号，第二个参数是中断服务程序（ISR），第三个参数是中断条件，即必须触发 ISR 的中断条件。在此代码中，我们将左右编码器引脚的PinA配置为中断；当脉冲上升时，它将调用 ISR：

void SetupEncoders() 
{ 
  // Quadrature encoders 
  // Left encoder 
  pinMode(Left_Encoder_PinA, INPUT_PULLUP); 
  // sets pin A as input   
  pinMode(Left_Encoder_PinB, INPUT_PULLUP); 
  // sets pin B as input 
  attachInterrupt(Left_Encoder_PinA, do_Left_Encoder, RISING); 
  // Right encoder 
  pinMode(Right_Encoder_PinA, INPUT_PULLUP); 
  // sets pin A as input 
  pinMode(Right_Encoder_PinB, INPUT_PULLUP); 
  // sets pin B as input 
  attachInterrupt(Right_Encoder_PinA, do_Right_Encoder, RISING); 
}

以下代码是 Energia 中内置的loop()函数。 loop()函数是一个无限循环，我们在其中放入了主代码。在此代码中，我们调用Update_Encoders()函数通过串行终端连续打印编码器值：

void loop() 
{ 
  Update_Encoders();                        
}

以下代码是Update_Encoders()函数的功能定义。它在一行中以起始字符e打印两个编码器值，并且这些值由制表符空格分隔。 Serial.print()函数是一个内置函数，它将打印作为参数给出的字符/字符串：

void Update_Encoders() 
{ 
  Serial.print("e"); 
  Serial.print("t"); 
  Serial.print(Left_Encoder_Ticks); 
  Serial.print("t"); 
  Serial.print(Right_Encoder_Ticks); 
  Serial.print("n"); 
 }

以下代码是左右编码器的 ISR 定义。当在每个引脚上检测到上升沿时，将调用一个 ISR。当前中断引脚是每个编码器的PinA。收到中断后，我们可以假设上升的PinA具有较高的值状态，因此无需读取该引脚。读取两个编码器的PinB并将引脚状态存储到LeftEncoderBSet或RightEncoderBSet。将当前状态与PinB的先前状态进行比较，并可以根据状态转换表检测方向，并确定计数是必须增加还是减少：

void do_Left_Encoder() 
{ 
  LeftEncoderBSet = digitalRead(Left_Encoder_PinB); 
  // read the input pin 
  Left_Encoder_Ticks -= LeftEncoderBSet ? -1 : +1; 
} 
void do_Right_Encoder() 
{ 
  RightEncoderBSet = digitalRead(Right_Encoder_PinB); 
  // read the input pin 
  Right_Encoder_Ticks += RightEncoderBSet ? -1 : +1; 
}

上载草图并使用 Energia 中的串行监视器查看输出。导航到工具| 串行监视器。手动移动两个电机，您会看到计数发生变化。在串行监视器中设置波特率，该波特率与代码中初始化的波特率相同；在这种情况下，它是115200。输出将如下所示：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-82qBJrfd-1681873784543)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00092.gif)]

将编码器连接到启动板

如果要将机器人升级到高精度和有效载荷，则必须考虑使用高质量的执行器，例如 Dynamixel。 Dynamixel 伺服器是智能执行器，具有内置的 PID 控制和监视伺服器和编码器参数（例如扭矩，位置等）的功能。在此机器人中，我们不使用 Dynamixel。

使用 Dynamixel 执行器

Dynamixel 是韩国制造商 ROBOTIS 开发的一种用于机器人的网络执行器。它具有通用的扩展功能，功率反馈功能，位置，速度，内部温度，输入电压等，因此被公司，大学和业余爱好者广泛使用。

Dynamixel 伺服器可以菊花链形式连接；它是一种以串行方式连接设备，通过连接的设备将一个设备连接到另一个设备的方法，并且可以从一个控制器控制所有连接的伺服器。 Dynamixel 伺服器通过 RS485 或 TTL 进行通信。可用的 Dynamixel 伺服器列表在这个页面中给出。

Dynamixel 的接口非常简单。 Dynamixel 带有一个称为 USB2Dyanmixel 的控制器，该控制器会将 USB 转换为 Dynamixel 兼容的 TTL/RS485 电平。下图显示了 Dynamixel 的接口图：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-ste2FJpA-1681873784544)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00093.jpeg)]

Dynamixel 执行器与 PC 的接口

ROBOTIS 提供了 Dynamixel SDK，用于访问电机寄存器；我们可以将值读取和写入 Dynamixel 寄存器，并检索数据，例如位置，温度，电压等。

设置 USB2Dynamixel 和 Dynamixel SDK 的说明在这个页面中给出。

Dynamixel 可以使用 Python 库进行编程。用于处理 Dynamixel 伺服器的 Python 库之一是 Pydynamixel。该包可用于 Windows 和 Linux。 Pydynamixel 将支持 RX，MX 和 EX 系列伺服器。

我们可以从这个页面下载 Pydynamixel Python 包。

下载包并将其解压缩到home文件夹。打开终端或 DOS 提示符，然后执行以下命令：

sudo python setup.py install

安装包后，我们可以尝试下面的 Python 示例，该示例将检测连接到 USB2Dynamixel 的伺服器并向该伺服器写入一些随机位置。此示例适用于 RX 和 MX 伺服器：

#!/usr/bin/env python

以下代码将导入此示例所需的必要 Python 模块。这也包括 Dynamixel Python 模块：

import os 
import dynamixel 
import time 
import random

以下代码定义了 Dynamixel 通信参数所需的主要参数。 nServos变量表示连接到总线的 Dynamixel 伺服器的数量。 portName变量指示 Dynamixel 伺服器连接到的 USB2Dynamixel 的串行端口。 baudRate变量是 USB2Dynamixel 和 Dynamixel 的通信速度：

# The number of Dynamixels on our bus. 
nServos = 1 
# Set your serial port accordingly. 
if os.name == "posix": 
    portName = "/dev/ttyUSB0" 
else: 
    portName = "COM6" 
# Default baud rate of the USB2Dynamixel device. 
baudRate = 1000000

以下代码是连接到 Dynamixel 伺服器的 Dynamixel Python 函数。如果已连接，程序将打印它并扫描通信总线以查找从 ID 1到255开始的伺服数。伺服 ID 是每个伺服的标识。我们将nServos设置为1，因此它将在总线上获得一个伺服后停止扫描：

# Connect to the serial port 
print "Connecting to serial port", portName, '...', 
serial = dynamixel.serial_stream.SerialStream( port=portName, baudrate=baudRate, timeout=1) 
print "Connected!" 
net = dynamixel.dynamixel_network.DynamixelNetwork( serial ) 
net.scan( 1, nServos )

以下代码会将 Dynamixel ID 和伺服对象附加到myActutors列表中。我们可以使用伺服 ID 和伺服对象将伺服值写入每个伺服。我们可以使用myActutors列表进行进一步处理：

# A list to hold the dynamixels 
myActuators = list() 
print myActuators 
This will create a list for storing  dynamixel actuators details. 
print "Scanning for Dynamixels...", 
for dyn in net.get_dynamixels(): 
    print dyn.id, 
    myActuators.append(net[dyn.id]) 
print "...Done"

以下代码将向总线上可用的每个 Dynamixel 执行器写入 450 至 600 的随机位置。 Dynamixel 中的位置范围是 0 到 1,023。这将设置伺服参数，例如speed，torque,torque_limt，max_torque等：

# Set the default speed and torque 
for actuator in myActuators: 
    actuator.moving_speed = 50 
    actuator.synchronized = True 
    actuator.torque_enable = True 
    actuator.torque_limit = 800 
    actuator.max_torque = 800

以下代码将打印当前执行器的当前位置：

# Move the servos randomly and print out their current positions 
while True: 
    for actuator in myActuators: 
        actuator.goal_position = random.randrange(450, 600) 
    net.synchronize()

以下代码将从执行器读取所有数据：

for actuator in myActuators: 
        actuator.read_all() 
        time.sleep(0.01) 
    for actuator in myActuators: 
        print actuator.cache[dynamixel.defs.REGISTER['Id']], actuator.cache[dynamixel.defs.REGISTER['CurrentPosition']] 
    time.sleep(2)

使用超声波距离传感器

导航机器人是移动机器人的重要功能之一。理想的导航意味着机器人可以规划其从当前位置到目的地的路径，并且可以毫无障碍地移动。我们在该机器人中使用了超声波距离传感器，以检测使用 Kinect 传感器无法检测到的附近物体。 Kinect 和超声波传感器的组合为该机器人提供了理想的避免碰撞的方法。

超声波距离传感器的工作方式如下。发射器将发送人耳听不到的超声波。发送超声波后，它将等待发射波的回波。如果没有回声，则表示机器人前方没有障碍物。如果接收传感器接收到任何回波，则会在接收器上生成一个脉冲，并且可以计算出波传播到物体并返回到接收器传感器所需的总时间。如果得到这个时间，则可以使用以下公式计算到障碍物的距离：

声速经过的时间/ 2 = 距物体的距离

在这里，声速可以取为 340m/s。

大多数超声波测距传感器的距离范围为 2 厘米至 400 厘米。在此机器人中，我们使用了称为 HC-SR04 的传感器模块。我们将研究如何将 HC-SR04 与 Tiva C LaunchPad 进行接口以获得障碍物的距离。

HC-SR04 与 Tiva C LaunchPad 的接口

下图说明了 HC-SR04 超声波传感器与 Tiva C LaunchPad 的接口电路：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Dtt2qjIS-1681873784544)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00094.gif)]

将超声波传感器连接到 Launchpad

超声波传感器的工作电压为 5V，该传感器的输入/输出也为 5V，因此我们需要 Trig 和 Echo 引脚上的电平转换器，以便与 3.3V 级启动板。如上图所示，在电平转换器中，我们需要施加 5V 的高电压和 3.3V 的低电压，以从一个电平切换到另一电平。 Trig 和 Echo 引脚连接在电平转换器的高压侧，低压侧引脚连接到 Launchpad。 Trig 引脚和 Echo 连接到启动板的第 10 和第 9 引脚。连接传感器后，我们可以看到如何对两个 I/O 引脚进行编程。

HC-SR04 的工作

下图显示了每个引脚上的波形时序。我们需要在触发输入端施加一个 10µs 的短脉冲以开始测距，然后该模块将以 40KHz 的频率发送一个八周期的超声波脉冲并提高其回声。回波是距离对象，它是脉冲宽度和比例范围。您可以使用以下公式计算发送触发信号和接收回波信号之间的时间间隔范围：

范围 = 回波引脚输出的高电平时间 * 速度（340M/S）/ 2

最好在每次触发之前使用 60 毫秒的延迟，以避免触发和回波之间重叠：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-UgDsufoq-1681873784544)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00095.gif)]

超声波传感器的输入输出波形

Tiva C Launchpad 的接口代码

以下用于 Launchpad 的 Energia 代码从超声传感器读取值，并通过串行端口监视值。

以下代码定义了 Launchpad 中用于处理超声回波和触发引脚的引脚，还定义了脉冲持续时间和距离（以厘米为单位）的变量：

const int echo = 9, Trig = 10; 
long duration, cm;

以下代码段是setup()函数。程序启动时将调用setup()函数。使用它来初始化变量，引脚模式，开始使用库等。每次启动或重置启动板后，设置函数将只运行一次。在setup()中，我们以 115200 的波特率初始化串行通信，并通过调用SetupUltrasonic();function:设置超声处理引脚的模式

void setup() 
{ 
  //Init Serial port with 115200 baud rate 
  Serial.begin(115200);   
  SetupUltrasonic();   
}

以下是超声波传感器的设置函数；它将Trigger引脚配置为OUTPUT，将Echo引脚配置为INPUT。 pinMode()函数用于将引脚设置为INPUT或OUTPUT：

void SetupUltrasonic() 
{ 
 pinMode(Trig, OUTPUT); 
 pinMode(echo, INPUT);  
}

创建用于初始化和设置初始值的setup()函数后，loop()函数将按照其名称的含义正确执行并连续循环，以允许您的程序进行更改和响应。使用它来主动控制启动板。

下面的代码是其主要循环。该函数是一个无限循环，并调用Update_Ultra_Sonic()函数通过串行端口更新和打印超声读数：

void loop() 
{ 
    Update_Ultra_Sonic(); 
    delay(200); 
}

以下代码是Update_Ultra_Sonic()函数的定义。此函数将执行以下操作。首先，它将触发引脚置于2微秒的LOW状态，并使10微秒的HIGH状态。在10微秒后，它将再次将引脚恢复为LOW状态。这是根据时序图。我们已经看到触发脉冲宽度为 10µs。

触发 10µs 后，我们必须从 Echo 引脚读取持续时间。持续时间是声音从传感器传播到物体以及从物体传播到传感器接收器所花费的时间。我们可以使用pulseIn()函数读取脉冲持续时间。得到持续时间后，我们可以使用microsecondsToCentimeters()函数将时间转换为厘米，如以下代码所示：

void Update_Ultra_Sonic() 
{ 
  digitalWrite(Trig, LOW); 
  delayMicroseconds(2); 
  digitalWrite(Trig, HIGH); 
  delayMicroseconds(10); 
  digitalWrite(Trig, LOW); 
  duration = pulseIn(echo, HIGH); 
  // convert the time into a distance 
  cm = microsecondsToCentimeters(duration); 
  //Sending through serial port 
  Serial.print("distance="); 
  Serial.print("t"); 
  Serial.print(cm); 
  Serial.print("n"); 
}

以下代码是从微秒到厘米的距离的转换函数。声速为 340m/s，即每厘米 29 微秒。因此，我们通过将总微秒除以 29/2 来获得总距离：

long microsecondsToCentimeters(long microseconds) 
{ 
return microseconds / 29 / 2; 
}

上载代码后，从“工具 | 工具”下的“Energia”菜单打开串行监视器。串行监视器，并将波特率更改为 115200。超声波传感器的值显示在以下屏幕截图中：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-hMhqhsBE-1681873784545)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00096.gif)]

Energia 串行监视器中的超声波距离传感器的输出

Tiva C LaunchPad 与 Python 的接口

在本节中，我们将研究如何将 Tiva C LaunchPad 与 Python 连接以从 PC 中的 Launchpad 接收数据。

PySerial 模块可用于将 Launchpad 与 Python 接口。有关 PySerial 的详细文档及其在 Windows，Linux 和 OS X 上的安装过程，请参见这个页面。

PySerial 在 Ubuntu 包管理器中可用，可以在终端中使用以下命令将其轻松安装在 Ubuntu 中：

$ sudo apt-get install python-serial

安装python-serial包后，我们可以编写 Python 代码以连接 Launchpad。接口代码在下一节中给出。

以下代码导入 Python 的serial模块和sys模块。 serial模块处理 Launchpad 的串行端口并执行诸如读取，写入等操作。 sys模块提供对解释器使用或维护的某些变量以及与解释器强烈交互的功能的访问。它始终可用：

#!/usr/bin/env python 
import serial 
import sys

当我们将 Launchpad 插入计算机时，设备在 OS 上注册为虚拟串行端口。在 Ubuntu 中，设备名称类似于/dev/ttyACMx。这里，x可以是数字；如果只有一台设备，则可能为 0。要与 Launchpad 交互，我们仅需要处理此设备文件。

以下代码将尝试以115200的波特率打开 Launchpad 的串行端口/dev/ttyACM0。如果失败，将显示Unable to open serial port：

try: 
    ser = serial.Serial('/dev/ttyACM0',115200) 
except: 
    print "Unable to open serial port"

以下代码将读取串行数据，直到串行字符换行（'n'）并将其打印在终端上为止。如果我们按键盘上的Ctrl + C组合键以退出程序，它将通过调用sys.exit(0)退出：

while True: 
    try: 
        line = ser.readline() 
        print line 
    except: 
        print "Unable to read from device" 
        sys.exit(0)

保存文件后，使用以下命令将文件的权限更改为可执行文件：

$ sudo chmod +X script_name
    $ ./ script_name

脚本的输出将如下所示：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-4CU6YlaG-1681873784545)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00097.gif)]

Energia 串行监视器中的超声波距离传感器的输出

使用红外接近传感器

红外传感器是查找障碍物和距机器人的距离的另一种方法。红外距离传感器的原理基于撞击障碍物时从表面反射的红外光。红外接收器将捕获反射光，并根据接收到的光量测量电压。

夏普 GP2D12 是最受欢迎的红外范围传感器之一。产品链接可以在这里找到。

下图显示了 Sharp GP2D12 传感器：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-XhyBYvij-1681873784545)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00098.gif)]

传感器发出一束红外光，并使用三角测量法测量距离。 GP2D12 的检测范围在 10 厘米至 80 厘米之间。光束是 6 厘米宽，相距 80 厘米。下图说明了红外光传感器的透射和反射：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-wtbRKI7G-1681873784549)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00099.gif)]

使用红外光传感器进行障碍物感应

传感器的左侧是一个红外发射器，它连续发送红外辐射。撞到某些物体后，红外光将反射并被红外接收器接收。红外传感器的接口电路如下所示：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-PiT9VPHi-1681873784550)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00100.jpeg)]

夏普红外传感器的引脚排列

模拟输出引脚 Vo 可以连接到 Launchpad 的 ADC 引脚。本节将进一步讨论 Sharp 距离传感器与 Tiva C Launchpad 的接口代码。在此代码中，我们选择 Launchpad 的第 18 引脚并将其设置为 ADC 模式，并从 Sharp 距离传感器读取电压电平。 GP2D12 红外传感器的距离公式如下：

范围 = (6,787 / (Volt - 3)) - 4

此处，电压是来自电压引脚的 ADC 的模拟电压值。

在代码的第一部分中，我们将 Tiva C LaunchPad 的第 18 引脚设置为输入引脚，并以 115200 的波特率启动串行通信：

int IR_SENSOR = 18; // Sensor is connected to the analog A3 
int intSensorResult = 0; //Sensor result 
float fltSensorCalc = 0; //Calculated value 
void setup() 
{ 
Serial.begin(115200); // Setup communication with computer 
   to present results serial monitor 
}

在下面的代码部分中，控制器不断读取模拟引脚并将其转换为以厘米为单位的距离值：

void loop() 
{ 
// read the value from the ir sensor 
intSensorResult = analogRead(IR_SENSOR); //Get sensor value 
//Calculate distance in cm according to the range equation 
fltSensorCalc = (6787.0 / (intSensorResult - 3.0)) - 4.0;  
Serial.print(fltSensorCalc); //Send distance to computer 
Serial.println(" cm"); //Add cm to result 
delay(200); //Wait 
}

这是连接锋利距离传感器的基本代码。红外传感器有一些缺点。其中一些如下：

我们不能在直射或间接阳光下使用它们，因此很难在室外机器人中使用它们
如果物体不反射，它们可能不起作用
范围方程式仅在范围内起作用

在下一部分中，我们将讨论 IMU 及其与 Tiva C LaunchPad 的接口。 IMU 可以提供里程计数据，并且可以用作导航算法的输入。

使用惯性测量单元

惯性测量单元（IMU）是一种电子设备，可以结合使用加速度计，陀螺仪和磁力计来测量速度，方向和重力。 IMU 在机器人技术中有很多应用。一些应用被用于平衡无人机（无人机）和机器人导航。

在本节中，我们将讨论 IMU 在移动机器人导航中的作用，以及市场上一些最新的 IMU 及其与 Launchpad 的接口。

惯性导航

IMU 提供相对于惯性空间的加速度和方向。如果知道初始位置，速度和方向，则可以通过积分感应到的加速度来计算速度，第二次积分给出位置。为了获得正确的机器人方向，需要确定机器人的方向。这可以通过积分来自陀螺仪的感测角速度来获得。

下图说明了惯性导航系统，该系统会将 IMU 值转换为里程表数据：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-is8UtHcX-1681873784550)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00101.gif)]

IMU 框图

我们使用导航方程将从 IMU 中获得的值转换为导航信息，并将其输入到估计过滤器（例如卡尔曼过滤器）中。 卡尔曼过滤器是一种根据测量数据估计系统状态的算法。由于加速度计和陀螺仪的误差，惯性导航系统（INS）的数据会有些漂移。为了限制漂移，通常通过直接提供积分量测量的其他传感器辅助 INS。基于测量和传感器误差模型，卡尔曼过滤器估计导航方程式中的误差以及所有有色传感器的误差。下图说明了使用卡尔曼过滤器的辅助惯性导航系统：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-kV1Zhgxu-1681873784550)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00102.gif)]

具有惯性导航系统的 IMU

与电机编码器一起，来自 IMU 的值可以用作里程表的值，并且可以用于航位推算，这是通过使用先前确定的位置来查找运动对象的当前位置的过程。

在下一节中，我们将介绍 InvenSense 最受欢迎的一种 IMU，称为 MPU 6050。

将 MPU 6050 与 Tiva C LaunchPad 连接

MPU-6000/MPU-6050 系列零件是全球首个也是唯一的六轴运动跟踪设备，旨在满足智能手机，平板电脑，可穿戴传感器和机器人的低功耗，低成本和高性能要求。

MPU-6000/6050 器件在硅芯片上结合了三轴陀螺仪和三轴加速度计以及能够处理复杂的九轴运动融合算法的板载数字运动处理器。下图显示了 MPU 6050 和 MPU 6050 的系统框图：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-0GMLqrML-1681873784550)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00103.gif)]

MPU 6050 的框图

MPU 6050 的分支板如下图所示，可以从此处购买：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-7vCuMmYQ-1681873784551)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00104.gif)]

MPU 6050 转接板

下表列出了从 Launchpad 到 MPU 6050 的连接。其余引脚可以断开连接：

Launchpad 引脚	MPU6050 引脚
+3.3V	VCC/VDD
GND	GND
PD0	SCL
PD1	SDA

下图显示了 MPU 6050 和 Tiva C Launchpad 的接口：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-0JonGNs8-1681873784551)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00105.gif)]

将 MPU 6050 转接板连接到 Launchpad

MPU 6050 和 Launchpad 使用 I2C 协议进行通信。电源电压为 3.3V，取自启动板。

在 Energia 中设置 MPU 6050 库

本节讨论 Energia 的接口代码。接口代码使用这个库连接 MPU 6050。

从前面的链接下载 ZIP 文件，然后从“文件 | 文件”导航到“Energia 偏好”，如以下屏幕截图所示：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-BFHb6AU9-1681873784551)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00106.jpeg)]

将 MPU 6050 转接板连接到 Launchpad

如上一个屏幕快照所示，转到“首选项”下的 Sketchbook 位置，并创建一个名为libraries的文件夹。将 ZIP 文件中 Arduino 文件夹中的文件提取到sketchbook/libraries位置。该存储库中的 Arduino 包也与 Launchpad 兼容。提取的文件包含 MPU 6050 传感器接口所需的I2Cdev，Wire和MPU6050包。 libraries文件夹中还有其他传感器包，但我们现在不使用它们。

前面的过程在 Ubuntu 中完成，但对于 Windows 和 macOS X 则相同。

电源接口代码

此代码用于将 MPU 6050 的原始值读取到 Launchpad。它使用了与 Energia IDE 兼容的 MPU 6050 第三方库。以下是每个代码块的说明。

在代码的第一部分中，我们包括用于将 MPU 6050 与 Launchpad 连接的必要标头，例如12C，Wire和MPU6050库，并创建名称为accelgyro的MPU6050对象。 MPU6050.h库包含一个名为MPU6050的类，用于向传感器发送数据和从传感器接收数据：

#include "Wire.h" 
#include "I2Cdev.h" 
#include "MPU6050.h" 
MPU6050 accelgyro;

在下一节中，我们将启动 I2C 和串行通信以与 MPU 6050 进行通信，并通过串行端口打印传感器值。串行通信波特率是115200，Setup_MPU6050()是用于初始化 MPU 6050 通信的自定义函数：

void setup() 
{ 
  //Init Serial port with 115200 buad rate 
  Serial.begin(115200);   
  Setup_MPU6050(); 
}

以下部分是Setup_MPU6050()函数的定义。 Wire库允许您与 I2C 设备进行通信。 MPU 6050 可以使用 I2C 进行通信。 Wire.begin()函数将启动 MPU 6050 和 Launchpad 之间的 I2C 通信；同样，它将使用MPU6050类中定义的initialize()方法初始化 MPU 6050 设备。如果一切成功，它将打印连接成功；否则，它将打印连接失败：

void Setup_MPU6050() 
{ 
   Wire.begin(); 
      // initialize device 
    Serial.println("Initializing I2C devices..."); 
    accelgyro.initialize(); 
    // verify connection 
    Serial.println("Testing device connections..."); 
    Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); 
}

以下代码是loop()函数，该函数连续读取传感器值并通过串行端口打印其值：Update_MPU6050()自定义函数负责从 MPU 6050 打印更新的值：

void loop() 
{ 
   //Update MPU 6050 
    Update_MPU6050(); 
}

Update_MPU6050()的定义如下。它声明了六个变量来处理三轴加速度计和陀螺仪的值。 MPU 6050 类中的getMotion6()函数负责从传感器读取新值。阅读它们后，它将通过串行端口进行打印：

void Update_MPU6050() 
{ 
   int16_t ax, ay, az; 
  int16_t gx, gy, gz; 
      // read raw accel/gyro measurements from device 
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); 
    // display tab-separated accel/gyro x/y/z values 
    Serial.print("i");Serial.print("t"); 
    Serial.print(ax); Serial.print("t"); 
    Serial.print(ay); Serial.print("t"); 
    Serial.print(az); Serial.print("t"); 
    Serial.print(gx); Serial.print("t"); 
    Serial.print(gy); Serial.print("t"); 
    Serial.println(gz); 
    Serial.print("n"); 
}

串行监视器的输出如下所示：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-AZUrpnyS-1681873784551)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00107.gif)]

MPU 6050 在串行监视器中的输出

我们可以使用用于超声波的 Python 代码读取这些值。以下是我们运行 Python 脚本时终端的屏幕截图：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-wFelYTBR-1681873784551)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00108.gif)]

Linux 终端中 MPU 6050 的输出

总结

在本章中，我们讨论了在机器人中使用的电机接口。我们已经研究了电机和编码器与名为 Tiva C Launchpad 的控制器板的接口。我们已经讨论了用于连接电机和编码器的控制器代码。将来，如果机器人需要高精度和高扭矩，我们将研究可以替代当前直流电动机的 Dynamixel 伺服器。我们还研究了可以在我们的机器人中使用的机器人传感器。我们讨论的传感器是超声波距离传感器，IR 接近传感器和 IMU。这三个传感器有助于机器人的导航。我们还讨论了将这些传感器连接到 Tiva C LaunchPad 的基本代码。在下一章中，我们将进一步讨论该机器人中使用的视觉传感器。

问题

什么是 H 桥电路？
什么是正交编码器？
什么是 4X 编码方案？
我们如何从编码器数据计算位移？
Dynamixel 执行器有什么特点？
什么是超声波传感器，它们如何工作？
您如何计算距超声波传感器的距离？
什么是红外接近传感器，它如何工作？

进一步阅读

通过以下链接阅读有关 Energia 编程的更多信息。

七、视觉传感器与 ROS 接口

在上一章中，我们研究了执行器以及如何使用 Tiva-C LaunchPad 板连接机器人的传感器。在本章中，我们将主要介绍视觉传感器及其与机器人一起使用的接口。

我们正在设计的机器人将具有 3D 视觉传感器，并且我们可以将其与视觉库（例如开源计算机视觉（OpenCV），开源自然交互（OpenNI）和点云库（PCL）。 3D 视觉传感器在我们的机器人中的主要应用是自主导航。

我们还将研究如何将视觉传感器与 ROS 接口连接，以及如何使用视觉库（例如 OpenCV）处理它所感测的图像。在本章的最后一部分，我们将研究在机器人中使用的映射和定位算法，称为 SLAM（同时定位和映射），以及如何使用 3D 视觉传感器，ROS 和图像处理库。

在本章中，我们将介绍以下主题：

机器人视觉传感器和图像库列表
OpenCV，OpenNI 和 PCL 简介
ROS-OpenCV 接口
使用 PCL-ROS 接口的点云处理
点云数据到激光扫描数据的转换
SLAM 简介

技术要求

您将需要安装 ROS Kinetic 的 Ubuntu 16.04 系统以及网络摄像头和深度摄像头，才能尝试本章中的示例。

在第一部分中，我们将介绍市场上可用于不同机器人的 2D 和 3D 视觉传感器。

机器人视觉传感器和图像库列表

2D 视觉传感器或普通相机可提供周围环境的 2D 图像帧，而 3D 视觉传感器可提供 2D 图像帧和称为每个图像点深度的附加参数。我们可以找到 3D 传感器相对于传感器轴的每个点的x，y和z距离。

市场上有很多视觉传感器。本章中提到了一些可以在我们的机器人中使用的 2D 和 3D 视觉传感器。

Pixy2/CMUcam5

下图显示了称为 Pixy2/CMUcam5 的最新 2D 视觉传感器，该传感器能够快速，准确地检测颜色对象，并且可以与 Arduino 开发板接口。 Pixy 可用于快速对象检测，并且用户可以教它需要跟踪哪个物体。 Pixy 模块具有一个 CMOS 传感器和基于 Arm Cortex M4/M0 内核的 NXP LPC4330，用于图像处理。下图显示了 Pixy/CMUcam5：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-kruRySQw-1681873784552)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00109.jpeg)]

Pixy/CMUcam5

最常用的 2D 视觉传感器是网络摄像头。它们包含一个 CMOS 传感器和 USB 接口，但没有像 Pixy 一样具有内置的视觉处理功能。

罗技 C920 网络摄像头

下图显示了 Logitech 流行的网络摄像头，该摄像头可以捕获高达 5 百万像素分辨率的图片和高清视频：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-bZVVF2lb-1681873784552)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00110.jpeg)]

罗技 HD C920 网络摄像头

Kinect 360

现在，我们将看一下市场上一些可用的 3D 视觉传感器。一些较流行的传感器是 Kinect，Intel RealSense D400 系列和 Orbbec Astra。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-vVGSyBJ6-1681873784552)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00111.gif)]

Kinect 传感器

Kinect 是最初为 Microsoft Xbox 360 游戏控制台开发的 3D 视觉传感器。它主要包含一个 RGB 相机，一个红外投影仪，一个 IR 深度相机，一个麦克风阵列和一个用于更改其倾斜度的电机。 RGB 摄像头和深度摄像头在 30Hz 时以640 x 480的分辨率捕获图像。 RGB 相机捕获 2D 彩色图像，而深度相机则捕获单色深度图像。 Kinect 的深度感应范围为 0.8m 至 4m。

Kinect 的一些应用是 3D 运动捕获，骨骼跟踪，面部识别和语音识别。

Kinect 可以使用 USB 2.0 接口与 PC 相连，并可以使用 Kinect SDK，OpenNI 和 OpenCV 进行编程。 Kinect SDK 仅适用于 Windows 平台，并且 SDK 由 Microsoft 开发和提供。另外两个库是开源的，可用于所有平台。我们在这里使用的 Kinect 是 Kinect 的第一个版本；最新版本的 Kinect 仅在 Windows 上运行时才支持 Kinect SDK（有关更多详细信息，请参见这个页面）。

Kinect 系列传感器的生产已经停产，但是您仍然可以在 Amazon 和 eBay 上找到该传感器。

英特尔实感 D400 系列

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-diPVIrYo-1681873784552)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00112.jpeg)]

英特尔实感 D400 系列

英特尔实感 D400 深度传感器是带有红外投影仪的立体相机，用于增强深度数据（有关更多详细信息，请参阅这个页面），如图 4 所示。D400 系列中最受欢迎的传感器型号是 D415 和 D435。在图 4 中，左侧的传感器为 D415，右侧的传感器为 D435。每个组件均由一对立体摄像机，一个 RGB 摄像机和一个 IR 投影仪组成。立体摄像机对借助红外投影仪计算环境深度。

这种深度相机的主要特点是它可以在室内和室外环境下工作。它可以以 90 fps 的速度提供1280 x 720分辨率的深度图像流，而 RGB 相机可以提供高达1280 x 720的分辨率。它具有 USB-C 接口，可以在传感器和计算机之间进行快速数据传输。。它外形小巧，轻巧，非常适合机器人视觉应用。

Kinect 和 Intel RealSense 的应用相同，除了语音识别。它们将在 Windows，Linux 和 Mac 上运行。我们可以使用 ROS，OpenNI 和 OpenCV 开发应用。下图显示了 D400 系列摄像机的框图：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-DhxRFdG2-1681873784553)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00113.jpeg)]

英特尔实感 D400 系列的框图

您可以在以下链接找到英特尔实感系列的数据表。

可以在以下链接中找到英特尔实感的深度传感器。

您可以在以下链接中找到英特尔实感 SDK。

Orbbec Astra 深度传感器

新型 Orbbec Astra 传感器是市场上 Kinect 的替代产品之一。与 Kinect 相比，它具有相似的规格，并使用相似的技术来获取深度信息。与 Kinect 相似，它具有 IR 投影仪，RGB 相机和 IR 传感器。它还配有麦克风，可帮助语音识别应用。下图显示了 Orbbec Astra 深度传感器的所有部分：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-OWScN9Tw-1681873784553)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00114.jpeg)]

Orbbec Astra 深度传感器

Astra 传感器有两种型号：Astra 和 AstraS。这两种型号之间的主要区别是深度范围。 Astra 的深度范围为 0.6-8m，而 Astra S 的范围为 0.4-2m。 Astra S 最适合 3D 扫描，而 Astra 可用于机器人应用。阿斯特拉的大小和重量远小于 Kinect。这两个模型都可以提供深度数据和 30 fps 的640 x 480分辨率的 RGB 图像。您可以使用更高的分辨率，例如1280 x 960，但可能会降低帧速率。它们还具有跟踪骨骼的能力，例如 Kinect。

该传感器符合 OpenNI 框架，因此使用 OpenNI 构建的应用也可以使用该传感器。我们将在机器人中使用此传感器。

该 SDK 与 Windows，Linux 和 Mac OS X 兼容。有关更多信息，您可以访问传感器的开发网站。

您还可以参考的传感器之一是 ZED 摄像机。这是一种立体视觉摄像头系统，能够以良好的帧频提供高分辨率。价格约为 450 美元，高于上述传感器。可用于需要传感器具有良好准确率的高端机器人应用。

我们将在接下来的部分中看到该传感器的 ROS 接口。

OpenCV，OpenNI 和 PCL 简介

让我们看一下我们将在机器人中使用的软件框架和库。首先，让我们看一下 OpenCV。这是我们将在此机器人中使用的库之一，用于对象检测和其他图像处理功能。

什么是 OpenCV？

OpenCV 是开放源代码，BSD 许可的计算机视觉库，其中包括数百种计算机视觉算法的实现。该库主要用于实时计算机视觉，是由 Intel Russia 的研究开发的，现在得到 Itseez 的积极支持。 2016 年，英特尔收购了 Itseez。

OpenCV 主要用 C 和 C++ 编写，其主要接口是 C++。它还具有 Python，Java 和 MATLAB/Octave 的良好接口，还具有其他语言（如 C# 和 Ruby）的包装。

在最新版本的 OpenCV 中，支持 CUDA 和 OpenCL 以启用 GPU 加速。

OpenCV 将在大多数操作系统平台（例如 Windows，Linux，Mac OS X，Android，FreeBSD，OpenBSD，iOS 和 BlackBerry）上运行。

在 Ubuntu 中，当我们安装ros-kinetic-desktop-full或ros-melodic-desktop-full包时，已经安装了 OpenCV，Python 包装程序和 ROS 包装程序。以下命令分别安装 OpenCV-ROS 包。

在动力学中：

$ sudo apt-get install ros-kinetic-vision-opencv

在旋律中：

$ sudo apt-get install ros-melodic-vision-opencv

如果要验证系统上已安装 OpenCV-Python 模块，请使用 Linux 终端，然后输入python命令。然后，您应该看到 Python 解释器。尝试在 Python 终端中执行以下命令以验证 OpenCV 安装：

>>> import cv2
    >>> cv2.__version__

如果此命令成功，则将在您的系统上安装此版本的 OpenCV。版本可能是 3.3.x 或 3.2.x。

如果要在 Windows 中尝试 OpenCV，可以尝试以下链接。

以下链接将指导您完成 Mac OS X 上 OpenCV 的安装过程。

OpenCV 的主要应用在以下领域：

对象检测
手势识别
人机交互
移动机器人
运动追踪
面部识别系统

从 Ubuntu 中的源代码安装 OpenCV

OpenCV 安装可以自定义。如果要自定义 OpenCV 安装，则可以尝试从源代码安装它。您可以在这个页面中找到如何进行此安装的方法。

要使用本章中的示例，最好将 OpenCV 与 ROS 一起使用。

使用 Python-OpenCV 接口读取和显示图像

第一个示例将以灰度加载图像并将其显示在屏幕上。

在下面的代码部分中，我们将导入numpy模块来处理图像数组。 cv2模块是 Python 的 OpenCV 包装器，我们可以使用它来访问 OpenCV Python API。 NumPy 是 Python 编程语言的扩展，增加了对大型多维数组和矩阵的支持，以及对这些数组进行操作的大型高级数学函数库（请参见这个页面了解更多信息）：

#!/usr/bin/env python 
import numpy as np 
import cv2

以下函数将读取robot.jpg图像并以灰度加载该图像。 cv2.imread()函数的第一个参数是图像的名称，下一个参数是指定加载图像颜色类型的标志。如果该标志大于 0，则该图像返回一个三通道 RGB 彩色图像；否则，返回 0。如果标志为 0，则加载的图像将为灰度图像；如果该标志小于 0，它将返回与加载的图像相同的图像：

img = cv2.imread('robot.jpg',0)

以下代码部分将使用imshow()函数显示读取的图像。 cv2.waitKey(0)函数是键盘绑定函数。其参数是时间（以毫秒为单位）。如果为 0，它将无限期地等待击键：

cv2.imshow('image', img) 
cv2.waitKey(0)

cv2.destroyAllWindows()函数只是破坏了我们创建的所有窗口：

cv2.destroyAllWindows()

将前面的代码另存为image_read.py，然后复制 JPG 文件并将其命名为robot.jpg。使用以下命令执行代码：

$python image_read.py

输出将以灰度加载图像，因为我们在imread()函数中使用了0作为值：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-ZkF7zADO-1681873784553)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00115.jpeg)]

读取图像代码的输出

以下示例将尝试使用开放式网络摄像头。当用户按下任意按钮时，程序将退出。

从网络摄像头捕获

以下代码将使用设备名称为/dev/video0或/dev/video1的网络摄像头捕获图像。

我们需要导入numpy和cv2模块，以从相机捕获图像：

#!/usr/bin/env python 
import numpy as np 
import cv2

以下函数将创建一个VideoCapture对象。 VideoCapture类用于从视频文件或摄像机捕获视频。 VideoCapture类的初始化参数是摄像机的索引或视频文件的名称。设备索引只是用于指定摄像机的数字。第一个摄像机索引为 0，设备名称为/dev/video0-这就是为什么我们将0放在以下代码中的原因：

cap = cv2.VideoCapture(0)

下面的代码节循环执行，以从VideoCapture对象读取图像帧，并显示每个帧。当按下任意键时，它将退出：

while(True): 
    # Capture frame-by-frame 
    ret, frame = cap.read() 
    # Display the resulting frame 
    cv2.imshow('frame', frame) 
    k = cv2.waitKey(30) 
    if k > 0: 
        break

以下是程序输出的屏幕截图：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-8jRqPQTo-1681873784553)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00116.gif)]

视频捕获的输出

您可以在以下位置浏览更多 OpenCV-Python 教程。

在下一节中，我们将研究 OpenNI 库及其应用。

什么是 OpenNI？

OpenNI 是一种多语言，跨平台的框架，它定义 API 以便使用自然交互（NI）编写应用（请参阅这个页面）。自然互动是指人们通过手势，表情和动作进行自然交流的方式，并通过环顾四周并操纵物理对象和材料来发现世界。

OpenNI API 由一组用于编写 NI 应用的接口组成。下图显示了 OpenNI 库的三层视图：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-xSf57n0H-1681873784554)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00117.jpeg)]

OpenNI 框架软件架构

顶层表示实现基于自然交互的应用的应用层。中间层是 OpenNI 层，它将提供与传感器和分析传感器数据的中间件组件交互的通信接口。中间件可用于全身分析，手点分析，手势检测等。中间层组件的一个示例是 NITE，它可以检测手势和骨骼。

底层包含捕获场景的视觉和音频元素的硬件设备。它可以包括 3D 传感器，RGB 相机，IR 相机和麦克风。

OpenNI 的最新版本是 OpenNI 2，它支持华硕 Xtion Pro 和 Primesense Carmine 等传感器。 OpenNI 的第一个版本主要支持 Kinect 360 传感器。

OpenNI 是跨平台的，并且已成功在 Linux，Mac OS X 和 Windows 上进行了编译和部署。

在下一节中，我们将看到如何在 Ubuntu 中安装 OpenNI。

在 Ubuntu 中安装 OpenNI

我们可以将 OpenNI 库与 ROS 包一起安装。 ROS 已经与 OpenNI 进行了接口，但是 ROS 桌面完全安装可能无法安装 OpenNI 包。如果是这样，我们需要从包管理器中安装它。

以下命令将在 Kinetic 和 Melodic 中安装 ROS-OpenNI 库（主要由 Kinect Xbox 360 传感器支持）：

$ sudo apt-get install ros-<version>-openni-launch

以下命令将安装 ROS-OpenNI 2 库（Asus Xtion Pro 和 Primesense Carmine 主要支持）：

$ sudo apt-get install ros-<version>-openni2-launch

可在这个页面获得适用于 Windows，Linux 和 MacOS X 的 OpenNI 的源代码和最新版本。

在下一节中，我们将研究如何安装 PCL。

什么是 PCL？

点云是空间中代表 3D 对象或环境的一组数据点。通常，从诸如 Kinect 和 LIDAR 的深度传感器生成点云。 PCL（点云库）是用于 2D/3D 图像和点云处理的大规模开放项目。 PCL 框架包含执行过滤，特征估计，表面重构，配准，模型拟合和分割的众多算法。使用这些方法，我们可以处理点云，提取关键描述符以根据对象的几何外观识别世界中的对象，从点云创建表面并对其进行可视化。

PCL 是根据 BSD 许可证发行的。它是开源的，可免费用于商业用途，也可免费用于研究用途。 PCL 是跨平台的，已成功编译并部署在 Linux，macOS X，Windows 和 Android/iOS 上。

您可以从这个页面下载 PCL。

PCL 已经集成到 ROS 中。 ROS 完整桌面安装中包含 PCL 库及其 ROS 接口。 PCL 是 ROS 的 3D 处理主干。有关 ROS-PCL 包的详细信息，请参见这个页面。

使用 Python，ROS，OpenCV 和 OpenNI 编程 Kinect

让我们看看如何在 ROS 中与 Kinect 传感器进行交互和使用。 ROS 与 OpenNI 驱动程序捆绑在一起，该驱动程序可以获取 Kinect 的 RGB 和深度图像。 ROS 中的 OpenNI 和 OpenNI 2 包可用于与 Microsoft Kinect，Primesense Carmine，Asus Xtion Pro 和 Pro Live 接口。

当我们安装 ROS 的openni_launch包时，它还将安装其从属包，例如openni_camera。 openni_camera包是 Kinect 驱动程序，用于发布原始数据和传感器信息，而openni_launch包包含 ROS 启动文件。这些启动文件一次启动多个节点，并发布数据，例如原始深度，RGB 和 IR 图像以及点云。

如何启动 OpenNI 驱动

您可以使用 USB 接口将 Kinect 传感器连接到计算机，并使用终端中的dmesg命令确保 PC 上已检测到 Kinect 传感器。设置 Kinect 之后，我们可以启动 ROS 的 OpenNI 驱动程序以从设备获取数据。

以下命令将打开 OpenNI 设备并加载所有 Nodelet（有关更多信息，请参见这个页面），以将原始深度或 RGB/IR 流转换为深度图像，视差图像和点云。 ROS nodelet包旨在提供一种在同一过程中运行多个算法的方法，并且算法之间的复制拷贝为零：

$ roslaunch openni_launch openni.launch

启动驱动程序后，可以使用以下命令列出驱动程序发布的各种主题：

$ rostopic list

您可以使用名为image_view的 ROS 工具查看 RGB 图像：

$ rosrun image_view image_view image:=/camera/rgb/image_color

在下一部分中，我们将学习如何将这些图像与 OpenCV 进行图像处理。

带有 OpenCV 的 ROS 接口

OpenCV 还集成到 ROS 中，主要用于图像处理。 vision_opencv ROS 栈包括完整的 OpenCV 库和与 ROS 的接口。

vision_opencv meta 包由各个包组成：

cv_bridge：包含CvBridge类。此类将 ROS 图像消息转换为 OpenCV 图像数据类型，反之亦然。
image_geometry：包含处理图像和像素几何的方法的集合。

下图显示了 OpenCV 如何与 ROS 接口：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-N6pA4xaa-1681873784554)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00118.gif)]

OpenCV-ROS 接口

OpenCV 的图像数据类型为IplImage和Mat。如果要在 ROS 中使用 OpenCV，则必须将IplImage或Mat转换为 ROS 图像消息。 ROS 包vision_opencv具有CvBridge类；此类可以将IplImage转换为 ROS 图像，反之亦然。一旦从任何一种视觉传感器获得 ROS 图像主题，就可以使用 ROS CvBridge 来将其从 ROS 主题转换为Mat或IplImage格式。

下一节将向您展示如何创建 ROS 包。此包包含一个节点，用于订阅 RGB 和深度图像，处理 RGB 图像以检测边缘并在将所有图像转换为与 OpenCV 等效的图像类型后显示所有图像。

创建具有 OpenCV 支持的 ROS 包

我们可以创建一个名为sample_opencv_pkg的包，该包具有以下依赖项：sensor_msgs，cv_bridge，rospy和std_msgs。 sensor_msgs依赖项定义了常用传感器（包括照相机和扫描激光测距仪）的 ROS 消息。 cv_bridge依赖项是 ROS 的 OpenCV 接口。

以下命令将创建具有上述依赖项的 ROS 包：

$ catkin-create-pkg sample_opencv_pkg sensor_msgs cv_bridge 
 rospy std_msgs

创建包后，在包内创建一个scripts文件夹；我们将其用作保存代码的位置，下一部分将提到该代码。

使用 Python，ROS 和`cv_bridge`显示 Kinect 图像

以下代码段给出了 Python 代码的第一部分。它主要涉及导入rospy，sys，cv2，sensor_msgs，cv_bridge,和numpy模块。 sensor_msgs依赖项导入图像和相机信息类型的 ROS 数据类型。 cv_bridge模块导入了CvBridge类，用于将 ROS 图像数据类型转换为 OpenCV 数据类型，反之亦然：

import rospy
import sys
import cv2
from sensor_msgs.msg import Image, CameraInfo
from cv_bridge import CvBridge, CvBridgeError
from std_msgs.msg import String
import numpy as np

以下代码部分是 Python 中的类定义，我们将使用它们来演示CvBridge函数。该类称为cvBridgeDemo：

class cvBridgeDemo(): 
    def __init__(self): 
        self.node_name = "cv_bridge_demo" 
        #Initialize the ros node 
        rospy.init_node(self.node_name) 
        # What we do during shutdown 
        rospy.on_shutdown(self.cleanup) 
        # Create the cv_bridge object 
        self.bridge = CvBridge() 
        # Subscribe to the camera image and depth topics and set 
        # the appropriate callbacks 
        self.image_sub = 
 rospy.Subscriber("/camera/rgb/image_raw", Image, 
 self.image_callback)        self.depth_sub = 
 rospy.Subscriber("/camera/depth/image_raw", Image, 
 self.depth_callback) 
 #Callback executed when the timer timeout 
      rospy.Timer(rospy.Duration(0.03), self.show_img_cb) 
      rospy.loginfo("Waiting for image topics...")

这是可视化实际 RGB 图像，已处理 RGB 图像和深度图像的回调：

def show_img_cb(self,event): 
         try:  
             cv2.namedWindow("RGB_Image", cv2.WINDOW_NORMAL) 
             cv2.moveWindow("RGB_Image", 25, 75) 
             cv2.namedWindow("Processed_Image", cv2.WINDOW_NORMAL) 
             cv2.moveWindow("Processed_Image", 500, 75) 
             # And one for the depth image 
             cv2.moveWindow("Depth_Image", 950, 75) 
             cv2.namedWindow("Depth_Image", cv2.WINDOW_NORMAL) 
             cv2.imshow("RGB_Image",self.frame) 
             cv2.imshow("Processed_Image",self.display_image) 
             cv2.imshow("Depth_Image",self.depth_display_image) 
             cv2.waitKey(3) 
         except: 
             pass

以下代码提供了 Kinect 彩色图像的回调函数。当在/camera/rgb/image_raw主题上收到彩色图像时，它将调用此。此函数将处理用于边缘检测的彩色框，并显示检测到的边缘和原始彩色图像：

def image_callback(self, ros_image): 
        # Use cv_bridge() to convert the ROS image to OpenCV format 
        try: 
            self.frame = self.bridge.imgmsg_to_cv2(ros_image, "bgr8") 
        except CvBridgeError, e: 
            print e 
       pass 
        # Convert the image to a Numpy array since most cv2 functions 
        # require Numpy arrays. 
        frame = np.array(self.frame, dtype=np.uint8) 
        # Process the frame using the process_image() function 
        self.display_image = self.process_image(frame)

以下代码提供了 Kinect 中深度图像的回调函数。在/camera/depth/raw_image主题上收到深度图像时，它将调用此函数。此函数将显示原始深度图像：

def depth_callback(self, ros_image): 
        # Use cv_bridge() to convert the ROS image to OpenCV format 
        try: 
            # The depth image is a single-channel float32 image 
            depth_image = self.bridge.imgmsg_to_cv2(ros_image, "32FC1") 
        except CvBridgeError, e: 
            print e 
       pass 
        # Convert the depth image to a Numpy array since most cv2 functions 
        # require Numpy arrays. 
        depth_array = np.array(depth_image, dtype=np.float32) 
        # Normalize the depth image to fall between 0 (black) and 1 (white) 
        cv2.normalize(depth_array, depth_array, 0, 1, cv2.NORM_MINMAX) 
        # Process the depth image 
        self.depth_display_image = self.process_depth_image(depth_array)

以下函数称为process_image(),，它将彩色图像转换为灰度，然后使图像模糊，并使用 Canny 边缘过滤器找到边缘：

def process_image(self, frame): 
        # Convert to grayscale 
        grey = cv2.cvtColor(frame, cv.CV_BGR2GRAY) 
        # Blur the image 
        grey = cv2.blur(grey, (7, 7)) 
        # Compute edges using the Canny edge filter 
        edges = cv2.Canny(grey, 15.0, 30.0) 
        return edges

以下函数称为process_depth_image()。它只是返回深度框架：

def process_depth_image(self, frame): 
        # Just return the raw image for this demo 
        return frame

当节点关闭时，以下函数将关闭图像窗口：

def cleanup(self): 
        print "Shutting down vision node." 
        cv2.destroyAllWindows()

以下代码是main()函数。它将初始化cvBridgeDemo()类并调用rospy.spin()函数：

def main(args): 
    try: 
        cvBridgeDemo() 
        rospy.spin() 
    except KeyboardInterrupt: 
        print "Shutting down vision node." 
        cv.DestroyAllWindows() 
if __name__ == '__main__': 
    main(sys.argv)

将前面的代码另存为cv_bridge_demo.py，并使用以下命令更改节点的权限。仅当我们授予rosrun命令可执行权限时，这些节点才可见：

$ chmod +X cv_bridge_demo.py

以下是启动驱动程序和节点的命令。使用以下命令启动 Kinect 驱动程序：

$ roslaunch openni_launch openni.launch

使用以下命令运行节点：

$ rosrun sample_opencv_pkg cv_bridge_demo.py

以下是输出的屏幕截图：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-5RTRNrQZ-1681873784554)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00119.jpeg)]

RGB，深度和边缘图像

将 Orbbec Astra 与 ROS 连接

Kinect 的替代品之一是 Orbbec Astra。有用于 Astra 的 ROS 驱动程序，我们可以看到如何设置该驱动程序并从该传感器获取图像，深度和点云。

安装 Astra–ROS 驱动程序

在这个页面和这个页面中提到了在 Ubuntu 中设置 Astra-ROS 驱动程序的完整说明。安装驱动程序后，可以使用以下命令启动它：

$ roslaunch astra_launch astra.launch

您也可以从 ROS 包系统信息库安装 Astra 驱动程序。这是安装这些包的命令：

$ sudo apt-get install ros-kinetic-astra-camera
    $ sudo apt-get install ros-kinetic-astra-launch

安装这些包后，您必须设置设备的权限才能使用该设备，如这个页面所述。您可以使用终端中的rostopic list命令检查从该驱动程序生成的 ROS 主题。另外，我们可以使用上一节中提到的相同的 Python 代码进行图像处理。

使用 Kinect，ROS，OpenNI 和 PCL 处理点云

3D 点云是将 3D 环境和 3D 对象表示为沿 x，y 和 z 轴的收集点的一种方式。我们可以从各种来源获得点云：要么可以通过编写程序来创建点云，要么可以通过深度传感器或激光扫描仪生成它。

PCL 本机支持 OpenNI 3D 接口。因此，它可以从设备（例如 Prime Sensor 的 3D 相机，Microsoft Kinect 或 Asus Xtion Pro）中获取和处理数据。

PCL 将包含在 ROS 完整桌面安装中。让我们看看如何在 RViz（ROS 中的数据可视化工具）中生成和可视化点云。

打开设备并生成点云

使用以下命令，打开一个新终端并启动 ROS-OpenNI 驱动程序以及点云生成器节点：

$ roslaunch openni_launch openni.launch

此命令将激活 Kinect 驱动程序并将原始数据处理为方便的输出，例如点云。

如果使用的是 Orbbec Astra，则可以使用以下命令：

$ roslaunch astra_launch astra.launch

我们将使用 RViz 3D 可视化工具来查看我们的点云。

以下命令将启动 RViz 工具：

$ rosrun rviz rviz

将固定帧的 RViz 选项（在“全局选项”下“显示”面板的顶部）设置为 camera_link。

在 RViz 面板的左侧面板中，单击“添加”按钮，然后选择 PointCloud2 显示选项。将其主题设置为/camera/depth/points（这是 Kinect 的主题；其他传感器将有所不同）

将 PointCloud2 的颜色转换器更改为 AxisColor。

以下屏幕截图显示了 RViz 点云数据的屏幕截图。您可以看到最近的对象用红色标记，最远的对象用紫色和蓝色标记。 Kinect 前面的对象表示为圆柱体和立方体：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-E4lxIchR-1681873784554)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00120.jpeg)]

在 Rviz 中可视化点云数据

点云数据到激光扫描数据的转换

我们在该机器人中使用 Astra 来复制昂贵的激光测距仪的功能。使用 ROS 的depthimage_to_laserscan包处理深度图像并将其转换为激光扫描仪的等效数据（有关更多信息，请参见这个页面）。

您可以从源代码安装此包，也可以使用 Ubuntu 包管理器。这是从 Ubuntu 包管理器安装此包的命令

$ sudo apt-get install ros-<version>-depthimage-to-laserscan

该包的主要功能是对深度图像的一部分进行切片并将其转换为等效的激光扫描数据类型。 ROS sensor_msgs/LaserScan消息类型用于发布激光扫描数据。此depthimage_to_laserscan包将执行此转换并伪造激光扫描仪数据。可以使用 RViz 查看激光扫描仪的输出。为了运行转换，我们必须启动将执行此操作的转换器节点。为了开始转换，我们必须在启动文件中指定它。以下是启动文件中启动depthimage_to_laserscan转换所需的代码：

<!-- Fake laser --> 
  <node pkg="nodelet" type="nodelet" 
 name="laserscan_nodelet_manager" args="manager"/>  <node pkg="nodelet" type="nodelet" 
 name="depthimage_to_laserscan"        args="load depthimage_to_laserscan/DepthImageToLaserScanNodelet 
 laserscan_nodelet_manager"> 
    <param name="scan_height" value="10"/> 
    <param name="output_frame_id" value="/camera_depth_frame"/> 
    <param name="range_min" value="0.45"/> 
    <remap from="image" to="/camera/depth/image_raw"/> 
    <remap from="scan" to="/scan"/> 
  </node>

深度图像的主题可以在每个传感器中更改。您必须根据您的深度图像主题更改主题名称。

除了启动 Nodelet 之外，我们还需要设置 Nodelet 的某些参数以实现更好的转换。有关每个参数的详细说明，请参见这个页面。

在以下屏幕截图中给出了前一视图的激光扫描。要查看激光扫描，请添加 LaserScan 选项。这类似于我们添加 PointCloud2 选项并将 LaserSan 的 Topic 值更改为/ scan 的方式：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-7bM5NzFX-1681873784554)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00121.jpeg)]

在 Rviz 中可视化激光扫描数据

将 ROS 和 Kinect 用于 SLAM

在我们的机器人中部署视觉传感器的主要目的是检测物体并在环境中导航机器人。 SLAM 是一种算法，用于移动机器人中，以通过跟踪机器人的当前位置来构建未知环境的地图或在已知环境中更新地图。

地图用于计划机器人的轨迹并在此路径中导航。使用地图，机器人将了解环境。移动机器人导航的两个主要挑战是映射和本地化。

映射涉及在机器人周围生成障碍物轮廓。通过映射，机器人将了解世界的外观。定位是估计机器人相对于我们构建的地图的位置的过程。

SLAM 从不同的传感器获取数据并使用它来构建地图。 2D/3D 视觉传感器可用于将数据输入 SLAM。 2D 视觉传感器（例如网络摄像头）和 3D 传感器（例如 Kinect）主要用作 SLAM 算法的输入。

名为 OpenSlam 的 SLAM 库与 ROS 集成为一个名为 Gmapping 的包。 gmapping包提供了一个节点来执行基于激光器的 SLAM 处理，称为slam_gmapping。这可以根据激光和移动机器人收集的位置数据创建 2D 地图。

gmapping包可从这个页面获得。

要使用slam_gmapping节点，我们必须输入机器人的里程数据和水平安装在机器人上的激光测距仪的激光扫描输出。

slam_gmapping节点订阅sensor_msgs/LaserScan消息和nav_msgs/Odometry消息以构建映射（nav_msgs/OccupancyGrid）。可以通过 ROS 主题或服务检索生成的地图。

我们已使用以下启动文件在 Chefbot 中使用 SLAM。该启动文件将启动slam_gmapping节点，并包含开始映射机器人环境所需的参数：

$ roslaunch chefbot_gazebo gmapping_demo.launch

总结

在本章中，我们介绍了可在 Chefbot 中使用的各种视觉传感器。我们在机器人中使用了 Kinect 和 Astra，并了解了 OpenCV，OpenNI，PCL 及其应用。我们还讨论了视觉传感器在机器人导航中的作用，流行的 SLAM 技术及其在 ROS 中的应用。在下一章中，我们将看到机器人的完整接口，并学习如何使用 Chefbot 进行自主导航。

问题

什么是 3D 传感器，它们与普通相机有何不同？
ROS 的主要特点是什么？
OpenCV，OpenNI 和 PCL 有哪些应用？
什么是 SLAM？
什么是 RGB-D SLAM，它如何工作？

进一步阅读

您可以通过以下链接阅读有关 ROS 中的机器人视觉包的更多信息：

八、构建 ChefBot 硬件和软件集成

在第 3 章“使用 ROS 和 URDF 建模差分机器人”中，我们研究了 ChefBot 机箱的设计。在本章中，我们将学习如何使用这些零件组装该机器人。我们还将研究该机器人与 Tiva-C LaunchPad 的传感器和其他电子组件的最终接口。连接后，我们将学习如何将机器人与 PC 连接，以及如何在实际机器人中使用 SLAM 和 AMCL 进行自主导航。

本章将涵盖以下主题：

构建 ChefBot 硬件
配置 ChefBot PC 和包
将 ChefBot 传感器与 Tiva-C Launchpad 连接
ChefBot 的嵌入式代码
了解 ChefBot ROS 包
在 ChefBot 上实现 SLAM
ChefBot 中的自主导航

技术要求

要测试本章中的应用和代码，您将需要安装 ROS Kinetic 的 Ubuntu 16.04 LTS PC 或笔记本电脑。

您还需要装配好的机器人底盘部件来装配机器人。

您应该拥有可以集成在机器人中的所有传感器和其他硬件组件。

我们已经讨论了将各个机器人组件和传感器与 Launchpad 连接的方法。在本章中，我们将尝试连接 ChefBot 的必要机械手组件和传感器，并对其进行编程，使其能够从所有传感器接收值并从 PC 控制信息。 Launchpad 将通过串行端口将所有传感器值发送到 PC，并从 PC 接收控制信息（例如，重置命令，速度数据等）。

从 Tiva C Launchpad 接收到串行端口数据后，ROS Python 节点将接收串行值并将其转换为 ROS 主题。 PC 中还存在其他 ROS 节点，这些节点订阅了这些传感器主题并计算了机器人的里程表。来自车轮编码器的数据和 IMU 值相结合，以计算机器人的里程表。机器人通过订阅超声传感器主题和激光扫描来检测障碍物，并使用 PID 节点控制车轮电机的速度。该节点将线速度命令转换为差速轮速度命令。运行这些节点之后，我们可以运行 SLAM 来绘制区域地图，运行 SLAM 之后，我们可以运行 AMCL 节点以进行本地化和自主导航。

在本章的第一部分“构建 ChefBot 硬件”中，我们将学习如何使用机器人的身体部位和电子组件组装 ChefBot 硬件。

构建 ChefBot 硬件

需要配置的机器人的第一部分是基板。基板包括两个电机及其连接的车轮，脚轮和基板支架。下图显示了底板的俯视图和仰视图：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-dAXa8Z8i-1681873784555)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00122.jpeg)]

带有电机，车轮和脚轮的底板

基板的半径为 15 厘米，通过从基板上切下两个部分，将电动机及其连接的车轮安装在板的相对侧。两个橡胶脚轮安装在基板的相对两侧，以实现良好的平衡和对机器人的支撑。我们可以为此机器人选择球形脚轮或橡胶脚轮。两个电动机的导线通过基板中心的孔被引到基板的顶部。为了扩展机器人的各层，我们将放置基板支架以连接以下各层。现在，让我们看一下带有中间板和连接管的下一层。有空心管连接底板和中间板。空心管可以连接到基板支架。

下图显示了中间板和连接管：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-HfsTILRD-1681873784555)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00123.jpeg)]

带连接管的中板

连接管将连接底板和中间板。有四个空心管，用于将底板连接到中间板。这些管子的一端是空心的，可以安装基板支撑，另一端是带有孔的硬塑料配件。中间板没有支撑，但连接管有四个孔：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-R7pik1D6-1681873784555)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00124.jpeg)]

完全组装好的机器人机身

中间板公连接器有助于连接中间板和底板管的顶部。我们可以使用顶板背面的四个支架将顶板安装在中间板管的顶部。我们可以将顶板的母连接器插入顶板支架。现在我们有了机器人的完整组装体。

机械手的底层可用于放置印刷电路板（PCB）和电池。在中间层，我们可以放置 Kinect/Orbecc 和 Intel NUC。如果需要，我们可以放置扬声器和麦克风。我们可以用顶板运送食物。下图显示了机器人的 PCB 原型；它由 Tiva-C LaunchPad，电机驱动器，电平转换器和连接两个电机，超声传感器和 IMU 的设备组成：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-BP6jcqim-1681873784555)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00125.jpeg)]

ChefBot PCB 原型

该板由放置在基板上的 12V 电池供电。这两个电机可以直接连接到 M1 和 M2 公连接器。 NUC PC 和 Kinect 放在中间板上。 LaunchPad 板和 Kinect 应该通过 USB 连接到 NUC PC。 PC 和 Kinect 本身使用相同的 12V 电池供电。我们可以使用铅酸或锂聚合物电池。在这里，我们将铅酸电池用于测试目的。稍后，我们将迁移到锂聚合物电池以获得更好的性能和更好的备用。下图显示了完整的 ChefBot 组装图：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-T2fVGhKE-1681873784555)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00126.jpeg)]

完全组装好的机器人机身

组装好机器人的所有零件之后，我们将开始使用机器人软件。 ChefBot 的嵌入式代码和 ROS 包在chapter_8下的代码中可用。让我们获取该代码并开始使用该软件。

配置 ChefBot PC 并设置 ChefBot ROS 包

在 ChefBot 中，我们使用 Intel 的 NUC PC 来处理机器人传感器数据和数据处理。在购买 NUC PC 之后，我们必须安装 Ubuntu 16.04 LTS。安装 Ubuntu 之后，请安装我们在前面各章中提到的完整 ROS 及其包。我们可以单独配置此 PC，完成所有设置的配置后，可以将其放入机器人中。以下是在 NUC PC 上安装 ChefBot 包的过程。

使用以下命令从 GitHub 克隆 ChefBot 的包：

$ git clone https://github.com/qboticslabs/learning_robotics_2nd_ed

我们可以在笔记本电脑中克隆此代码，然后将ChefBot文件夹复制到英特尔的 NUC PC。 ChefBot文件夹由 ChefBot 硬件的 ROS 包组成。在 NUC PC 中，创建一个 ROS catkin 工作区，复制ChefBot文件夹，然后将其移动到 catkin 工作区的src目录中。

只需使用以下命令即可构建并安装 ChefBot 的源代码。这应该在我们创建的catkin工作空间内执行：

$ catkin_make

如果所有依赖项都已正确安装在 NUC 中，则 ChefBot 包将在此系统中生成并安装。在 NUC PC 上设置 ChefBot 包后，我们可以切换到 ChefBot 的嵌入式代码。现在，我们可以连接 LaunchPad 中的所有传感器。将代码上传到 LaunchPad 中之后，我们可以再次查看 ROS 包以及如何运行它们。从 GitHub 克隆的代码包含 Tiva-C LaunchPad 代码，将在下一节中对其进行说明。

将 ChefBot 传感器连接到 Tiva-C LaunchPad

我们已经研究了将在 ChefBot 中使用的各个传感器的接口。在本节中，我们将学习如何将传感器集成到 LaunchPad 板上。 GitHub 上的克隆文件中提供了用于编程 Tiva-C LaunchPad 的 Energia 代码。显示 Tiva-C LaunchPad 与传感器的连接的连接图如下。通过此图，我们了解传感器如何与 LaunchPad 互连：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-5vtS5fY7-1681873784556)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00127.jpeg)]

ChefBot 的传感器接口图

M1 和 M2 是我们在此机器人中使用的两个差动驱动电机。我们将在这里使用的电动机是带有 Pololu 编码器的直流齿轮电动机。电机端子连接到 Pololu 的双 VNH2SP30 电机驱动器。其中一台电机极性相反，因为在差速转向中，一台电机的旋转方向与另一台相反。如果我们向两个电机发送相同的控制信号，则每个电机将以相反的方向旋转。为避免这种情况，我们将交换一台电机的电缆。电机驱动器通过 3.3V-5V 双向电平转换器连接到 Tiva-C LaunchPad。我们将在此处使用的电平转换器之一可在这个页面上找到。

每个编码器的两个通道使用电平转换器连接到 LaunchPad。目前，我们正在使用一个超声波距离传感器进行障碍物检测。将来，如果需要，我们可以增加传感器的数量。为了获得良好的里程估计，我们将 IMU 传感器 MPU 6050 通过 I2C 接口放置。这些引脚直接连接到 LaunchPad，因为 MPU6050 兼容 3.3V。为了从 ROS 节点重置 LaunchPad，我们分配了一个引脚作为输出并将其连接到 LaunchPad 的重置引脚。当特定字符发送到 LaunchPad 时，它将输出引脚设置为高电平并重置设备。在某些情况下，计算错误可能会累积并影响机器人的导航。我们正在重置 LaunchPad 以清除此错误。为了监视电池电量，我们分配了另一个引脚来读取电池值。 Energia 代码中当前未实现此功能。

您从 GitHub 下载的代码包含嵌入式代码和编译此代码所需的从属库。我们可以在这里看到代码的主要部分，并且由于我们已经研究了所有部分，因此无需解释所有部分。

ChefBot 的嵌入式代码

本节将讨论 LaunchPad 代码的主要部分。以下是代码中使用的头文件：

//Library to communicate with I2C devices 
#include "Wire.h" 
//I2C communication library for MPU6050 
#include "I2Cdev.h" 
//MPU6050 interfacing library 
#include "MPU6050_6Axis_MotionApps20.h" 
//Processing incoming serial data 
#include <Messenger.h> 
//Contain definition of maximum limits of various data type 
#include <limits.h>

该代码中使用的主要库用于与 MPU 6050 进行通信并处理输入到 LaunchPad 的串行数据。 MPU 6050 可以使用内置的数字运动处理器（DMP）提供四元数或欧拉值的方向。访问 DMP 的功能写在MPU6050_6Axis_MotionApps20.h中。该库具有诸如I2Cdev.h和Wire.h之类的依赖项；这就是为什么我们还要包含此标头的原因。这两个库用于 I2C 通信。 Messenger.h库可让您处理来自任何来源的文本数据流，并将帮助您从中提取数据。 limits.h标头包含各种数据类型的最大限制的定义。

包含头文件后，我们需要创建一个对象来处理 MPU6050，并使用Messenger类处理传入的串行数据：

//Creating MPU6050 Object 
MPU6050 accelgyro(0x68); 
//Messenger object 
Messenger Messenger_Handler = Messenger();

声明了Messenger对象后，主要部分处理了电机驱动器，编码器，超声传感器，MPU 6050，复位和电池引脚的分配引脚。分配引脚后，我们可以查看代码的setup()函数。 setup()函数的定义在以下代码中给出：

//Setup serial, encoders, ultrasonic, MPU6050 and Reset functions 
void setup() 
{ 
  //Init Serial port with 115200 baud rate 
  Serial.begin(115200); 
  //Setup Encoders 
  SetupEncoders(); 
  //Setup Motors 
  SetupMotors(); 
  //Setup Ultrasonic 
  SetupUltrasonic(); 
  //Setup MPU 6050 
  Setup_MPU6050(); 
  //Setup Reset pins 
  SetupReset(); 
  //Set up Messenger object handler 
  Messenger_Handler.attach(OnMssageCompleted); 
}

前面的函数包含一个自定义例程，用于配置和分配所有传感器的引脚。此函数将以 115,200 的波特率初始化串行通信，并设置编码器，电机驱动器，超声传感器和 MPU6050 的引脚。 SetupReset()函数将分配一个引脚以复位设备，如前面的连接图所示。在前面的章节中，我们已经看到了每个传感器的设置例程，因此无需解释每个函数的定义。 Messenger类处理器附加到名为OnMssageCompleted()的函数，该函数将在将数据输入Messenger_Handler时调用。

以下是代码的主要loop()函数。该函数的主要目的是读取和处理串行数据，以及发送可用的传感器值：

void loop() 
{ 
    //Read from Serial port 
    Read_From_Serial(); 
    //Send time information through serial port 
    Update_Time(); 
    //Send encoders values through serial port 
    Update_Encoders(); 
    //Send ultrasonic values through serial port 
    Update_Ultra_Sonic(); 
    //Update motor speed values with corresponding speed received from PC and send speed values through serial port 
    Update_Motors(); 
    //Send MPU 6050 values through serial port 
    Update_MPU6050(); 
    //Send battery values through serial port 
    Update_Battery(); 
}

Read_From_Serial()函数将从 PC 读取串行数据，并将数据馈送到Messenger_Handler处理器以进行处理。 Update_Time()函数将在嵌入式板上的每次操作后更新时间。我们可以将此时间值在 PC 中进行处理，也可以改用 PC 的时间。

我们可以在 Energia 的 IDE 中编译代码，并在 LaunchPad 中刻录代码。上传代码后，我们可以查看用于处理 LaunchPad 传感器值的 ROS 节点。

为 ChefBot 编写 ROS Python 驱动程序

将嵌入式代码上传到 LaunchPad 之后，下一步是处理 LaunchPad 上的串行数据，并将其转换为 ROS 主题以进行进一步处理。 launchpad_node.py ROS Python 驱动程序节点将 Tiva-C LaunchPad 与 ROS 接口。 launchpad_node.py文件位于ChefBot_bringup包中的script文件夹中。以下是launchpad_node.py重要代码部分的说明：

#ROS Python client 
import rospy 
import sys 
import time 
import math 
#This python module helps to receive values from serial port which execute in a thread 
from SerialDataGateway import SerialDataGateway 
#Importing required ROS data types for the code 
from std_msgs.msg import Int16,Int32, Int64, Float32, 
 String, Header, UInt64 
#Importing ROS data type for IMU 
from sensor_msgs.msg import Imu

launchpad_node.py文件导入前面的模块。我们可以看到的主要模块是SerialDataGateway。这是一个自定义模块，编写该模块可通过线程从 LaunchPad 板接收串行数据。我们还需要一些 ROS 数据类型来处理传感器数据。以下代码段给出了节点的main函数：

if __name__ =='__main__': 
  rospy.init_node('launchpad_ros',anonymous=True) 
  launchpad = Launchpad_Class() 
  try: 
    launchpad.Start() 
    rospy.spin() 
  except rospy.ROSInterruptException: 
    rospy.logwarn("Error in main function") 
  launchpad.Reset_Launchpad() 
  launchpad.Stop()

该节点的主要类别为Launchpad_Class()。此类包含启动，停止串行数据并将其转换为 ROS 主题的所有方法。在 main 函数中，我们将创建Launchpad_Class()的对象。创建对象后，我们将调用Start()方法，该方法将启动 Tiva-C LaunchPad 与 PC 之间的串行通信。如果我们通过键入Ctrl + C来中断驱动程序节点，它将重置 LaunchPad 并停止 PC 与 LaunchPad 之间的串行通信。

以下代码段来自Launchpad_Class()的构造器。在以下代码段中，我们将从 ROS 参数中检索 LaunchPad 板的端口和波特率，并使用这些参数初始化SerialDateGateway对象。当任何传入的串行数据到达串行端口时，SerialDataGateway对象将在此类内调用_HandleReceivedLine()函数。此函数将处理串行数据的每一行，并将其提取，转换并插入每种 ROS 主题数据类型的相应标头中：

#Get serial port and baud rate of Tiva C Launchpad 
port = rospy.get_param("~port", "/dev/ttyACM0") 
baudRate = int(rospy.get_param("~baudRate", 115200)) 
################################################################# 
rospy.loginfo("Starting with serial port: 
 " + port + ", baud rate: " + str(baudRate))#Initializing SerialDataGateway object with serial port, baud
  rate and callback function to handle incoming serial dataself._SerialDataGateway = SerialDataGateway(port, 
 baudRate, self._HandleReceivedLine) 
rospy.loginfo("Started serial communication") 
###################################################################Subscribers and Publishers 
#Publisher for left and right wheel encoder values 
self._Left_Encoder = rospy.Publisher('lwheel',Int64,queue_size 
 = 10)self._Right_Encoder = rospy.Publisher('rwheel',Int64,queue_size 
 = 10)

#Publisher for Battery level(for upgrade purpose) 
self._Battery_Level = 
 rospy.Publisher('battery_level',Float32,queue_size = 10) 
#Publisher for Ultrasonic distance sensor 
self._Ultrasonic_Value = 
 rospy.Publisher('ultrasonic_distance',Float32,queue_size = 10) 
#Publisher for IMU rotation quaternion values 
self._qx_ = rospy.Publisher('qx',Float32,queue_size = 10) 
self._qy_ = rospy.Publisher('qy',Float32,queue_size = 10) 
self._qz_ = rospy.Publisher('qz',Float32,queue_size = 10) 
self._qw_ = rospy.Publisher('qw',Float32,queue_size = 10) 
#Publisher for entire serial data 
self._SerialPublisher = rospy.Publisher('serial', 
 String,queue_size=10)

我们将为传感器（例如编码器，IMU 和超声传感器）以及整个串行数据（用于调试）创建 ROS 发布器对象。我们还将把速度命令订阅到机器人的左手轮和右手轮。当速度命令到达主题时，它将调用相应的回调以将速度命令发送到机器人的 LaunchPad：

self._left_motor_speed = rospy.Subscriber('left_wheel_speed',Float32,self._Update_Left_Speed) 
self._right_motor_speed = rospy.Subscriber('right_wheel_speed',Float32,self._Update_Right_Speed)

设置完 ChefBot 驱动程序节点后，我们需要将机器人与 ROS 导航栈连接，以执行自主导航。进行自主导航的基本要求是，机器人驱动程序节点从 ROS 导航栈接收速度命令。可以使用遥控操作机器人。除这些功能外，机器人还必须能够计算其位置或里程数据，并生成要发送到导航栈的 TF 数据。必须有一个 PID 控制器来控制机器人的电动机速度。以下 ROS 包可帮助我们执行这些功能。 differential_drive包包含执行前面的操作的节点。我们将在包中重用这些节点以实现这些功能。您可以在这个页面的 ROS 中找到differential_drive包。

下图显示了这些节点如何相互通信：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-jwCp3aId-1681873784556)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00128.jpeg)]

显示 ROS 节点的机械手框图

ChefBot_bringup包中每个节点的用途如下：

twist_to_motors.py：此节点会将 ROS Twist命令或线速度和角速度转换为单个电动机速度目标。目标速度以~rate（以赫兹为单位）的速率发布，并且在Twist消息停止后发布timeout_ticks时间的速度。以下是此节点将发布和订阅的主题和参数：

发布主题：

lwheel_vtarget(std_msgs/Float32)：这是左轮的目标速度（以 m/s 为单位）。

rwheel_vtarget（std_msgs/Float32）：这是右轮的目标速度（以 m/s 为单位）。

订阅主题：

Twist（geometry_msgs/Twist）：这是机器人的目标Twist命令。该机器人使用 x 方向的线速度和 Twist 消息的角速度θ。

重要的 ROS 参数：

~base_width（float, default: 0.1）：这是机器人两个轮子之间的距离，以米为单位。

~rate（int, default: 50）：这是发布速度目标的速率（赫兹）。

~timeout_ticks（int, default:2）：这是停止 Twist 消息后发布的速度目标消息的编号。

pid_velocity.py：这是一个简单的 PID 控制器，可通过获取车轮编码器的反馈来控制每个电动机的速度。在差动驱动系统中，每个车轮需要一个 PID 控制器。它将从每个车轮读取编码器数据并控制每个车轮的速度。

发布主题：

motor_cmd（Float32）：这是 PID 控制器输出到电动机的最终输出。我们可以使用out_min和out_max ROS 参数更改 PID 输出的范围。

wheel_vel（Float32）：这是机械手轮的当前速度，单位为 m/s。

订阅主题：

wheel（Int16）：本主题是旋转编码器的输出。机器人的每个编码器都有各自的主题。

wheel_vtarget（Float32）：这是目标速度，单位为 m/s。

重要参数：

~Kp（float,default: 10）：该参数是 PID 控制器的比例增益。

~Ki（float, default: 10）：该参数是 PID 控制器的积分增益。

~Kd（float, default: 0.001）：该参数是 PID 控制器的微分增益。

~out_min（float, default: 255）：这是电机速度值的最小限制。此参数将速度值限制为称为wheel_vel主题的电动机。

~out_max（float, default: 255）：这是wheel_vel主题的最大限制（以赫兹为单位）。

~rate（float, default: 20）：这是发布wheel_vel主题的比率。

ticks_meter（float, default: 20）：这是每米的车轮编码器刻度数。这是一个全局参数，因为它也在其他节点中使用。

vel_threshold（float, default: 0.001）：如果机器人速度降至该参数以下，我们将车轮视为静止。如果车轮的速度小于vel_threshold，我们将其视为零。

encoder_min（int, default: 32768）：这是编码器读数的最小值。

encoder_max（int, default: 32768）：这是编码器读数的最大值。

wheel_low_wrap（int, default: 0.3 * (encoder_max - encoder_min) + encoder_min）：这些值决定里程表是处于负方向还是正方向。

wheel_high_wrap（int, default: 0.7 * (encoder_max - encoder_min) + encoder_min）：这些值决定里程表是处于负方向还是正方向。

diff_tf.py：此节点计算里程表的转换并在里程表框架和机器人的基础框架之间广播。

发布主题：

odom（nav_msgs/odometry）：发布里程表（机器人的当前姿势和扭曲）。

tf：这提供了里程表框架和机器人基础链接之间的转换。

订阅主题：

lwheel（std_msgs/Int16），rwheel（std_msgs/Int16）：这些是机器人左右编码器的输出值。

ChefBot_keyboard_teleop.py：此节点使用键盘上的控件发送Twist命令。

发布主题：

cmd_vel_mux/input/teleop（geometry_msgs/Twist）：这将使用键盘命令发布 Twist 消息。

现在，我们已经研究了ChefBot_bringup包中的节点，我们将研究启动文件的功能。

了解 ChefBot ROS 启动文件

现在，我们将研究ChefBot_bringup包的每个启动文件的功能：

robot_standalone.launch：此启动文件的主要功能是启动launchpad_node，pid_velocity，diff_tf,和twist_to_motor之类的节点，以从机器人获取传感器值并将命令速度发送给机器人。
keyboard_teleop.launch：此启动文件将使用键盘开始遥控操作。它启动ChefBot_keyboard_teleop.py节点以执行键盘遥控操作。
3dsensor.launch：此文件将启动 Kinect OpenNI 驱动程序，并开始发布 RGB 和深度流。它还将启动深度激光扫描仪节点，该节点会将点云数据转换为激光扫描数据。
gmapping_demo.launch：此启动文件将启动 SLAM 映射节点以映射机器人周围的区域。
amcl_demo.launch：使用 AMCL，机器人可以定位并预测其在地图上的位置。在地图上定位机器人之后，我们可以命令机器人移至地图上的某个位置。然后，机器人可以自主地从其当前位置移动到目标位置。
view_robot.launch：此启动文件显示 RViz 中的机械手 URDF 模型。
view_navigation.launch：此启动文件显示机器人导航所需的所有传感器。

使用 ChefBot Python 节点和启动文件

我们已经在英特尔的 NUC PC 中设置了 ChefBot ROS 包，并将嵌入式代码上传到 LaunchPad 板上。下一步是将 NUC PC 放在机器人上，配置从笔记本电脑到机器人的远程连接，测试每个节点，并使用 ChefBot 的启动文件执行自主导航。

与 ChefBot 一起使用之前，我们应该拥有的主要设备是好的无线路由器。机器人和远程笔记本电脑必须通过同一网络连接。如果机械手 PC 和远程膝上型计算机位于同一网络上，则用户可以使用 IP 地址通过 SSH 通过 SSH 从远程便携式计算机连接到机械手 PC。在将机器人 PC 放入机器人之前，我们应该将机器人 PC 连接到无线网络，这样一旦将其连接到无线网络，它就会记住连接细节。机器人通电后，PC 应自动连接到无线网络。一旦机器人 PC 连接到无线网络，我们就可以将其放入实际的机器人中。下图显示了机器人和远程 PC 的连接图：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-XzHYut2n-1681873784556)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00129.jpeg)]

机器人与远程 PC 的无线连接图

上图假定 ChefBot 的 IP 为192.168.1.106，远程 PC 的 IP 为192.168.1.101。

我们可以使用 SSH 远程访问 ChefBot 终端。我们可以使用以下命令登录 ChefBot，其中robot是 ChefBot PC 的用户名：

$ ssh robot@192.168.1.106

登录 ChefBot PC 时，它将要求输入机器人 PC 密码。输入机器人 PC 的密码后，我们可以访问机器人 PC 终端。登录到机器人 PC 后，我们可以开始测试 ChefBot 的 ROS 节点，并测试是否从 ChefBot 内部的 LaunchPad 板上接收到串行值。请注意，如果使用的是新终端，则应通过 SSH 重新登录 ChefBot PC。

如果ChefBot_bringup包已正确安装在 PC 上，并且已连接 LaunchPad 板，则在运行 ROS 驱动程序节点之前，我们可以运行miniterm.py工具来检查串行值是否通过 USB 正确到达 PC 。我们可以使用dmesg命令找到串行设备名称。我们可以使用以下命令运行miniterm.py：

$ miniterm.py /dev/ttyACM0 115200

如果显示拒绝权限消息，请在udev文件夹中编写规则，以设置 USB 设备的许可，这在第 6 章“将执行器和传感器与机器人控制器接口”，或者我们可以使用以下命令临时更改权限。在这里，我们假设ttyACM0是 LaunchPad 的设备名称。如果您的 PC 中的设备名称不同，则必须使用该名称代替ttyACM0：

$ sudo chmod 777 /dev/ttyACM0

如果一切正常，我们将获得以下屏幕快照中所示的值：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-XFdVdxiD-1681873784556)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00130.jpeg)]

miniterm.py的输出

字母b用于指示机器人的电池读数；目前，它尚未实现。该值现在设置为零。这些值来自 Tiva C Launchpad。有多种使用微控制器板感测电压的方法。下面给出了一种方法。字母t表示机器人开始运行嵌入式代码后经过的总时间（以微秒为单位）。第二个值是在 LaunchPad 中完成一项完整操作所花费的时间（以秒为单位）。如果我们正在执行机器人参数的实时计算，则可以使用此值。目前，我们尚未使用此值，但将来可能会使用它。字母e分别指示左和右编码器的值。这两个值都为零，因为机器人没有移动。字母u表示超声波距离传感器的值。我们获得的距离值以厘米为单位。字母s表示机器人的当前轮速。该值用于检查目的。实际上，速度是 PC 本身的控制输出。

要将串行数据转换为 ROS 主题，我们必须运行名为launchpad_node.py的驱动器节点。以下代码显示了如何执行此节点。

首先，我们必须在启动任何节点之前运行roscore：

$ roscore

使用以下命令运行launchpad_node.py：

$ rosrun ChefBot_bringup launchpad_node.py

如果一切正常，我们将在运行的终端的节点中获得以下输出：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-q21mr0CN-1681873784557)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00131.jpeg)]

launchpad_node.py的输出

运行launchpad_node.py之后，我们将看到生成以下主题，如以下屏幕截图所示：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-PQUy5vMM-1681873784557)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00132.gif)]

launchpad_node.py生成的主题

我们可以通过订阅/serial主题来查看驱动程序节点接收的串行数据。我们可以将其用于调试目的。如果串行主题显示的数据与miniterm.py中的数据相同，则可以确认节点运行正常。以下屏幕截图是/serial主题的输出：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-MAe2DSQ4-1681873784557)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00133.gif)]

LaunchPad 节点发布的/serial主题的输出

设置ChefBot_bringup包后，我们就可以开始使用 ChefBot 的自主导航了。当前，我们仅访问 ChefBot PC 的终端。为了可视化机器人的模型，传感器数据，地图等，我们必须在用户的 PC 中使用 RViz。我们必须在机器人和用户 PC 中进行一些配置才能执行此操作。应该注意的是，用户的 PC 应具有与 ChefBot PC 相同的软件设置。

我们要做的第一件事是将 ChefBot PC 设置为 ROS 主设备。我们可以通过设置ROS_MASTER_URI值将 ChefBot PC 设置为 ROS 主设备。 ROS_MASTER_URI设置是必需设置；它向节点通知 ROS 主站的统一资源标识符（URI）。当您为 ChefBot PC 和远程 PC 设置相同的ROS_MASTER_URI时，我们可以在远程 PC 中访问 ChefBot PC 的主题。因此，如果我们在本地运行 RViz，则它将可视化 ChefBot PC 中生成的主题。

假定 ChefBot PC IP 为192.168.1.106，远程 PC IP 为192.168.1.10。您可以为 Chefbot PC 和远程 PC 设置一个静态 IP，以便在所有测试中该 IP 始终是相同的，否则，如果它是自动的，则在每个测试中您可能会获得不同的 IP。要在每个系统中设置ROS_MASTER_URI，应在home文件夹中的.bashrc文件中包含以下命令。下图显示了在每个系统中包含.bashrc文件所需的设置：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Slnpzn9Y-1681873784557)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00134.jpeg)]

ChefBot 的网络配置

在每台 PC 的.bashrc底部添加这些行，然后根据您的网络更改 IP 地址。

建立完这些设置后，我们只需在 ChefBot PC 终端上启动roscore，然后在远程 PC 上执行rostopic list命令。

如果您看到任何主题，则设置完成。我们首先可以使用键盘遥控操作机器人，以检查机器人的功能并确认是否获得传感器值。

我们可以使用以下命令启动机器人驱动程序和其他节点。请注意，这应该在使用 SSH 登录后在 ChefBot 终端中执行：

$ roslaunch ChefBot_bringup robot_standalone.launch

启动机械手驱动程序和节点后，使用以下命令启动键盘遥控操作。这也必须在 ChefBot PC 的新终端上完成：

$ roslaunch ChefBot_bringup keyboard_teleop.launch

要激活 Kinect，请执行以下命令。此命令也在 ChefBot 终端上执行：

$roslaunch ChefBot_bringup 3dsensor_kinect.launch

如果您使用的是 Orbecc Astra，请使用以下启动文件启动传感器：

$ roslaunch ChefBot_bringup 3d_sensor_astra.launch

要查看传感器数据，我们可以执行以下命令。这将在 RViz 中查看机械手模型，应在远程 PC 上执行。如果在远程 PC 上设置ChefBot_bringup包，则可以访问以下命令，并通过 ChefBot PC 可视化机器人模型和传感器数据：

$ roslaunch ChefBot_bringup view_robot.launch

以下屏幕截图是 RViz 的输出。我们可以在屏幕截图中看到 LaserScan 和 PointCloud 映射的数据：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-iq194UTZ-1681873784557)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00135.jpeg)]

RViz 中的 ChefBot LaserScan 数据

上面的屏幕截图显示了 RViz 中的 LaserScan。我们需要在 RViz 的左侧部分打上 LaserScan 主题，以显示激光扫描数据。激光扫描数据标记在视口上。如果要查看 Kinect/Astra 中的点云数据，请单击 RViz 左侧的“添加”按钮，然后从弹出窗口中选择 PointCloud2。选择主题，从列表中单击/camera/depth_registered，您将看到类似于以下屏幕快照中所示的图像：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-l16ZJJRQ-1681873784558)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00136.jpeg)]

带有 PointCloud 数据的 ChefBot

使用传感器后，我们可以执行 SLAM 绘制房间的地图。以下过程可帮助我们在此机器人上启动 SLAM。

与 SLAM 一起在 ROS 上绘制房间地图

要执行映射，我们必须执行以下命令。

以下命令在 ChefBot 终端中启动机器人驱动程序：

$ roslaunch ChefBot_bringup robot_standalone.launch

以下命令将启动映射过程。请注意，它应该在 ChefBot 终端上执行：

$ roslaunch ChefBot_bringup gmapping_demo.launch

仅当收到的里程表值正确时，才能进行映射。如果从机器人接收到里程计值，我们将收到有关前一条命令的以下消息。如果收到此消息，我们可以确认 Gmapping 可以正常工作：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-tEB2AhrZ-1681873784558)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00137.jpeg)]

带有 PointCloud 数据的 ChefBot

要开始键盘遥控操作，请使用以下命令：

$ roslaunch ChefBot_bringup keyboard_teleop.launch

要查看正在创建的地图，我们需要使用以下命令在远程系统上启动 RViz：

$ roslaunch ChefBot_bringup view_navigation.launch

在 RViz 中查看机器人后，可以使用键盘移动机器人并查看正在创建的地图。绘制完整个区域的地图后，我们可以在 ChefBot PC 终端上使用以下命令保存地图：

$rosrun map_server map_saver -f ~/test_map

在前面的代码中，test_map是要存储在home文件夹中的地图的名称。以下屏幕快照显示了由机器人创建的房间的地图：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-sOVatgje-1681873784558)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00138.jpeg)]

映射房间

存储地图后，我们可以使用 ROS 处理定位和自主导航功能。

使用 ROS 定位和导航

构建地图后，关闭所有应用，然后使用以下命令重新运行机器人驱动程序：

$ roslaunch ChefBot_bringup robot_standalone.launch

使用以下命令在存储的地图上启动本地化和导航：

$ roslaunch ChefBot_bringup amcl_demo.launch map_file:=~/test_map.yaml

在远程 PC 上使用以下命令开始查看机器人：

$ roslaunch ChefBot_bringup view_navigation.launch

在 RViz 中，我们可能需要使用 2D 姿势估计按钮指定机器人的初始姿势。我们可以使用此按钮在地图上更改机器人姿势。如果机器人能够访问地图，那么我们可以使用 2D Nav Goal 按钮来命令机器人移至所需位置。开始定位时，可以使用 AMCL 算法看到机器人周围的粒子云：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-bZK5JdYB-1681873784558)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00139.gif)]

使用 AMCL 本地化机器人

以下是机器人从当前位置自动导航到目标位置时的屏幕快照。球门位置标记为黑点：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-yWp2WHHq-1681873784558)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00140.jpeg)]

使用地图进行自主导航

从机器人到黑点的黑线是机器人到达目标位置的计划路径。如果机器人无法找到地图，则可能需要微调ChefBot_bringupparam文件夹中的参数文件。有关更多微调的详细信息，可以在这个页面上查看 ROS 上的 AMCL 包。

总结

本章内容涉及组装 ChefBot 的硬件以及将嵌入式代码和 ROS 代码集成到机器人中以执行自主导航。我们看到了使用第 6 章，“将执行器和传感器连接到机器人控制器”的设计制造的机器人硬件零件。我们组装了机器人的各个部分，并连接了我们为机器人设计的原型 PCB。它由 LaunchPad 板，电机驱动器，左移位器，超声波传感器和 IMU 组成。 LaunchPad 板上闪烁着新的嵌入式代码，该代码可以与机器人中的所有传感器进行接口，并可以从 PC 发送或接收数据。

看完嵌入式代码之后，我们将 ROS Python 驱动程序节点配置为与 LaunchPad 板上的串行数据接口。与 LaunchPad 板接口后，我们使用 ROS 存储库中differential_drive包中的节点计算了里程计数据和差分驱动器控制。我们将机器人与 ROS 导航栈连接。这使我们能够使用 SLAM 和 AMCL 进行自主导航。我们还查看了 SLAM 和 AMCL，创建了地图，并命令机器人自主导航。

问题

机械手 ROS 驱动程序节点的用途是什么？
PID 控制器在导航中的作用是什么？
如何将编码器数据转换为里程表数据？
SLAM 在机器人导航中的作用是什么？
AMCL 在机器人导航中的作用是什么？

进一步阅读

您可以从以下链接中了解有关 ROS 中的机器人视觉包的更多信息：

九、使用 Qt 和 Python 为机器人设计 GUI

在上一章中，我们讨论了用于执行自主导航的机器人硬件组件和包的集成。集成之后，下一步是构建 GUI 来控制机器人。我们正在构建一个 GUI，该 GUI 可以充当底层 ROS 命令的触发器。用户可以使用 GUI 按钮来代替在终端上运行所有命令。我们要设计的 GUI 是针对具有九张桌子的典型酒店房间。用户可以在旅馆房间的地图上设置桌子位置，并命令机器人去特定的桌子上运送食物。交付食物后，用户可以命令机器人转到其原始位置。

本章将涵盖以下主题：

在 Ubuntu 上安装 Qt
PyQt 和 PySide 简介
Qt Designer 简介
Qt 信号和插槽
将 Qt UI 文件转换为 Python 文件
使用 ChefBot GUI 应用
RQT 及其功能简介

技术要求

要在本章中测试应用和代码，您需要安装了 ROS Kinetic 的 Ubuntu 16.04 LTS PC /笔记本电脑。

您需要了解已安装的 Qt，PyQt 和 RQT。

当前可用的两个最受欢迎的 GUI 框架是 Qt 和 GTK+。和 GTK+ 是开放源代码，跨平台的用户界面工具包和开发平台。这两个软件框架已在 GNOME 和 KDE 等 Linux 桌面环境中广泛使用。

在本章中，我们将使用 Qt 框架的 Python 绑定来实现 GUI，因为与其他方法相比，Qt 的 Python 绑定更容易开发。我们将研究如何从头开始开发 GUI 并使用 Python 对其进行编程。在讨论了基本的 Python 和 Qt 编程之后，我们将讨论 Qt 和 Python 的 ROS 接口，它们已经在 ROS 中可用。我们将首先了解什么是 Qt UI 框架以及如何在我们的 PC 上安装它。

在 Ubuntu 16.04 LTS 上安装 Qt

Qt 是跨平台的应用框架，被广泛用于开发具有 GUI 界面和命令行工具的应用软件。 Qt 几乎可用于所有操作系统，例如 Windows，macOS X，Android 等。用于开发 Qt 应用的主要编程语言是 C++，但是存在诸如 Python，Ruby，Java 等语言的绑定。让我们看一下如何在 Ubuntu 16.04 上安装 Qt SDK。我们将从 Ubuntu 中的高级打包工具（APT）安装 Qt。 APT 已随 Ubuntu 安装一起提供。因此，对于安装 Qt/Qt SDK，我们可以简单地使用以下命令，该命令将从 Ubuntu 包存储库中安装 Qt SDK 及其必需的依赖项。我们可以使用以下命令安装 Qt 版本 4：

$ sudo apt-get install qt-sdk

此命令将安装整个 Qt SDK 及其项目所需的库。 Ubuntu 存储库中可用的包可能不是最新版本。要获取最新版本的 Qt，我们可以从以下链接下载适用于各种 OS 平台的 Qt 的在线或离线安装程序。

在系统上安装 Qt 之后，我们将看到如何使用 Qt 开发 GUI 并与 Python 交互。

使用 Qt 的 Python 绑定

让我们看看如何连接 Python 和 Qt。通常，Python 中有两个模块可用于连接到 Qt 用户界面。两种最受欢迎的框架是：

PyQt
PySide

PyQt

PyQt 是 Qt 跨平台最流行的 Python 绑定之一。 PyQt 由 Riverbank Computing Limited 开发和维护。它为 Qt 版本 4 和 Qt 版本 5 提供绑定，并随附 GPL（版本 2 或 3）以及商业许可证。 PyQt 可用于 Qt 版本 4 和 5，分别称为 PyQt4 和 PyQt5。这两个模块与 Python 版本 2 和 3 兼容。PyQt 包含 620 多个类，涵盖了用户界面，XML，网络通信，Web 等。

PyQt 在 Windows，Linux 和 macOS X 中可用。这是安装 Qt SDK 和 Python 以便安装 PyQt 的先决条件。 Windows 和 macOS X 的二进制文件位于以下链接中。

我们将看到如何使用 Python 2.7 在 Ubuntu 16.04 上安装 PyQt4。

在 Ubuntu 16.04 LTS 中安装 PyQt

如果要在 Ubuntu/Linux 上安装 PyQt，请使用以下命令。此命令将安装 PyQt 库，其依赖项以及一些 Qt 工具：

$ sudo apt-get install python-qt4 pyqt4-dev-tools

PySide

PySide 是一个开源软件项目，为 Qt 框架提供 Python 绑定。 PySide 项目由诺基亚发起，为多个平台提供了完整的 Qt 绑定。 PySide 中用于包装 Qt 库的技术与 PyQt 不同，但是两者的 API 相似。 Qt 5 当前不支持 PySide。PySide 可用于 Windows，Linux 和 macOSX。以下链接将指导您在 Windows 和 macOS X 上设置 PySide。

PySide 的前提条件与 PyQt 相同。让我们看看如何在 Ubuntu 16.04 LTS 上安装 PySide。

在 Ubuntu 16.04 LTS 上安装 PySide

PySide 包在 Ubuntu 包存储库中可用。以下命令将在 Ubuntu 上安装 PySide 模块和 Qt 工具：

$ sudo apt-get install python-pyside pyside-tools

让我们使用这两个模块，看看两者之间的区别。

使用 PyQt 和 PySide

安装 PyQt 和 PySide 包后，我们将研究如何使用 PyQt 和 PySide 编写 Hello World GUI。 PyQt 和 PySide 之间的主要区别仅在于某些命令。大多数步骤是相同的。让我们看看如何制作 Qt GUI 并将其转换为 Python 代码。

Qt Designer 简介

Qt Designer 是用于将控件设计和插入到 Qt GUI 中的工具。 Qt GUI 基本上是一个 XML 文件，其中包含其组件和控件的信息。使用 GUI 的第一步涉及其设计。 Qt Designer 工具提供各种选项来制作出色的 GUI。

通过在终端上输入designer-qt4命令来启动 Qt Designer。以下屏幕截图显示了运行此命令后将看到的内容：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-0aga92Kl-1681873784559)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00141.jpeg)]

Qt 4 Designer

上面的屏幕快照显示了 Qt Designer 界面。从“新建表单”窗口中选择“窗口小部件”选项，然后单击“创建”按钮。这将创建一个空的小部件；我们可以将各种 GUI 控件从 Qt 4 Designer 的左侧拖动到空窗口小部件。 Qt 小部件是 Qt GUI 的基本构建块。下面的屏幕快照显示了一个从 Qt Designer 的左侧窗口拖动了PushButton的表单：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-PNrZvGBg-1681873784559)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00142.jpeg)]

Qt Designer 小部件表单

我们将要构建的Hello World应用将具有一个PushButton。当我们单击按钮时，Hello World消息将被打印在终端上。在构建Hello World应用之前，我们需要了解什么是 Qt 信号和插槽，因为我们必须使用这些功能来构建Hello World应用。

Qt 信号和插槽

在 Qt 中，使用信号和插槽功能处理 GUI 事件。当事件发生时，会从 GUI 发出信号。 Qt 小部件具有许多预定义的信号，用户可以为 GUI 事件添加自定义信号。插槽是响应特定信号而调用的函数。在此示例中，我们使用PushButton的clicked()信号并为此信号创建一个自定义插槽。

我们可以在此自定义函数中编写自己的代码。让我们看看如何创建按钮，将信号连接到插槽以及将整个 GUI 转换为 Python。以下是创建 Hello World GUI 应用所涉及的步骤：

从 Qt Designer 拖动并创建一个PushButton到空窗体。
为按钮单击事件分配一个插槽，该插槽发出一个称为clicked()的信号。
将设计的 UI 文件保存在.ui扩展名中。
将 UI 文件转换为 Python。
编写自定义插槽的定义。
在定义的插槽/函数内打印Hello World消息。

我们已经将一个按钮从 Qt Designer 拖到一个空的窗口中。按F4键在按钮上插入一个插槽。当我们按下F4键时，PushButton变成红色，我们可以从按钮上拖动一条线并将地面符号放置在主窗口中。在下面的屏幕快照中显示了该内容：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-bKg8dCGv-1681873784559)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00143.jpeg)]

在 Qt 4 Designer 中分配插槽和信号

从左侧选择clicked()信号，然后单击“编辑”按钮以创建一个新的自定义插槽。当我们单击“编辑”按钮时，将弹出另一个窗口以创建自定义函数。您可以通过单击+符号来创建自定义函数。我们创建了一个名为message()的自定义插槽，如以下屏幕截图所示：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-memiWzz5-1681873784559)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00144.jpeg)]

在 Qt 4 Designer 中分配插槽和信号

单击确定按钮，将 UI 文件另存为hello_world.ui，然后退出 Qt Designer。保存 UI 文件后，让我们看看如何将 Qt UI 文件转换为 Python 文件。

从以下链接中了解有关 Qt 信号和插槽的更多信息

https://doc.qt.io/qt-5/signalsandslots.html

将 UI 文件转换为 Python 代码

设计 UI 文件之后，我们可以将 UI 文件转换为其等效的 Python 代码。使用pyuic编译器完成转换。我们在安装 PyQt/PySide 时已经安装了此工具。以下是将 Qt UI 文件转换为 Python 文件的命令。

我们必须对 PyQt 和 PySide 使用不同的命令。以下命令将 UI 转换为其等效的 PyQt 文件：

$ pyuic4 -x hello_world.ui -o hello_world.py

pyuic4是一个 UI 编译器，用于将 UI 文件转换为其等效的 Python 代码。我们需要在-x参数后提及 UI 文件名，并在-o参数后提及输出文件名。

PySide 命令没有太多更改。 PySide 使用pyside-uic代替pyuic4将 UI 文件转换为 Python 文件。其余参数相同：

$ pyside-uic -x hello_world.ui -o hello_world.py

前面的命令将为 UI 文件生成等效的 Python 代码。这将创建一个具有 GUI 组件的 Python 类。生成的脚本将没有自定义函数message()的定义。我们应该添加此自定义函数来生成代码。以下过程将指导您添加自定义函数；因此，当您单击按钮时，将执行自定义函数message()。

在 PyQt 代码中添加插槽定义

这里给出了从 PyQt 生成的 Python 代码。 pyuic4和pyside-uic生成的代码是相同的，只是导入模块名称不同。其他所有部分都相同。使用 PyQt 生成的代码的说明也适用于 PySide 代码。从前面的转换生成的代码如下。代码结构和参数可以根据您设计的 UI 文件进行更改：

from PyQt4 import QtCore, QtGui 
try: 
    _fromUtf8 = QtCore.QString.fromUtf8 
except AttributeError: 
    _fromUtf8 = lambda s: s 
class Ui_Form(object): 
    def setupUi(self, Form): 
        Form.setObjectName(_fromUtf8("Form")) 
        Form.resize(514, 355) 
        self.pushButton = QtGui.QPushButton(Form) 
        self.pushButton.setGeometry(QtCore.QRect(150, 80, 191, 61)) 
        self.pushButton.setObjectName(_fromUtf8("pushButton")) 
        self.retranslateUi(Form) 
        QtCore.QObject.connect(self.pushButton, QtCore.SIGNAL(_fromUtf8("clicked()")), Form.message) 
        QtCore.QMetaObject.connectSlotsByName(Form) 
    def retranslateUi(self, Form): 
        Form.setWindowTitle(QtGui.QApplication.translate("Form", "Form", None, QtGui.QApplication.UnicodeUTF8)) 
       self.pushButton.setText( QtGui.QApplication.translate("Form", "Press", None, QtGui.QApplication.UnicodeUTF8)) 
#This following code should be added manually 
if __name__ == "__main__": 
   import sys 
    app = QtGui.QApplication(sys.argv) 
    Form = QtGui.QWidget() 
    ui = Ui_Form() 
    ui.setupUi(Form) 
    Form.show() 
    sys.exit(app.exec_())

前面的代码是我们在 Qt Designer 应用中设计的 Qt UI 文件的等效 Python 脚本。这是此代码工作的分步过程：

该代码将从if __name__ == "__main__":开始执行。 PyQt 代码中的第一件事是创建一个QApplication对象。 QApplication类管理 GUI 应用的控制流和主要设置。 QApplication类包含主事件循环，其中将处理和调度 Windows 系统和其他来源的所有事件。它还处理应用的初始化和完成。 QApplication类位于QtGui模块内部。此代码创建名为app的QApplication对象。我们必须手动添加主要代码。
Form = QtGui.QWidget()行将创建一个QWidget类的名为Form的对象，该对象存在于QtGui模块内部。 QWidget类是 Qt 所有用户界面对象的基类。它可以从主 Windows 系统接收鼠标和键盘事件。
ui = Ui_Form()行创建一个在代码中定义的Ui_Form()类的名为ui的对象。 Ui_Form()对象可以接受我们在上一行中创建的QWidget类，并且可以在此QWidget对象中添加按钮，文本，按钮控件和其他 UI 组件。 Ui_Form()类包含两个函数：setupUi()和retranslateUi()。我们可以将QWidget对象传递给setupUi()函数。此函数将在此小部件对象上添加 UI 组件，例如按钮，为信号分配插槽等。如果需要，retranslateUi()函数会将 UI 的语言翻译成其他语言。例如，如果需要将英语翻译为西班牙语，则可以在此函数中提及相应的西班牙语单词。
Form.show()行显示带有按钮和文本的最终窗口。

接下来是创建插槽函数，该函数将打印Hello World消息。插槽定义在Ui_Form()类内部创建。以下步骤将名为message()的插槽插入Ui_Form()类。

message()函数定义如下：

def message(self): 
    print "Hello World"

这应该作为函数插入到Ui_Form()类中。另外，在Ui_Form()类内的setupUi()函数中更改以下行：

QtCore.QObject.connect(self.pushButton, QtCore.SIGNAL(_fromUtf8("clicked()")), Form.message)

Form.message参数应替换为self.message参数。前一行将PushBbutton信号clicked(),连接到我们已经在Ui_Form()类中插入的self.message()插槽。

Hello World GUI 应用的操作

将Form.message参数替换为self.message参数后，我们可以执行代码，输出将如下所示：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Fc9mc0Ge-1681873784560)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00145.jpeg)]

运行 Pyqt4 应用

当我们单击“按下”按钮时，它将打印 Hello world 消息。这就是通过 Python 和 Qt 设置自定义 GUI 的全部内容。

在下一节中，我们将看到我们为机器人设计的实际 GUI。

使用 ChefBot 的控件 GUI

在 PyQt 中完成Hello World应用之后，我们现在将讨论用于控制 ChefBot 的 GUI。构建 GUI 的主要用途是创建一种更轻松的方式来控制机器人。例如，如果将机器人部署在旅馆中以提供食物，则控制该机器人的人员不必了解启动和停止该机器人的复杂命令；因此，为 ChefBot 构建 GUI 可以降低复杂性并使用户更轻松。我们计划使用 PyQt，ROS 和 Python 接口构建 GUI。可以在 GitHub 上的以下链接上获得 ChefBot ROS 包。

如果尚未克隆代码，则可以使用以下命令进行克隆：

$ git clone https://github.com/qboticslabs/learning_robotics_2nd_ed.git

GUI 代码robot_gui.py放置在chefbot_bringup包内的scripts文件夹中。

以下屏幕截图显示了我们为 ChefBot 设计的 GUI：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-I93PsrBv-1681873784560)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00146.jpeg)]

运行 Pyqt4 应用

GUI 具有以下功能：

它可以监视机器人电池状态和机器人状态。机器人状态指示机器人的工作状态。例如，如果机器人遇到错误，它将在此 GUI 上指示错误。
它可以命令机器人移到桌子上运送食物。 GUI 上有一个旋转框小部件，用于输入表的位置。当前，我们正在为具有 9 个表的房间计划此 GUI，但是我们可以根据需要将其扩展为任意数量。输入表格编号后，我们可以通过单击“执行”按钮命令机器人进入该表格。机器人将进入该位置。如果要使机器人返回初始位置，可以单击“主页”按钮。如果要取消当前的机器人运动，请单击“取消”以停止机器人。此 GUI 应用的工作如下：

当我们必须在酒店中部署 ChefBot 时，我们要做的第一步是创建房间地图。正确绘制整个房间的地图后，我们必须将地图保存在机器人 PC 上。机械手仅执行一次映射。绘制地图后，我们可以运行定位和导航例程，并命令机器人进入地图上的某个位置。 ChefBot ROS 包随附类似酒店环境的地图和仿真模型。现在，我们可以运行此仿真和本地化以测试 GUI，在下一章中，我们将讨论如何使用 GUI 控制硬件。如果您在本地系统上安装 ChefBot ROS 包，我们可以模拟酒店环境并测试 GUI。

使用以下命令以类似旅馆的安排启动 ChefBot 模拟：

$roslaunch chefbot_gazebo chefbot_hotel_world.launch

启动 ChefBot 模拟之后，我们可以使用已构建的地图运行定位和导航例程。该地图放置在chefbot_bringup包中。我们可以在该包中看到一个map文件夹。在这里，我们将使用此地图执行此测试。我们可以使用以下命令加载本地化和导航例程：

$ roslaunch chefbot_gazebo amcl_demo.launch 
 map_file:=/home/<user_name>/catkin_ws/src/chefbot/chefbot_bringup/map/hotel1.yaml

映射文件的路径可以在不同的系统中更改，因此请使用系统中的路径而不是此路径。

如果提到的路径正确，它将开始运行 ROS 导航栈。如果要在地图上查看机器人的位置或手动设置机器人的初始位置，可以使用以下命令使用 RViz：

$ roslaunch chefbot_bringup view_navigation.launch

在 RViz 中，我们可以使用 2D 导航目标按钮命令机器人转到任何地图坐标。

我们也可以使用编程命令机器人去任何地图坐标。 ROS 导航栈使用 ROS actionlib库工作。 ROS actionlib库用于执行可抢占的任务；它类似于 ROS 服务。相对于 ROS 服务的优势在于，如果我们当时不希望取消请求，可以将其取消。

在 GUI 中，我们可以命令机器人使用 Python actionlib库转到地图坐标。我们可以使用以下技术来获得桌子在地图上的位置。

启动模拟器和 AMCL 节点后，启动键盘遥操作并将机器人移到每个桌子附近。使用以下命令获取机器人的平移和旋转：

$ rosrun tf tf_echo /map /base_link

当我们单击Go按钮时，该位置将被馈送到导航栈，并且机器人将规划其路径并达到其目标。我们甚至可以随时取消任务。因此，ChefBot GUI 充当actionlib客户端，该客户端将地图坐标发送到actionlib服务器；即导航栈。

现在，我们可以使用以下命令运行机器人 GUI 来控制机器人：

$ rosrun chefbot_bringup robot_gui.py

我们可以选择一个表号，然后单击“执行”按钮以将机器人移至每个表。

假设您克隆了文件并获得了robot_gui.py文件，我们将讨论为actionlib客户端添加到Ui_Form()类中的主要插槽，并获取电池和机械手状态的值。

我们需要为此 GUI 应用导入以下 Python 模块：

import rospy 
import actionlib 
from move_base_msgs.msg import * 
import time 
from PyQt4 import QtCore, QtGui

我们需要的其他模块是 ROS Python 客户端rospy和actionlib模块，用于将值发送到导航栈。 move_base_msgs模块包含需要发送到导航栈的目标的消息定义。

Python 字典中提到了每个桌子附近的机器人位置。以下代码显示了每个桌子附近机器人位置的硬编码值：

table_position = dict() 
table_position[0] = (-0.465, 0.37, 0.010, 0, 0, 0.998, 0.069) 
table_position[1] = (0.599, 1.03, 0.010, 0, 0, 1.00, -0.020) 
table_position[2] = (4.415, 0.645, 0.010, 0, 0, -0.034, 0.999) 
table_position[3] = (7.409, 0.812, 0.010, 0, 0, -0.119, 0.993) 
table_position[4] = (1.757, 4.377, 0.010, 0, 0, -0.040, 0.999) 
table_position[5] = (1.757, 4.377, 0.010, 0, 0, -0.040, 0.999) 
table_position[6] = (1.757, 4.377, 0.010, 0, 0, -0.040, 0.999) 
table_position[7] = (1.757, 4.377, 0.010, 0, 0, -0.040, 0.999) 
table_position[8] = (1.757, 4.377, 0.010, 0, 0, -0.040, 0.999) 
table_position[9] = (1.757, 4.377, 0.010, 0, 0, -0.040, 0.999)

通过访问此字典，我们可以访问机器人在每个桌子附近的位置。

当前，我们仅出于演示目的插入了四个值。您可以通过查找其他表的位置来添加更多值。

我们正在分配一些变量来处理Ui_Form()类中的表号，机械手的位置以及actionlib客户端：

#Handle table number from spin box 
self.table_no = 0 
#Stores current table robot position 
self.current_table_position = 0 
#Creating Actionlib client 
self.client = actionlib.SimpleActionClient('move_base',MoveBaseAction) 
#Creating goal message definition 
self.goal = MoveBaseGoal() 
#Start this function for updating battery and robot status 
self.update_values()

以下代码显示了此代码中按钮和旋转框小部件的信号和插槽分配：

#Handle spinbox signal and assign to slot set_table_number() 
QtCore.QObject.connect(self.spinBox, QtCore.SIGNAL(_fromUtf8("valueChanged(int)")), self.set_table_number) 
#Handle Home button signal and assign to slot Home() 
QtCore.QObject.connect(self.pushButton_3, QtCore.SIGNAL(_fromUtf8("clicked()")), self.Home) 
#Handle Go button signal and assign to slot Go() 
QtCore.QObject.connect(self.pushButton, QtCore.SIGNAL(_fromUtf8("clicked()")), self.Go) 
#Handle Cancel button signal and assign to slot Cancel() 
QtCore.QObject.connect(self.pushButton_2, QtCore.SIGNAL(_fromUtf8("clicked()")), self.Cancel)

以下插槽处理 UI 中的旋转框值并分配一个表号。同样，它将表号转换为相应的机器人位置：

def set_table_number(self): 
  self.table_no = self.spinBox.value() 
  self.current_table_position = table_position[self.table_no]

这是“转到”按钮的“转到”插槽的定义。此函数会将所选表格的机械手位置插入目标消息标头中，并将其发送到导航栈中：

def Go(self): 
  #Assigning x,y,z pose and orientation to target_pose message 
  self.goal.target_pose.pose.position.x=float(self.current_table  _position[0]) 
  self.goal.target_pose.pose.position.y=float(self.current_table  _position[1]) 
  self.goal.target_pose.pose.position.z=float(self.current_table  _position[2]) 
  self.goal.target_pose.pose.orientation.x =     float(self.current_table_position[3]) 
  self.goal.target_pose.pose.orientation.y=   float(self.current_table_position[4]) 
  self.goal.target_pose.pose.orientation.z=   float(self.current_table_position[5]) 
  #Frame id 
  self.goal.target_pose.header.frame_id= 'map' 
  #Time stamp 
  self.goal.target_pose.header.stamp = rospy.Time.now() 
  #Sending goal to navigation stack 
  self.client.send_goal(self.goal)

以下代码是Cancel()插槽定义。这将取消当时计划执行的所有机械手路径：

def Cancel(self): 
  self.client.cancel_all_goals()

以下代码是Home()的定义。这会将表格位置设置为零，并调用Go()函数。位置为零的表格是机器人的原始位置：

def Home(self): 
  self.current_table_position = table_position[0] 
  self.Go()

以下定义适用于update_values()和add()函数。 update_values()方法将开始在线程中更新电池电量和机器人状态。 add()函数将检索电池状态和机器人状态的 ROS 参数，并将它们分别设置为进度条和标签：

def update_values(self): 
    self.thread = WorkThread() 
    QtCore.QObject.connect( self.thread,    QtCore.SIGNAL("update(QString)"), self.add ) 
    self.thread.start() 
def add(self,text): 
  battery_value = rospy.get_param("battery_value") 
  robot_status = rospy.get_param("robot_status") 
   self.progressBar.setProperty("value", battery_value) 
     self.label_4.setText(_fromUtf8(robot_status))

此处提供了先前函数中使用的WorkThread()类。 WorkThread()类继承自 Qt 提供的用于线程化的QThread。线程仅以特定的延迟发出信号update(Qstring)。在先前的函数update_values()中，update(QString)信号连接到self.add()插槽；因此，从线程发出信号update(QString)时，它将调用add()插槽并更新电池和状态值：

class WorkThread(QtCore.QThread): 
  def __init__(self): 
    QtCore.QThread.__init__(self) 
   def __del__(self): 
    self.wait() 
   def run(self): 
    while True: 
      time.sleep(0.3) # artificial time delay 
      self.emit( QtCore.SIGNAL('update(QString)'), " " ) 
      return

我们已经讨论了如何为 ChefBot 制作 GUI，但是该 GUI 仅适用于控制 ChefBot 的用户。如果有人要调试和检查机器人数据，则可能需要其他工具。 ROS 提供了一个出色的调试工具，可以可视化来自机器人的数据。

RQT 工具是一种流行的 ROS 工具。它基于用于 ROS 的 GUI 开发的基于 Qt 的框架。让我们讨论一下 RQT 工具，安装过程以及如何检查机器人中的传感器数据。

在 Ubuntu 16.04 LTS 中安装和使用 RQT

RQT 是 ROS 中的软件框架，它以插件的形式实现各种 GUI 工具。我们可以将插件添加为 RQT 中的可停靠窗口。

可以使用以下命令在 Ubuntu 16.04 中安装 RQT。在安装 RQT 之前，请确保已完整安装 ROS Indigo。

$ sudo apt-get install ros-<ros_version>-rqt

安装 RQT 包后，我们可以访问 RQT 的 GUI 实现，称为rqt_gui，在其中我们可以将rqt plugins停靠在单个窗口中。

让我们开始使用rqt_gui。

在运行rqt_gui之前，请运行roscore命令：

$ roscore

运行以下命令以启动rqt_gui：

$ rosrun rqt_gui rqt_gui

如果命令运行正常，我们将获得以下窗口：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-NHHDyGI7-1681873784560)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00147.jpeg)]

运行 RQT

我们可以在运行时加载和卸载插件。要分析 ROS 消息日志，我们可以从“插件 | 加载”来加载 Console 插件。在以下示例中，我们加载控制台插件并在rospy_tutorials内运行一个对话器节点，该节点将向一个名为/chatter的主题发送Hello World消息。

运行以下命令以启动节点talker.py：

$rosrun rospy_tutorials talker.py

在以下屏幕截图中，rqt_gui装有两个名为 Console 和 Topic Monitor 的插件。主题监视器插件可以从插件|插件中加载。话题| 主题监视器。控制台插件监视每个节点上打印的消息及其严重性。这对于调试目的非常有用。在以下屏幕截图中，rqt_gui的左侧部分装有控制台插件，而右侧则装有主题监视器。主题监视器将列出可用的主题，并将监视其值。

在以下屏幕截图中，控制台插件监视talker.py节点的消息及其严重性级别，主题监视器监视/chatter主题内的值：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-i11pOAgz-1681873784560)(https://gitcode.net/apachecn/apachecn-cv-zh/-/raw/master/docs/learn-robot-py/img/00148.jpeg)]

使用不同的插件运行 RQT

我们还可以在rqt_gui上可视化数据，例如图像和曲线图。对于机器人的导航和检查，有一些插件可用于在rqt_gui上嵌入 RViz。导航查看器插件来自/map主题的视图。可视化插件可在“插件 | 插件”中找到。可视化。

我们也可以使用 RQT 创建 GUI。创建可加载到rqt_gui的 RQT 插件的说明可在以下位置找到。

总结

在本章中，我们讨论了为 ChefBot 创建 GUI 的方法，该方法可供不了解机器人内部工作原理的普通用户使用。我们使用称为 PyQt 的 Qt 的 Python 绑定来创建此 GUI。在研究主要的 GUI 设计之前，我们研究了Hello World应用，以更轻松地理解 PyQt。 UI 设计使用 Qt Designer 工具完成，并且使用 Python UI 编译器将 UI 文件转换为等效的 Python 脚本。在 Qt Designer 中设计了主 GUI 之后，我们将 UI 文件转换为 Python 脚本，并在生成的脚本中插入了必要的插槽。 ChefBot GUI 可以启动机器人，选择一个表号，并命令机器人进入该位置。每个表的位置都来自生成的地图，在此我们在此 Python 脚本中对位置进行了硬编码以进行测试。选择表格后，我们在地图上设置了目标位置，然后单击“执行”按钮，机器人将移至预期位置。用户可以随时取消操作，并命令机器人回到原位。 GUI 也可以接收机器人的实时状态及其电池状态。在讨论了机器人 GUI 之后，我们看了 ROS 中的调试 GUI 工具 RQT。我们看到了一些用于调试机器人数据的插件。在下一章中，我们将看到机器人的完整测试和校准。

问题

Linux 平台上有哪些流行的 UI 工具包？
PyQt 和 PySide Qt 绑定之间有什么区别？
如何将 Qt UI 文件转换为 Python 脚本？
什么是 Qt 信号和插槽？
什么是 RQT，它的主要应用是什么？

进一步阅读

在以下链接中了解有关 ROS 中的机器人视觉包的更多信息：

十、评估

第 1 章，机器人操作系统入门

这是 ROS 的三个主要功能：

消息传递接口，可与不同程序通信
现成的机器人算法，使机器人原型制作更快
可视化机器人数据和调试的软件工具

ROS 中概念的不同级别是 ROS 文件系统级别，ROS 计算图级别和 ROS 社区级别。
Catkin 构建系统是使用 CMake 和 Python 脚本构建的。此工具可帮助我们构建 ROS 包。
ROS 主题是一个命名的总线，其中一个节点可以与另一节点通信。主题中使用的消息类型是 ROS 消息。
ROS 计算图的不同概念是 ROS 节点，ROS 主题，ROS 消息，ROS 主控，ROS 服务和
ROS 袋子。
ROS 主机充当连接两个 ROS 节点开始相互通信的中介程序。
Gazebo 的重要特征是：

动态仿真：它包括 ODE，Bullet，Simbody 和 Dart 等物理引擎
先进的 3D 图形：它使用 OGRE 框架创建高质量的照明，阴影和纹理
插件支持：这将使开发人员可以添加新的机器人，传感器和环境控制
TCP/IP 传输：使用基于套接字的消息传递接口控制 Gazebo

第 2 章，了解差动机器人的基础

完整的机器人可以在任何方向上自由移动，并且可控制的自由度等于总自由度。基于 Omni 轮的机器人是完整机器人的示例。非完整机器人对其运动有约束，因此可控制的自由度将不等于总自由度。差分驱动器配置是非完整配置的一个示例。
机器人运动学在不考虑质量和惯性的情况下处理机器人的运动，而机器人动力学是质量和惯性，运动和相关转矩之间的关系。
ICC 代表瞬时曲率中心，它是机器人围绕其旋转的车轮轴上的一个虚点。
这是从车轮速度中查找机器人当前位置的过程。
寻找达到目标位置的车轮速度。

第 3 章，建模差动机器人

机器人建模是创建具有机器人所有参数的 2D 和 3D 机器人表示的过程，其中包括机器人的运动学和动态参数。
2D 模型主要包括机器人零件的精确尺寸，这有助于我们计算机器人的运动学并帮助制造机器人零件。
机器人的 3D 模型是机器人硬件的精确复制品，具有使用 CAD 软件设计的物理机器人的所有参数。这用于创建机器人仿真和机器人的 3D 打印部件。
如果您知道 Blender 脚本 API，则使用 Python 脚本创建 3D 模型比手动建模要容易和准确得多。
URDF 是 ROS 中机器人的 3D 机器人模型表示。它具有机器人的运动学和动态参数。

第 4 章，使用 ROS 模拟差动机器人

可以使用 Gazebo 插件在 Gazebo 中进行传感器建模。传感器模型可以使用 C++ 编写，可以将其插入到 Gazebo 仿真器中。
使用 Gazebo ROS 插件将 ROS 连接到 Gazebo。当我们将此插件加载到 Gazebo 中时，我们可以通过 ROS 接口控制 Gazebo。
重要的标签是<inertia>，<collision>和<gazebo>。
ROS 中的 Gmapping 包是 Fast SLAM 算法的实现，可以在机器人中用于映射环境并在其上进行定位。在 ROS 中使用 Gmapping 是一个简单的过程，包括具有必要参数和主题（例如里程计和激光扫描）的 Gmapping 节点。
Move_base节点具有处理机器人中各种导航子系统的功能。它具有处理全局和局部计划器的功能，也可以处理机器人的地图。一旦节点收到目标位置，该目标位置就会馈入导航子系统，以到达该目标位置。
AMCL 代表自适应蒙特卡洛定位，这是一种在给定地图上定位机器人的算法。 ROS 中有一个 ROS 包，用于在我们的机器人中部署 AMCL。我们可以使用适当的输入和必要的参数启动 AMCL 节点。

第 5 章，设计 ChefBot 硬件和电路

这是为满足机器人所需规格的机器人找到合适的机器人硬件组件的过程。它还涉及电路设计和计算每个组件的电流，以确保机器人组件的稳定性。
它是控制电动机方向和速度的开关电路。
主要组件是用于计算车轮速度的车轮编码器以及用于检测机器人周围障碍物的激光测距仪或深度传感器。
我们需要检查它是否符合机器人的规格。
映射，障碍物检测，对象检测和跟踪。

第 6 章，执行器和传感器与机器人控制器的接口

开关电路，用于控制机器人中电机的速度。
可以检测车轮旋转速度和方向的传感器。
在 4X 编码方案中，我们提取编码器脉冲之间的最大过渡，以便从单个旋转中获得更多计数。
使用编码器计数和每计数距离，我们可以轻松计算出车轮的位移。
它是一种智能致动器，具有一个电机和一个微控制器，可以直接连接到 PC 并用于自定义致动器的不同设置。可以菊花链方式连接，适用于机械臂。
它是用于查找范围的传感器，具有一个发射器和一个接收器。发射器发射超声波，接收器接收超声波。这些过程之间的延迟用于距离测量。
范围 = 回音针输出的高电平时间 * 速度（340M/S）/ 2。
它正在发送 IR 脉冲并由 IR 接收器接收。根据距离，IR 接收器中的电压会发生变化，我们可以使用以下公式计算距离：

范围 = (6787 / (V - 3)) - 4

第 7 章，将视觉传感器与 ROS 接口

大多数 3D 深度传感器都有附加的视觉传感器来检测深度。它可能使用 IR 投影方法或使用立体视觉。
消息传递界面，可视化和调试机器人的工具，现成的机器人算法。
OpenCV 主要具有计算机视觉算法，OpenNI 具有用于实现 NI 应用的算法实现，而 PCL 具有用于处理点云数据的算法。
它代表同时本地化和映射。它是通常用于映射机器人环境并同时对其进行定位的算法。
它是一种在 3D 模式下映射机器人环境的算法。

第 8 章，构建 ChefBot 硬件和软件集成

它是机器人底层控制器和 PC 等高层控制器之间的中介程序。它将低级数据转换为 ROS 等效数据。
PID 是一种控制环反馈机制，可通过获取机器人位置的反馈来达到机器人目标位置。
使用编码器数据，我们可以使用机器人运动学方程来计算机器人所经过的距离。这些值是里程表数据。
它主要用于映射环境。
它主要用于在静态地图中定位机器人。

第 9 章，使用 Qt 和 Python 为机器人设计 GUI

Qt 和 GTK。
两种绑定几乎相同，只是名称不同。 PyQt 许可证是 GPL，而 PySide 随 LGPL 一起提供。另外，PySide 拥有许多有关其 API 的文档。
我们可以使用名为 pyuic 的 Py UI 编译器。
Qt 插槽是程序中可由 Qt 信号触发的函数。例如，click是可以调用名为hello()的函数的信号。
Rqt 是 ROS 中有用的 GUI 工具之一。我们可以创建 RQT 插件，并可以插入 RQT GUI 中。 RQT 中已有一些插件可以进行可视化，调试等。

Python 机器人学习手册：6~10

六、将执行器和传感器连接到机器人控制器

技术要求

直流减速电机与 Tiva C LaunchPad 的接口

差动轮式机器人

安装 Energy IDE

电机接口代码

正交编码器与 Tiva C Launchpad 的接口

处理编码器数据

正交编码器接口代码

使用 Dynamixel 执行器

使用超声波距离传感器

HC-SR04 与 Tiva C LaunchPad 的接口

HC-SR04 的工作

Tiva C Launchpad 的接口代码

Tiva C LaunchPad 与 Python 的接口

使用红外接近传感器

使用惯性测量单元

惯性导航

将 MPU 6050 与 Tiva C LaunchPad 连接

在 Energia 中设置 MPU 6050 库

电源接口代码

总结

问题

进一步阅读

七、视觉传感器与 ROS 接口

技术要求

机器人视觉传感器和图像库列表

Pixy2/CMUcam5

罗技 C920 网络摄像头

Kinect 360

英特尔实感 D400 系列

Orbbec Astra 深度传感器

OpenCV，OpenNI 和 PCL 简介

什么是 OpenCV？

从 Ubuntu 中的源代码安装 OpenCV

使用 Python-OpenCV 接口读取和显示图像

从网络摄像头捕获

什么是 OpenNI？

在 Ubuntu 中安装 OpenNI

什么是 PCL？

使用 Python，ROS，OpenCV 和 OpenNI 编程 Kinect

如何启动 OpenNI 驱动

带有 OpenCV 的 ROS 接口

创建具有 OpenCV 支持的 ROS 包

使用 Python，ROS 和cv_bridge显示 Kinect 图像

将 Orbbec Astra 与 ROS 连接

安装 Astra–ROS 驱动程序

使用 Kinect，ROS，OpenNI 和 PCL 处理点云

打开设备并生成点云

点云数据到激光扫描数据的转换

将 ROS 和 Kinect 用于 SLAM

总结

问题

进一步阅读

八、构建 ChefBot 硬件和软件集成

技术要求

构建 ChefBot 硬件

配置 ChefBot PC 并设置 ChefBot ROS 包

将 ChefBot 传感器连接到 Tiva-C LaunchPad

ChefBot 的嵌入式代码

为 ChefBot 编写 ROS Python 驱动程序

了解 ChefBot ROS 启动文件

使用 ChefBot Python 节点和启动文件

与 SLAM 一起在 ROS 上绘制房间地图

使用 ROS 定位和导航

总结

问题

进一步阅读

九、使用 Qt 和 Python 为机器人设计 GUI

技术要求

在 Ubuntu 16.04 LTS 上安装 Qt

使用 Qt 的 Python 绑定

PyQt

在 Ubuntu 16.04 LTS 中安装 PyQt

PySide

在 Ubuntu 16.04 LTS 上安装 PySide

使用 PyQt 和 PySide

Qt Designer 简介

Qt 信号和插槽

使用 Python，ROS 和`cv_bridge`显示 Kinect 图像