前情提要:
【Tutlebot迎宾机器人(一)】总体原理与设计
【Tutlebot迎宾机器人(二)】具体开发实现

2.5 各个模块的 python 封装

我进行的是状态机的编写,同时负责整个系统的集成,队友们提供给我的各个模块的代码只编写了发布者或服务器,并没有提供订阅者和客户端,并且大多代码没有进行封装。为了便于在状态机中通过 对象或函数直接订阅话题或调用服务器,我将组内所有模块的调用封装成类或函数,形成一个个 Python 功能包。

2.5.1 人脸识别模块

我的队友将人脸识别的 SDK 封装成了服务 /face_detect ,这里我在 face_detect_client1.py 中 编写了客户端类用于调用该服务,功能是检测图片中包含的人脸数、每个人脸的位置以及年龄。faceNum 是该张图片中人脸的数目,face1 中包含了该张人脸在图片中的左上角左边以及宽高和年龄值,调用服 务最终返回的是一个字符串,在后面代码的编写了会进行字符串的分割。调用人脸检测的成员函数时,先 设置一个阻塞等待服务 rospy.wait_for_service(‘/face_detect’)。整个类的声明如下(具体函数定 义请查看源码),测试效果如图所示

# face_detect_client1.py
class detectClient ():
    def __init__(self):
        self.faceNum = 0 
        self.face1 = 0 
        self.face2 = 0 
        self.face3 = 0 
    def face_detect_client(self, imgPath):

在这里插入图片描述
face_compare_client.py 因为人脸匹配的服务器只返回置信度,所以这里只编写了一个函数用于 调用该服务,形参是需要匹配的两张人脸的照片的路径,可填写工作空间的下图片的相对路径或图片的 绝对路径。

# face_compare_client.py 
def face_compare_client(srcPath, dstPath)

在这里插入图片描述

2.5.2 相机拍照模块

在 take_a_photo.py 中编写的相机拍照的类及其成员,声明如下:

class takePhoto:
    def __init__(self, guest_name)
    def image_callback(self, msg)

因为相机的话题是 /camera/rgb/raw,消息类型为 Image 这不是服务器,需要去订阅,就必须监听,无法限制收到几次,这就要选择另一种可以阻塞等待订阅消息的方法,注意订阅时需要少一个斜杠,具体写法如下:

while self.img is None:
      try:
            self.img = rospy.wait_for_message('camera/rgb/image_raw',
                                                Image,
                                                timeout=10)
      except:
            print 'failed'
            pass

因为各个模块需要被主程序文件 smach_state.py import ,所以各个模块在作为模块引入时,并不执行`if name == ‘__main__‘ 中的主函数内容,由于smach_state.py` 中已经声明了 ROS 节点, 调用的其他模块中不能再次声明 ROS 节点,在 ROS 中,一个 main 函数中只能声明一个 ROS 节点。同 时为了便于分开测试各个 Python 模块,ROS 的节点声明切记要写在 main 函数中,不能写在类的初始 化函数中!但也必须要声明节点,否则无法订阅到消息。

2.5.3 语音模块

语音识别的订阅和发布封装在 voiceRecognition_psub.py 中,类声明如下:

class voiceRecognition():
      def __init__(self):
          self.pub = rospy.Publisher('voiceWakeup', String, queue_size=1)
          self.rate = rospy.Rate(10)  # 1 Hz
          self.name = ''
          self.favoriteDrink = ''
          self.isUnderstand = False
          self.voice_msg = None
      def voice_recognition_pub(self):
      def voice_recognition_sub(self):

voice_recognition_pub 用于唤醒在线识别。如果只想发布一次消息,并确保对方可以收到,像这 里在线语音识别的唤醒,发布者的声明和消息发布之间必须延迟一段时间,这里我选择让程序休眠一秒, 否则在线语音识别那边收不到我的唤醒消息。对于这种只调用一次的,使用服务器客户端模式更加合适, 但科大讯飞提供的 SDK 只能订阅唤醒消息,所以这里就只能这样解决,在测试的过程中,延迟一秒发送消息,没有碰到唤醒失败的情况。

voice_recognition_sub 函数并不是订阅唤醒消息,而是订阅语音识别返回的识别结果,话题名称 是 voiceWords,这里我使用 find() 函数对字符串中的文字进行查找,若找到 name 或 drink 这两个关 键词,则使用 split() 函数将“is ”后的字符分割出来,注意这里的“is ”后有个空格,不能将其包含 在姓名或饮料名称中。若不包含关键词,则将 |isUnderstand| 变量赋值为 verb|False|,该参数会用于状 态机中判断是否听懂,是否需要重新询问客人。若想对语义进行一定的识别,需要建立饮料的词汇袋,在 其中进行检索。

语音合成封装的模块只包含一个函数:

def voiceSynthesis_pub(words):
      pub = rospy.Publisher("xfwords", String, queue_size=1)
      rospy.sleep(1)
      pub.publish(words)

这里发布的话题是 xfwords ,这里是只发布一次,同样需要延时一秒再发布,确保订阅者可以收到。

2.5.4 运动模块

首先在定位时若不将未知进行指定的初始化,机器人聚集粒子后定位仍然不准确,所以需要将机器 人的位置初始化在靠近其实际位置的附近。在 rviz 中可以手动放置,但存在随机误差,这里我编写封装 了 init_location.py 模块用于初始化位置,减少了人工摆放的随机误差。

import sys, rospy, tf, actionlib
from geometry_msgs.msg import *
from tf.transformations import quaternion_from_euler


def init_location(x, y):
      pub = rospy.Publisher('initialpose',
                              PoseWithCovarianceStamped,
                              queue_size=1)
      p = PoseWithCovarianceStamped()
      p.header.frame_id = "map"
      p.pose.pose.position.x = x
      p.pose.pose.position.y = y
      p.pose.pose.position.z = 0
      p.pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 0))
      p.pose.covariance = \
      [ 0.1 , 0,    0, 0, 0, 0,
            0   , 0.1 , 0, 0, 0, 0,
            0   , 0   , 0, 0, 0, 0,
            0   , 0   , 0, 0, 0, 0,
            0   , 0   , 0, 0, 0, 0,
            0   , 0   , 0, 0, 0, 0.1 ]
      for t in range(0, 5):
            rospy.sleep(1)
            pub.publish(p)

      rospy.loginfo("Init location is ( %d, %d )" %
                  (p.pose.pose.position.x, p.pose.pose.position.y))

这里需要 import 较多相关的模块,发布的话题名称为 initialpose ,由于移动机器人是在平面 2D 地 图上定位的,所以只需要给 pose.pose.position.xpose.pose.position.y 赋值。

机器人在聚集粒子时需要原地旋转,为了减少键盘控制旋转的繁琐,我编写了 rotate.py 模块用于 实现机器人原地旋转指定角度这一动作。

class rotate():
      def __init__(self, goal_angle):

在类的初始化时,指定旋转的角度(单位为 rad ),默认角速度为 1rand/s 。这里发布的话题是 /cmd_vel ,消息类型为 Twist。

2.5.5 导航模块

导航模块用到了 actionlib 功能包集合,相关介绍在这篇博客 ROS actionlib 学习,ROS 官网的相关 介绍在 actionlibDetailedDescription,第三方教程可见 actionlib-开发简单的 action 客户端。导航客户端 模块的类声明如下:

class navigate2destination():
      def __init__(self):
          self.goal_pose = MoveBaseGoal()
          self.goal_pose.target_pose.header.frame_id = 'map'
          self.navigationClient = actionlib.SimpleActionClient(
              'move_base', MoveBaseAction)
      def navigation_client(self, pose):

传入的 pose 参数格式类似如 [(1.522, 0.444, 0.0), (0.0, 0.0, -0.519, 0.85)]。

2.5.6 信息保存模块

以 a+ 方式打开文件可以在文件中追加内容,无文件时可以新建文件,函数声明如下:

def save_info(name, age, favoriteDrink, pos)

将客人的姓名、年龄、最爱的饮品以及位置以字符串写入 txt 文件,写入的内容如 18 所示。
在这里插入图片描述

2.5.7 介绍客人模块

在介绍之前,先找出最年长的客人放在最后介绍,所以需要对三位客人的年龄进行排序,我选择将 客人的年龄放在一个列表中,返回列表中最大值的索引,即客人的编号,这样就解决了排序算法在排序 后之前的索引找不到了的问题。具体函数实现如下(定义在 findOldest.py中):

def findOldest(age0, age1, age2):
      age_0 = int(age0)
      age_1 = int(age1)
      age_2 = int(age2)
      age = [age_0, age_1, age_2]
      return age.index(max(age))

需要向每位客人介绍另外两位客人的姓名和最爱的饮品,这里我将介绍 A 给 B 封装成了类来实现, 而不是将 A 和 B 介绍给 C,这样减少了代码量,提高代码的复用性,在具体实现时,初始化时打招呼,调 用两次 intro_A2B(name, drink) 函数即可,该函数会发布 xfwords 话题,调用科大讯飞语音合成 API, 注意需要在最后延时,防止介绍两人的语音混叠,这里我选择设定为休眠 8 秒。类声明如下:

class introA2B:
      def __init__(self, B_name):
          self.B_name = B_name
          words = "Hello, %s." % self.B_name
          voiceSynthesis_pub(words)
      def intro_A2B(self, name, drink):

2.6 状态机模块

该状态机作为 ROS 的一个节点,其中调用了上述我编写封装的各个模块,状态图如图 19 所示。
在这里插入图片描述

图 19: 迎宾机器人的状态图

这里我将识别人作为一个被嵌套的状态机,其中包括了如下状态:
1) 面对一位客人;
2) 询问这位客人;
3) 再次询问;
4) 重复客人的信息;
5) 保存图片信息;
6) 结束。

navigate2guests 状态中,输入关键字有客人所在房间的位置 guestPos_in ,流程如算法 1 所示。
在这里插入图片描述
face2aguest 状态是面对其中一位客人,输入关键字有估算的每个客人的位置 eachGuestPos_in 和 已询问过的客人的数量 guest_counter_in 。这里每个客人的位置是机器人在面对所有客人时拍照的图 片中的大致位置。该状态中执行的流程如算法 2 所示。
在这里插入图片描述
ask_guest 状态是询问当前面对的客人的信息,输入关键字有存储所有客人信息的 guest_info_in 和已询问过的客人的数量 ask_counter_in ,输出的关键字有询问过当前客人后,所有客人的信息 guest_info_out。 该状态中执行的流程如算法 3 所示。
在这里插入图片描述
repeat the answer 状态是用于语音合成以重复客人的信息,为了减少数据的重映射,降低复杂度,我将此状态作为一个中间状态,因为下一个保存信息的状态还需要用到客人的信息数据,所有直接在保存信息状态中将客人的信息合成语音以输出。

save_img_info 状态用于保存客人的姓名、年龄、最爱的饮品以及位置到 txt 文件,并将客人的照片 保存到指定的文件夹。输入关键字有存储所有客人信息的 sii_guest_info_in 和已询问过的客人的数量 sii_counter_in,输出的关键字有存储所有客人信息的 sii_guest_info_out 和已询问过的客人的数量 sii_counter_out,该状态中执行的流程如算法 4 所示。
在这里插入图片描述
re-ask 状态作为中间状态,用于在机器人不理解客人回答的话时重新跳转到询问状态,再次询问, 直到机器人理解客人所回答的姓名和最爱的饮品。由于是作为状态之间的中转状态,该状态没有输入、输 出关键字。

introduce_guest 状态是向每位客人介绍其他客人,该功能只在这一个状态中实现,没有再进行状 态的循环执行。输入关键字有存储所有客人信息的 introInfo_in 和各个客人的位置 introPos_in,没 有输出关键字。该状态中执行的流程如算法 5 所示。
在这里插入图片描述

guide2sofa 状态是将最年长的人引导至沙发,上面那个状态是设置为最后向最年长的人介绍其他客 人,所以跳到该状态时,直接将当前面对的客人,即最年长的人,引导至沙发。该状态的流程如算法 6 所示。

在这里插入图片描述

主函数中,参数映射关系如图 20 所示。

在这里插入图片描述

图 20: 主函数和各状态之间的参数映射关系