Rodney - 迟来的自主机器人（第二部分）
5.00/5 (16投票s)
关于 ROS（机器人操作系统）家用机器人系列中的第二部分
引言
Rodney机器人项目是一个业余机器人项目，旨在设计和构建一个自主的家庭机器人。本文是该系列文章的第二篇，描述了该项目。
背景
在第一部分中，为了帮助定义机器人的需求，我们选择了第一个任务，并将其分解为多个设计目标，使其更易于管理。
任务摘自文章让我们建造一个机器人！，内容如下：传递消息给... - 既然机器人能够识别家庭成员，那么能否让它成为“消息传递者和提醒者”呢？我可以说“机器人，提醒（人名）晚上6点来车站接我”。然后，即使该家庭成员将手机静音，或正在听大声的音乐，或（插入任何不来车站接我的理由），机器人也可以在房子里漫游，找到那个人，并向他们传递消息。
该任务的设计目标是
- 设计目标1：能够使用摄像头环顾四周，搜索人脸，尝试识别看到的人，并为任何被识别的人显示消息
- 设计目标2：面部表情和语音合成。Rodney需要能够传达消息
- 设计目标3：通过远程键盘和/或游戏手柄控制移动
- 设计目标4：增加激光测距仪或类似的测距传感器以辅助导航
- 设计目标5：自主移动
- 设计目标6：任务分配和完成通知
在第一部分中，我们使用ROS（机器人操作系统）添加了平移/倾斜功能来移动头部和摄像头。在这一部分中，我将添加人脸识别和控制节点来完成设计目标1。
任务1，设计目标1（续）
访问树莓派摄像头图像
如第一部分所述，我们可以利用ROS社区已完成的工作来简化我们的生活。我安装的树莓派Ubuntu镜像包含一个ROS包raspicam_node，我们将利用此包来访问摄像头。如果您在树莓派上使用不同的操作系统镜像，您仍然可以从GitHub站点安装该节点。
要将节点添加到我们的系统，我们只需在启动文件中包含一个提供的ROS启动文件。我将使用1280 x 960像素的图像分辨率，因此在启动文件中添加以下内容将以所需分辨率启动raspicam_node。
<include file="$(find raspicam_node)/launch/camerav2_1280x960.launch" />
ROS使用自己的图像格式在节点之间传递图像。在raspicam_node的情况下，此图像采用压缩格式。现在，当我们使用图像进行人脸检测和识别时，我们将使用OpenCV来处理图像。因此，我们需要一种方法将ROS图像转换为OpenCV图像并再转换回来。没问题，ROS社区中的好心人已经制作了一个名为cv_bridge的包，它将为我们完成这项工作。
我将使用Python编写人脸识别节点，原因有二：1）pan_tilt节点是用C++编写的，用Python编写下一个节点将为我们提供两种语言的示例；2）我已有一个想要使用的人脸识别Python库。
现在我正在使用树莓派摄像头，但您可以使用不同的摄像头。您可能会发现您的摄像头已经有可用的ROS节点，或者您可能需要将摄像头库封装在自己的ROS节点中。尽管我们的人脸识别节点订阅的主题名称是* /raspicam/image/compressed*，但如果您不想更改代码，您始终可以在启动文件中将主题名称重新映射以匹配此名称。
<remap from="my_camera_node/image/compressed" to="raspicam_node/image compressed" />
检测和识别人脸
在系统尝试识别人脸之前，我们需要用我们希望识别的对象对其进行训练。我们将编写两个非ROS Python脚本来训练我们的系统。第一个脚本* data_set_generator.py*将使用摄像头捕获每个对象的人脸图像。第二个脚本* training.py*将使用第一个脚本收集的图像进行实际训练。第二个脚本的输出是一个包含训练数据的yaml文件，ROS节点将在其初始化期间加载此训练数据。如果您希望将新人员添加到对象列表中，则需要重新运行每个脚本。
我们节点的ROS包名为face_recognition，位于* face_recognition*文件夹中。* scripts*子文件夹包含我们的两个训练脚本。
每个脚本都利用了OpenCV内置的人脸检测和人脸识别功能。如果您希望完全理解其工作原理，我建议您阅读互联网上提供的许多文章。其中一篇文章可以在此处找到。我遇到的所有文章都没有我想要的确切代码，所以我从许多文章中借用了代码。在这里，我将对每个脚本进行高级描述，从* data_set_generator.py*开始。
在必要的导入之后，我们使用OpenCV库加载分类器，声明一个辅助函数，用于确保我们需要的文件夹存在。* dataset *文件夹将保存所有捕获的图像，* trainer *文件夹将保存包含训练数据的yaml文件以及包含我们的对象的姓名和ID的文件。
import cv2
import os
import io
import numpy
import yaml
import picamera
# Detect object in video stream using Haarcascade Frontal Face
face_detector = cv2.CascadeClassifier('../classifiers/haarcascade_frontalface_default.xml')
def assure_path_exists(path):
dir = os.path.dirname(path)
if not os.path.exists(dir):
os.makedirs(dir)
assure_path_exists("dataset/")
assure_path_exists("../trainer/")
接下来，我们设置摄像头分辨率，设置一些变量，包括保存我们对象列表的文件名，并打开该文件。
然后我们创建一个窗口来显示从摄像头读取的图像。这将使对象能够将自己定位在摄像头的视野内。
然后脚本会提示用户输入主题的唯一ID、主题姓名以及是否为低光照条件。唯一ID应从1开始，每次添加新主题时递增1，姓名是您希望机器人用于此主题的名称。理想情况下，您应该为每个主题运行此脚本两次，一次在明亮光线下，第二次在低光照条件下。这将提高识别算法成功的机会。每次运行脚本将拍摄100张主题图像，每张图像的文件名由主题ID和图像编号构成。在明亮光线下拍摄的图像编号为0到99，在低光照下拍摄的图像编号为100到199。
下一步是，如果对象尚不存在，则将其添加到名称文件中。
with picamera.PiCamera() as camera:
camera.resolution = (1280, 960)
looping = True
count = 0
end = 99
names_dict = {}
name_file = '../trainer/names.yml'
# Open the file of IDs and names to append the new one to
if os.path.exists(name_file):
with open(name_file, 'r') as stream:
names_dict = yaml.load(stream)
cv2.namedWindow('frame', cv2.WINDOW_NORMAL)
face_id = raw_input("What is this persons ID number? ")
name = raw_input("What is this persons name? ")
low_light = raw_input("Low light Y/N?" )
if low_light == 'Y' or low_light == 'y':
count = 100
end = 199
# If not already in the dictionary add details
if not face_id in names_dict:
names_dict[int(face_id)]=name
with open(name_file, 'w') as outfile:
yaml.dump(names_dict, outfile, default_flow_style=False)
然后进入一个循环来捕获图像。循环的每次迭代都会从摄像头捕获一张图像，并将其转换为numpy数组以供OpenCV调用。
使用OpenCV，我们将图像转换为灰度图像并尝试在图像中检测人脸。如果检测到人脸，则裁剪图像中人脸周围的区域，图像样本数量增加，并将裁剪后的灰度图像存储在* dataset*文件夹中。从摄像头获取的原始图像以及叠加在人脸周围的帧会显示给用户。
然后检查用户是否按下了键盘上的“q”键，该键用于提前退出程序。否则，检查是否已捕获一百张人脸图像，如果是，则退出此主循环。
while(looping):
# Create a memory stream so image doesn't need to be saved to a file
stream = io.BytesIO()
camera.capture(stream, format='jpeg')
#Convert picture to numpy array
buff = numpy.fromstring(stream.getvalue(), dtype=numpy.uint8)
# Now create an OpenCV image
image_frame = cv2.imdecode(buff, 1)
# Convert frame to grayscale
gray = cv2.cvtColor(image_frame, cv2.COLOR_BGR2GRAY)
# Detect frames of different sizes, list of faces rectangles
faces = face_detector.detectMultiScale(gray, 1.3, 5)
# Although faces could contain more than one face we only expect one
# person to be in the data set image otherwise it would confuse
# the whole thing
if (len(faces) != 0):
# Expecting one face only on the data set image
(x, y, w, h) = faces[0]
# Crop the image frame into rectangle
cv2.rectangle(image_frame, (x,y), (x+w,y+h), (255,0,0), 4)
# Increment sample face image
count += 1
# Save the captured image into the datasets folder
cv2.imwrite("dataset/User." + str(face_id) + '.' + str(count) + ".jpg", gray[y:y+h,x:x+w])
# Display the video frame, with bounded rectangle on the person's face
cv2.imshow('frame', image_frame)
# To stop taking video, press 'q' for at least 100ms
if cv2.waitKey(100) & 0xFF == ord('q'):
looping = False
# If image taken reach 100, stop taking video
elif count>end:
looping = False
最后两行关闭显示图像的窗口，并打印一条消息，指示过程已完成。
# Close all started windows
cv2.destroyAllWindows()
print("Data prepared")
为每个主题运行脚本后，或者为新主题重新运行脚本后，您将运行* training.py*脚本。
* training.py*脚本首先导入并定义assure_path_exists函数，然后使用相同的分类器文件创建OpenCV类LBPHFaceRecognizer_create和CascadeClassifier的实例。
import cv2
import os
import numpy as np
def assure_path_exists(path):
dir = os.path.dirname(path)
if not os.path.exists(dir):
os.makedirs(dir)
# Create Local Binary Patterns Histograms for face recognization
recognizer = cv2.face.LBPHFaceRecognizer_create()
# Using prebuilt frontal face training model, for face detection
detector = cv2.CascadeClassifier("../classifiers/haarcascade_frontalface_default.xml");
get_images_and_labels函数读取每个存储的图像，检测人脸并从文件名中获取ID。
# Create method to get the images and label data
def get_images_and_labels(path):
# Get all file path
image_paths = [os.path.join(path,f) for f in os.listdir(path)]
# Initialize empty face sample
face_samples=[]
# Initialize empty id
ids = []
# Loop all the file path
for image_path in image_paths:
# The stored image is grayscale so read in gray scale
gray = cv2.imread(image_path, cv2.IMREAD_GRAYSCALE)
# Get the image id
id = int(os.path.split(image_path)[-1].split(".")[1])
# Get the face from the training images
# Don't need any scaling as these images already full face
faces = detector.detectMultiScale(gray);
# During testing not always detected face on image, which
# is odd as it should be just an image that was saved
if (len(faces) == 0):
print "No face on " + image_path
else:
# We know each image is only of one face
(x, y, w, h) = faces[0]
# Add the image to face samples
face_samples.append(gray[y:y+h,x:x+w])
# Add the ID to IDs
ids.append(id)
# Pass the face array and IDs array
return face_samples,ids
获取所有面部和ID后，它们将被传递给OpenCV人脸识别器，识别器的数据将保存到磁盘。我们的节点将使用的人脸识别库稍后将加载此数据来训练识别器。
# Get the faces and IDs
faces,ids = get_images_and_labels('dataset')
# Train the model using the faces and IDs
recognizer.train(faces, np.array(ids))
# Save the model into trainer.yml
assure_path_exists('../trainer/')
recognizer.save('../trainer/trainer.yml')
print("Done")
ROS节点本身的代碼位於* src *子文件夾中的* face_recognition_node.py*文件中。代碼使用了一個庫文件，* face_recognition_lib.py*，其中包含FaceRecognition類。此文件位於* src/face_recognition_lib*子文件夾中。
在描述节点的代码之前，我将讨论FaceRecognition类。在必要的导入和类声明之后，它定义了许多函数。
类的构造函数创建OpenCV人脸识别器，然后读取训练脚本创建的训练文件。然后它打开包含姓名和ID列表的文件，并创建分类器。最后，它存储传递给它的置信度值。此值将用于确定是否接受建议的人脸ID。
def __init__(self, path, confidence):
# Create Local Binary Patterns Histograms for face recognization
self.__face_recognizer = cv2.face.LBPHFaceRecognizer_create()
# Load the trained mode
self.__face_recognizer.read(path + '/trainer/trainer.yml')
# Load the names file
with open(path + '/trainer/names.yml', 'r') as stream:
self.__names_dict = yaml.load(stream)
# Detect object in image using Haarcascade Frontal Face
self.__face_detector = cv2.CascadeClassifier
(path + '/classifiers/haarcascade_frontalface_default.xml')
# Confidence level,
# the confidence of the system in recognising a face must be greater than
# this level to be accepted by the system as a recognised face.
self.__confidence_level = confidence
声明了两个函数，如果检测到人脸，将用于修改捕获的图像。第一个函数将在图像上绘制一个矩形，第二个函数将在图像上绘制提供的文本。
# Function to draw rectangle on image according to given (x, y) coordinates
# and the given width and height
def draw_rectangle(self, img, rect, bgr):
(x, y, w, h) = rect
cv2.rectangle(img, (x, y), (x+w, y+h), bgr, 4)
# Function to draw text on give image starting at the passed (x, y) coordinates.
def draw_text(self, img, text, x, y, bgr):
cv2.putText(img, text, (x, y), cv2.FONT_HERSHEY_PLAIN, 3.0, bgr, 4)
下一个函数detect_faces用于检测所提供图像中的任何人脸。它将图像转换为灰度，以便OpenCV可以用于检测任何人脸。如果检测到人脸，则返回每个面部的数据以及人脸在图像中的位置。请注意，这部分代码是为了允许图像中出现多个人脸而编写的。
# This function detects any faces using OpenCV from the supplied image
def detect_faces(self, img):
face_data = []
#convert the test image to gray image as opencv face detector expects gray images
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
#let's detect multiscale (some images may be closer to camera than others) images
#result is a list of faces
faces_detected = self.__face_detector.detectMultiScale(gray, 1.3, 5);
#if no faces are detected then return None
if (len(faces_detected) == 0):
return None, None
#return only the face part of the image
for face in faces_detected:
(x, y, w, h) = face
face_data.append(gray[y:y+w, x:x+h])
# faces_detected is a list of rectangles where faces have been detected.
# face_data is a list of the data for the faces detected
return face_data, faces_detected
类中的最后一个函数scan_for_faces将由节点调用，以对提供的图像进行人脸检测和识别。此函数调用detect_faces函数，如果检测到任何人脸，它将遍历每个人脸，调用OpenCV人脸预测器，该预测器返回识别出的人脸ID和指示预测置信度的值。此值将转换为置信度百分比。从提供的ID中获取姓名。在人脸周围绘制一个矩形，以及对象的姓名和置信度。如果预测的置信度超过存储的阈值，则绘制在图像上的框和文本将是绿色，否则为红色。此外，如果超过置信度阈值，将输入ID和姓名的字典条目。一旦检测到的所有面部都已分析，此字典将返回给调用者。
# This class function will be called from outside to scan the supplied img.
# First it attempts to detect faces in the image and then if any are found
# it attempts for recognise them against know subjects. It will adjust the
# supplied image.
def scan_for_faces(self, img):
# First do the face detection, returned faces is a list of the faces detected
# and rects is a list of rectangles of where the faces are in the image
faces, rects = self.detect_faces(img)
# Create a dictionary of IDs and Names of those detected in the image
detected_dict = {}
# If we detected faces then process each one
if(faces != None):
for index in range(len(faces)):
# Predict the image using our face recognizer
label, confidence = self.__face_recognizer.predict(faces[index])
our_confidence = round(100 - confidence, 2)
# Get name of respective label returned by face recognizer
name_text = self.__names_dict[label]
name_text_confidence = name_text + " {0:.2f}%".format(our_confidence)
if(our_confidence > self.__confidence_level):
colour = (0, 255, 0)
else:
colour = (0, 0, 255)
#draw a rectangle around face(s) detected
self.draw_rectangle(img, rects[index], colour)
#draw name of predicted person(s) and the confidence value
self.draw_text(img, name_text_confidence, rects[index,0], rects[index,1]-5, colour)
if(our_confidence > self.__confidence_level):
# Add details to the dictionary to be returned
detected_dict[label]=name_text
return detected_dict
接下来，我将描述ROS节点本身。
main例程初始化节点的ROS并创建FaceRecognitionNode的实例，然后调用spin来处理订阅和发布的主题。
def main(args):
rospy.init_node('face_recognition_node', anonymous=False)
frn = FaceRecognitionNode()
rospy.loginfo("Face recognition node started")
try:
rospy.spin()
except KeyboardInterrupt:
print("Shutting down")
if __name__ == '__main__':
main(sys.argv)
文件的其余部分包含FaceRecognitionNode类。
FaceRecognitionNode的构造函数创建CVBridge实例，如前所述，这将用于将ROS图像转换为OpenCV图像。然后它注册将发布* face_recognition_node/image/compressed*主题，该主题将用于发送带有识别出的所有人脸周围框的图像，并在图像上打印对象名称和置信度。此主题用于测试节点。
该节点订阅主题* raspicam_node/image/compressed*，该主题将包含来自摄像头的最新图像，函数callback注册为在新图像接收时调用。
置信度阈值从参数服务器读取，如果参数服务器中没有该值，则设置为默认值20%。
构造函数然后创建上面描述的FaceRecognition类的一个实例。
最后，我们创建并启动一个动作服务器。当节点需要执行一个可能需要一些时间的非阻塞任务时，会使用动作。发送一个请求来执行任务，任务完成后会收到一个回复。在任务期间也可以收到反馈。任务请求也可以取消。有关ROS动作的更多信息可以在这里找到。包含我们用户定义动作消息的文件将在本节后面描述。
class FaceRecognitionNode:
def __init__(self):
self.__bridge = CvBridge()
self.__image_pub = rospy.Publisher('face_recognition_node/image/compressed',
CompressedImage, queue_size=1)
self.__image_sub = rospy.Subscriber('raspicam_node/image/compressed',
CompressedImage, self.callback)
confidence_level = rospy.get_param('/face_rec_python/confidence_level', 20)
rospy.loginfo("FaceRecognitionNode: Confidence level %s", str(confidence_level))
# Create the face_recognition_lib class instance
self.__frc = face_recognition_lib.FaceRecognition
(roslib.packages.get_pkg_dir('face_recognition', required=True), confidence_level)
# Create the Action server
self.__as = actionlib.SimpleActionServer
('face_recognition', scan_for_facesAction, self.do_action, False)
self.__as.start()
当收到执行动作的请求时，动作服务器会调用函数do_action。
此函数将最后接收到的图像从ROS图像转换为OpenCV图像。调用scan_for_faces以检查图像中已知的人脸。调整后的图像（可能包含叠加的框和名称）被转换回ROS图像并发布到* face_recognition_node/image/compressed*主题。
从scan_for_faces调用返回的姓名和ID然后用于创建动作的结果。如果没有识别人脸，则结果中的数据将为空。
def do_action(self, goal):
# Scan the current image for faces recognised
# The image may show more than one face.
# The returned dictionary will contain the unique IDs
# and Names of any subjects recognised.
# If no detection/recognition dictionary will be empty
image = self.__bridge.compressed_imgmsg_to_cv2(self.__current_image)
# In the next call image will be altered if faces are recognised
detected_dict = self.__frc.scan_for_faces(image)
try:
self.__image_pub.publish(self.__bridge.cv2_to_compressed_imgmsg(image))
except CvBridgeError as e:
print(e)
# Now post a message with the list of IDs and names
ids = []
names = []
for k, v in detected_dict.items():
ids.append(k)
names.append(v)
# Set result for the action
result = scan_for_facesResult()
result.ids_detected = ids
result.names_detected = names
self.__as.set_succeeded(result)
每当在raspicam/image/compressed主题上收到消息时，都会调用callback函数。
此函数只是存储当前图像，以备人脸识别之需。
# Callback for new image received
def callback(self, data):
# Each time we receive an image we store it ready in case then asked to scan it
self.__current_image = data
节点包还包含一个* config.yaml*文件，可用于设置置信度级别而无需重新编译代码。该包还包含一个启动文件* test.launch*，可用于测试节点。除了启动此节点外，它还将启动摄像头节点。
人脸识别动作
如上所述，face_recognition包使用用户定义的动作消息来启动操作并返回查找已知人脸的结果。包face_recognition_msgs包含我们的第一个ROS动作scan_for_faces.action。
包的文件位于* face_recognition_msgs*文件夹中，动作的文件存储在* action*子文件夹中。
动作规范包含目标、结果和反馈部分。它看起来类似于消息定义文件，只是每个部分都由三个破折号（---）分隔。
# This action scans the next camera image for faces that are recognised
# There are no parameters to start the action
---
# Results of the scan
uint16[] ids_detected
string[] names_detected
---
# No parameters for feedback
第一个三个破折号上方是目标。在我们的例子中，我们没有任何目标参数，仅仅收到目标就会启动我们的动作。
结果参数位于前三个破折号下方，在我们的案例中，包含任何识别出的人脸的ID数组和名称数组。
第二个三个破折号下方是反馈参数。在我们的案例中，我们不会返回任何反馈。
头部控制
现在我们有了一个节点来对来自摄像头的图像执行人脸识别操作，并且从本文的第一部分中，我们有了平移/倾斜功能来移动连接到头部的伺服电机。现在我们将添加一个节点，该节点接受头部应该处于的位置，但以步进方式将头部移动到该目标，这样当头部从一个极端移动到另一个极端时，机器人就不会晃动。该节点还将接受不仅要移动到的绝对位置，还要接受相对于当前位置移动的相对距离。
我们节点的ROS包名为head_control，位于* head_control*文件夹中。该包包含所有常见的ROS文件和文件夹。
* action*文件夹包含* point_head.action*文件。此动作将传递一个包含要移动到的位置的目标，以及头部在前往目标位置途中的反馈。
# This action moves the head to a given position
# The goal definition is the position to move the head to if absolute is true,
# otherwise the pan/tilt values are how much relative to move the head.
# Values are radians.
bool absolute
float64 pan
float64 tilt
---
# There are no parameters for the result
---
# Feedback gives the current position
float64 current_pan
float64 current_tilt
目标包含以弧度表示的平移和倾斜值。它还包含一个布尔标志absolute。如果该标志为true，则pan和tilt值是绝对目标位置。如果该标志为false，则这些值表示头部应相对于当前位置移动的量。
反馈值表示头部在前往目标位置途中的当前位置。
* config*文件夹包含一个* config.yaml*文件，可用于覆盖一些默认配置值。您可以配置
- 头部的默认位置
- 平移和倾斜设备每次请求的最大移动量。这是为了防止伺服电机一次跳动到请求的角度，导致头部在到达目标位置时颤抖。
head:
position:
pan: 0.0
tilt: 0.0
max_step:
pan: 0.0872665
tilt: 0.0872665
* include/head_control*和* src*文件夹包含该包的C++代码。对于该包，我们有一个C++类HeadControlNode和一个包含在* head_control_node.cpp*文件中的main例程。
main例程通知ROS我们的节点，创建节点的类实例并将其传递给节点句柄和节点名称。在该项目中，我们第一次不将节点的完全控制权交给ROS。到目前为止，在所有其他节点中，我们都通过在C++中调用ros::spin或在Python代码中调用rospy.spin将控制权交给ROS。这些调用将控制权交给ROS，我们的代码只有在ROS收到消息时调用我们的回调函数时才能运行。在这个节点中，我想保留控制权，因为我们希望以小增量步进方式将伺服电机移动到目标位置。如果我们允许伺服电机一次移动大角度，当达到目标位置时，头部会颤抖着停止。我们保留控制权的方式是调用ros::spinOnce。在这个函数中，ROS将发布我们请求的任何主题并通过调用我们的回调来处理任何传入的主题，但随后会将控制权返回给main。
在进入while循环之前，我们创建一个ros::Rate实例并向其传递我们希望保持的计时，在我们的例子中，我将速率设置为10Hz。在循环内部，我们调用r.sleep，此Rate实例将通过计算循环中完成工作所花费的时间来尝试将循环保持在10Hz。
只要对ros::ok的调用返回true，我们的循环就会继续，当节点完成关闭时，例如，当您在键盘上按下Ctrl-c时，它将返回false。
在循环中，我们将调用moveServo，本文稍后将对此进行描述。
int main(int argc, char **argv),
{
ros::init(argc, argv, "head_control_node");
ros::NodeHandle n;
std::string node_name = ros::this_node::getName();
HeadControlNode head_control(n, node_name);
ROS_INFO("%s started", node_name.c_str());
// We need control of the node so that we can step the servos to the target
// position in small steps to stop the head shuddering if it was to move in one big step
ros::Rate r(10); // 10Hz
while(ros::ok())
{
// See if the servos need moving
head_control.moveServo();
ros::spinOnce();
r.sleep();
}
return 0;
}
文件的其余部分包含HeadControlNode类。
HeadControlNode的构造函数向动作服务器注册我们的回调函数pointHeadCallback。当动作服务器接收到动作目标时，将调用此回调，这实际上启动了该动作。
然后，我们通过调用as_.start启动服务器运行。
构造函数随后声明它将发布主题* pan_tilt_node/joints*，该主题将用于将所需的平移/倾斜位置传递给平移/倾斜节点。
然后我们设置一些配置默认值，并从参数服务器读取任何可用的覆盖值。
接下来，我们设置关节状态消息中关节的名称，因为它们不会改变。
最后，我们发布一条消息，将头部移动到已知起点。我们无法以小步移动到此位置，因为我们不知道上电后的起始位置。
// Constructor
HeadControlNode::HeadControlNode(ros::NodeHandle n, std::string name) : as_(n, name, false)
{
nh_ = n;
as_.registerGoalCallback(boost::bind(&HeadControlNode::pointHeadCallback, this));
as_.start();
// Topic to move head
move_head_pub_ = nh_.advertise<sensor_msgs::JointState>("pan_tilt_node/joints", 10, true);
// Obtain any configuration values from the parameter server.
// If they don't exist use the defaults
// Joint names
nh_.param<std::string>("/servo/index0/pan/joint_name", pan_joint_name_, "reserved_pan0");
nh_.param<std::string>("/servo/index0/tilt/joint_name", tilt_joint_name_, "reserved_tilt0");
// Maximum angle we can move in one go
nh_.param("/head/max_step/pan", pan_step_, 0.174533);
nh_.param("/head/max_step/tilt", tilt_step_, 0.174533);
double pan; // Pan default position to return to
double tilt; // Tilt default position to return to
nh_.param("/head/position/pan", pan, 0.0);
nh_.param("/head/position/tilt", tilt, 0.0);
default_position_.pan = pan;
default_position_.tilt = tilt;
// Set up the message we will publish
msg_.name.push_back(pan_joint_name_);
msg_.name.push_back(tilt_joint_name_);
msg_.position.push_back(0.0);
msg_.position.push_back(0.0);
// We will often return to this position when a task is completed
current_pan_tilt_ = default_position_;
// We don't know where the servo starts from so just jump to the required position
// Publish a start position to get the head in a known position.
publishJointState(current_pan_tilt_);
move_head_ = false;
movement_complete_ = false;
target_pan_tilt_ = current_pan_tilt_;
}
现在我将简要描述构成该类的函数。
当动作服务器接收到目标消息时，ROS会调用pointHeadCallback。目标数据是绝对目标位置还是相对目标位置取决于绝对标志的状态。
该函数调用动作服务器以通知它目标已被接受，它存储新的目标位置并将move_head标志设置为true，表示需要移动头部。
// This callback is for the point head action
void HeadControlNode::pointHeadCallback()
{
head_control::point_headGoal::ConstPtr goal;
goal = as_.acceptNewGoal();
// Set the target position to the request position
if (goal->absolute == true)
{
target_pan_tilt_.pan = goal->pan;
target_pan_tilt_.tilt = goal->tilt;
}
else
{
target_pan_tilt_.pan += goal->pan;
target_pan_tilt_.tilt += goal->tilt;
}
// Indicate that the servos should be moved
move_head_ = true;
movement_complete_ = false;
}
函数moveServo是main在我们的循环中调用的函数。它检查是否发出了移动头部的请求，如果是，则进入一个“if”、“else if”、“else”结构。
此结构的“if”部分检查动作是否已被抢占。如果是，则接受抢占并进行清理。
“else if”部分检查头部运动是否完成，如果完成，则计数器递减。此计数器用于包含头部停止运动和模糊任何摄像头图像所需的时间，即伺服电机达到目标位置后。当计数器达到零时，动作完成的事实将报告给动作服务器。结果中没有参数。
“else”部分负责计算伺服器朝目标位置的下一步移动，它还使用辅助函数publishJointState发布带有下一个所需伺服器位置的关节状态消息，并将反馈报告给动作服务器。此反馈将包含伺服器刚刚被要求移动到的位置。
// Function to move the servos if required by a step amount.
// This is to stop the head shuddering if the servo
// is moved to the target position in one movement.
void HeadControlNode::moveServo()
{
if(move_head_ == true)
{
if(as_.isPreemptRequested() || !ros::ok())
{
as_.setPreempted();
movement_complete_ = false;
move_head_ = false;
}
else if(movement_complete_ == true)
{
// We have reached the target but give time to settle
loop_count_down_--;
if(loop_count_down_ <= 0)
{
movement_complete_ = false;
move_head_ = false;
head_control::point_headResult result;
as_.setSucceeded(result);
}
}
else
{
if((target_pan_tilt_.pan == current_pan_tilt_.pan) &&
(target_pan_tilt_.tilt == current_pan_tilt_.tilt))
{
// Last time around we must have requested the final move
movement_complete_ = true;
loop_count_down_ = 8;
}
else
{
// Still moving, calculate pan movement
if(std::abs(target_pan_tilt_.pan - current_pan_tilt_.pan) > pan_step_)
{
// Distance to target to great to move in one go
if(target_pan_tilt_.pan > current_pan_tilt_.pan)
{
// Add the step to current
current_pan_tilt_.pan += pan_step_;
}
else
{
// Subtract step from current
current_pan_tilt_.pan -= pan_step_;
}
}
else
{
// Can move to the target position in one go
// (or pan is in fact already there but tilt is not)
current_pan_tilt_.pan = target_pan_tilt_.pan;
}
// Calculate tilt movement
if(std::abs(target_pan_tilt_.tilt - current_pan_tilt_.tilt) > tilt_step_)
{
// Distance to target to great to move in one go
if(target_pan_tilt_.tilt > current_pan_tilt_.tilt)
{
// Add the step to current
current_pan_tilt_.tilt += tilt_step_;
}
else
{
// Subtract step from current
current_pan_tilt_.tilt -= tilt_step_;
}
}
else
{
// Can move to the target position in one go
// (or tilt is in fact already there but pan is not)
current_pan_tilt_.tilt = target_pan_tilt_.tilt;
}
// Publish the movement
publishJointState(current_pan_tilt_);
// Publish feedback
head_control::point_headFeedback feedback;
feedback.current_pan = current_pan_tilt_.pan;
feedback.current_tilt = current_pan_tilt_.tilt;
as_.publishFeedback(feedback);
}
}
}
}
类中的最后一个函数是辅助函数publishJointState。此函数更新关节状态消息中的位置值，然后发布该消息。
// This function creates and publishes a joint state message
void HeadControlNode::publishJointState(struct position pan_tilt)
{
msg_.position[0] = pan_tilt.pan;
msg_.position[1] = pan_tilt.tilt;
msg_.header.stamp = ros::Time::now();
move_head_pub_.publish(msg_);
}
该包还在* launch*文件夹中包含一个启动文件。此文件* test.launch*将启动所有开发的用于移动头部的节点。
<?xml version="1.0" ?>
<launch>
<rosparam command="load" file="$(find pan_tilt)/config/config.yaml" />
<rosparam command="load" file="$(find head_control)/config/config.yaml" />
<node pkg="pan_tilt" type="pan_tilt_node" name="pan_tilt_node" output="screen" />
<node pkg="rosserial_python" type="serial_node.py"
name="serial_node" output="screen" args="/dev/ttyUSB0"/>
<node pkg="head_control" type="head_control_node" name="head_control_node" output="screen"/>
</launch>
动作客户端
在我们的两个节点中都包含了动作服务器之后，您会期望有动作客户端来连接它们。这绝对是与服务器通信的一种方法。在本文的稍后部分，我将介绍一个ROS包，它将允许我们创建状态机和子状态机来控制我们的机器人任务。使用这个包，可以将一个单独的状态分配为动作客户端，所有通信都由我们幕后完成。
为了测试我们目前开发的系统并展示如何编写动作客户端，我们将在每个节点中编写两个测试节点，其中都包含一个动作客户端。
我们的第一个节点是一个非常简单的Python节点，将用于测试人脸识别节点。此节点的ROS包名为rodney_recognition_test，位于* rodney_recognition_test*文件夹中。该包包含所有常见的ROS文件和文件夹。
所有代码都包含在* src*文件夹中的* rodney_recognition_test_node.py*文件中。
代码初始化我们的节点并创建一个动作客户端。请注意，传递给SimpleActionClient的名称，在我们的例子中是“face_recognition”，必须与提供给动作服务器的名称匹配。
然后我们调用wait_for_server，代码会在这里等待，直到能够与服务器建立联系。然后我们创建一个目标，在这种情况下不包含任何数据，并将目标发送到服务器。
在我们的简单示例中，我们然后等待结果返回，并且节点通过打印识别出并在结果中返回的任何人脸的ID和姓名来结束。
#!/usr/bin/env python
import rospy
import actionlib
from face_recognition_msgs.msg import scan_for_facesAction,
scan_for_facesGoal, scan_for_facesResult
rospy.init_node('face_recognition_client')
client = actionlib.SimpleActionClient('face_recognition', scan_for_facesAction)
client.wait_for_server()
goal = scan_for_facesGoal()
client.send_goal(goal)
client.wait_for_result()
result = client.get_result()
print(result.ids_detected)
print(result.names_detected)
在我们的下一个包中，为了测试head_control节点，我们将编写一个稍微复杂一点的节点，这次是用C++编写的。
我们的ROS包名为rodney_head_test，位于* rodney_head_test*文件夹中。该包包含所有常见的ROS文件和文件夹。
* include/rodney_head_test*和* src*文件夹包含该包的C++代码。对于该包，我们有一个C++类RodneyHeadTestNode和一个包含在* rodney_head_test_node.cpp*文件中的main例程。
main例程通知ROS我们的节点，创建节点的类实例并将其传递给节点句柄，记录节点已启动，并通过调用ros::spin将控制权交给ROS。
int main(int argc, char **argv)
{
ros::init(argc, argv, "rodney_head_test");
ros::NodeHandle n;
RodneyHeadTestNode rodney_head_test_node(n);
std::string node_name = ros::this_node::getName();
ROS_INFO("%s started", node_name.c_str());
ros::spin();
return 0;
}
构造函数创建我们的动作客户端实例ac_，并将其传递给动作服务器的名称，在我们的例子中是* head_control_node*。这必须与我们在HeadControlNode构造函数中创建动作服务器时给它的名称匹配。
然后我们读取配置参数以限制伺服电机的移动。
我们将使用一个键盘节点，可从https://github.com/lrse/ros-keyboard获取，与系统进行交互。在构造函数中，我们订阅主题* keyboard/keydown*，并在该主题收到消息时调用函数keyboardCallBack。
调用ac_.waitForServer将在构造函数中等待，直到我们的动作服务器正在运行。
// Constructor
RodneyHeadTestNode::RodneyHeadTestNode(ros::NodeHandle n) : ac_("head_control_node", true)
{
nh_ = n;
// Subscribe to receive keyboard input
key_sub_ = nh_.subscribe("keyboard/keydown", 100,
&RodneyHeadTestNode::keyboardCallBack, this);
nh_.param("/servo/index0/pan/max", max_pan_radians_, M_PI/2.0);
nh_.param("/servo/index0/pan/min", min_pan_radians_, -(M_PI/2.0));
nh_.param("/servo/index0/tilt/max", max_tilt_radians_, M_PI/2.0);
nh_.param("/servo/index0/tilt/min", min_tilt_radians_, -(M_PI/2.0));
ROS_INFO("RodneyHeadTestNode: Waiting for action server to start");
// wait for the action server to start
ac_.waitForServer(); //will wait for infinite time
moving_ = false;
ROS_INFO("RodneyHeadTestNode: Action server started");
}
函数keyboardCallBack检查收到的消息并根据按下的键运行测试。
它创建一个动作目标实例，设置目标参数，并通过调用ac_.sendGoal将其传递给动作服务器。通过调用，我们传递三个回调函数
doneCB在动作完成时调用activeCB在动作激活时调用，以及feedbackCB在收到动作进度反馈时调用
动作可以被抢占，所以如果按下“c”键并且头部正在移动，我们将通过调用ac_.cancelGoal取消动作。
void RodneyHeadTestNode::keyboardCallBack(const keyboard::Key::ConstPtr& msg)
{
head_control::point_headGoal goal;
// Check for key 1 with no modifiers apart from num lock is allowed
if((msg->code == keyboard::Key::KEY_1) &&
((msg->modifiers & ~keyboard::Key::MODIFIER_NUM) == 0))
{
// Key 1, Test 1 move to max pan and tilt
goal.absolute = true;
goal.pan = max_pan_radians_;
goal.tilt = max_tilt_radians_;
// Need boost::bind to pass in the 'this' pointer
ac_.sendGoal(goal,
boost::bind(&RodneyHeadTestNode::doneCB, this, _1, _2),
boost::bind(&RodneyHeadTestNode::activeCB, this),
boost::bind(&RodneyHeadTestNode::feedbackCB, this, _1));
}
if((msg->code == keyboard::Key::KEY_2) &&
((msg->modifiers & ~keyboard::Key::MODIFIER_NUM) == 0))
{
// Key 2, test 2 move to min pan and tilt
goal.absolute = true;
goal.pan = min_pan_radians_;
goal.tilt = min_tilt_radians_;
// Need boost::bind to pass in the 'this' pointer
ac_.sendGoal(goal,
boost::bind(&RodneyHeadTestNode::doneCB, this, _1, _2),
boost::bind(&RodneyHeadTestNode::activeCB, this),
boost::bind(&RodneyHeadTestNode::feedbackCB, this, _1));
}
if((msg->code == keyboard::Key::KEY_3) &&
((msg->modifiers & ~keyboard::Key::MODIFIER_NUM) == 0))
{
// Key 3, test 3 move to pan 0, tilt 0
goal.absolute = true;
goal.pan = 0.0;
goal.tilt = 0.0;
// Need boost::bind to pass in the 'this' pointer
ac_.sendGoal(goal,
boost::bind(&RodneyHeadTestNode::doneCB, this, _1, _2),
boost::bind(&RodneyHeadTestNode::activeCB, this),
boost::bind(&RodneyHeadTestNode::feedbackCB, this, _1));
}
if((msg->code == keyboard::Key::KEY_4) &&
((msg->modifiers & ~keyboard::Key::MODIFIER_NUM) == 0))
{
// Key 4, test 4 move to pan 0, tilt -45 degress
goal.absolute = true;
goal.pan = 0.0;
goal.tilt = -0.785398;
// Need boost::bind to pass in the 'this' pointer
ac_.sendGoal(goal,
boost::bind(&RodneyHeadTestNode::doneCB, this, _1, _2),
boost::bind(&RodneyHeadTestNode::activeCB, this),
boost::bind(&RodneyHeadTestNode::feedbackCB, this, _1));
}
if((msg->code == keyboard::Key::KEY_5) &&
((msg->modifiers & ~keyboard::Key::MODIFIER_NUM) == 0))
{
// Key 5, test 5 move tilt up by 10 degrees
goal.absolute = false;
goal.pan = 0;
goal.tilt = -0.174533;
// Need boost::bind to pass in the 'this' pointer
ac_.sendGoal(goal,
boost::bind(&RodneyHeadTestNode::doneCB, this, _1, _2),
boost::bind(&RodneyHeadTestNode::activeCB, this),
boost::bind(&RodneyHeadTestNode::feedbackCB, this, _1));
}
if((msg->code == keyboard::Key::KEY_6) &&
((msg->modifiers & ~keyboard::Key::MODIFIER_NUM) == 0))
{
// Key 6, test 6 move pan by 20 anti-clockwise
goal.absolute = false;
goal.pan = 0.349066;
goal.tilt = 0;
// Need boost::bind to pass in the 'this' pointer
ac_.sendGoal(goal,
boost::bind(&RodneyHeadTestNode::doneCB, this, _1, _2),
boost::bind(&RodneyHeadTestNode::activeCB, this),
boost::bind(&RodneyHeadTestNode::feedbackCB, this, _1));
}
if((msg->code == keyboard::Key::KEY_7) &&
((msg->modifiers & ~keyboard::Key::MODIFIER_NUM) == 0))
{
// Key 7, test 7 move pan by 20 clockwise and tilt by 10 down
goal.absolute = false;
goal.pan = -0.349066;
goal.tilt = 0.174533;
// Need boost::bind to pass in the 'this' pointer
ac_.sendGoal(goal,
boost::bind(&RodneyHeadTestNode::doneCB, this, _1, _2),
boost::bind(&RodneyHeadTestNode::activeCB, this),
boost::bind(&RodneyHeadTestNode::feedbackCB, this, _1));
}
else if((msg->code == keyboard::Key::KEY_c) &&
((msg->modifiers & ~RodneyHeadTestNode::SHIFT_CAPS_NUM_LOCK_) == 0))
{
// Key 'c' or 'C', cancel action
if(moving_ == true)
{
ac_.cancelGoal();
}
}
else
{
;
}
}
当动作变为活动状态时，回调函数activeCB被调用，这里我们记录事实并设置一个成员变量，指示正在发生运动。
// Called once when the goal becomes active
void RodneyHeadTestNode::activeCB()
{
ROS_INFO("RodneyHeadTestNode: Goal just went active");
moving_ = true;
}
当收到动作进度反馈时，调用回调函数feedbackCB。如果您还记得，我们的反馈包括伺服电机在前往目标位置途中的当前位置。
// Called every time feedback is received for the goal
void RodneyHeadTestNode::feedbackCB(const head_control::point_headFeedbackConstPtr& feedback)
{
ROS_INFO("Feedback pan=%f, tilt=%f", feedback->current_pan, feedback->current_tilt);
}
当动作完成时，调用回调函数donCB。在这种情况下，结果数据为空。
// Called once when the goal completes
void RodneyHeadTestNode::doneCB(const actionlib::SimpleClientGoalState& state,
const head_control::point_headResultConstPtr& result)
{
ROS_INFO("RodneyHeadTestNode: Finished in state [%s]", state.toString().c_str());
moving_ = false;
}
Using the Code
在本文中，我们将分别测试这两个节点。在第四部分中，我们将它们结合起来，以便机器人可以在其头部运动范围内扫描房间，寻找它认识的人脸。
如前所述，在测试代码时，我将在树莓派上运行系统代码，并在单独的Linux工作站上运行测试代码。树莓派还将连接到Arduino nano，Arduino nano又连接到伺服电机，并运行文章第一部分的草图。
请注意，为了区分以下说明中的树莓派和工作站，代码在树莓派上位于名为“rodney_ws”的文件夹（工作区）中，在工作站上位于名为“test_ws”的文件夹中。
在树莓派上构建ROS包
如果尚未完成，请在树莓派上创建一个catkin工作区，并使用以下命令对其进行初始化
$ mkdir -p ~/rodney_ws/src
$ cd ~/rodney_ws/
$ catkin_make
将包* face_recognition*、* face_recognition_msgs*、* head_control*、* pan_tilt*和* servo_msgs*复制到* ~/rodney_ws/src*文件夹中，然后构建代码。一个小技巧是，我不会将代码复制到* src*文件夹中，而是在* src*文件夹中创建一个指向代码位置的符号链接。这样，我就可以使用相同代码文件创建多个工作区。
$ cd ~/rodney_ws/
$ catkin_make
检查构建是否无任何错误地完成。
在工作站上构建ROS测试包
您可以在树莓派上构建和运行测试包，但我将使用与树莓派在同一网络上的Linux工作站。
使用以下命令创建一个工作空间
$ mkdir -p ~/test_ws/src
$ cd ~/test_ws/
$ catkin_make
将face_recognition、face_recognition_msgs、head_control、pan_tilt、servo_msgs、rodney_recognition_test、rodney_head_test和ros-keyboard（来自https://github.com/lrse/ros-keyboard）复制到* ~/test_ws/src*文件夹中，然后使用以下命令构建代码
$ cd ~/test_ws/
$ catkin_make
检查构建是否无任何错误地完成。
提示
在工作站和树莓派上运行ROS代码和工具时，可能需要在多个终端上重复输入许多命令。在下一节中，我将包含要输入的完整命令，但这里有一些可以节省您所有这些输入的技巧。
在树莓派上，为了节省输入“source devel/setup.bash”，我已将其添加到树莓派的* .bashrc*文件中。
$ cd ~/
$ nano .bashrc
然后将 "source /home/ubuntu/rodney_ws/devel/setup.bash" 添加到文件末尾，保存并退出。
在工作站上运行测试代码和工具时，它也需要知道 ROS master 的位置，所以我已经将以下内容添加到工作站的 .bashrc 文件中。
alias rodney='source ~/test_ws/devel/setup.bash; \
export ROS_MASTER_URI=http://ubiquityrobot:11311'
然后只需在终端输入 "rodney"，这两个命令就会运行，省去了大量的输入。
运行代码
首先，我们将测试人脸识别节点。使用启动文件在树莓派上用以下命令启动节点
$ cd ~/rodney_ws/
$ source devel/setup.bash
$ roslaunch face_recognition test.launch
当节点在树莓派上运行时，我将使用同一网络上的Linux工作站运行一些测试。
注意：由于我们将使用用户定义的主题，因此代码也需要在该工作站上构建。如果您愿意，可以在运行系统节点的同一树莓派上运行测试。
在工作站上，运行以下命令以检查节点是否正在运行并连接到正确的主题。您可以在运行roslaunch的输出中看到主节点的名称。由于我使用的是Ubiquity ROS Ubuntu镜像并且没有更改名称，因此我的主节点是ubiquityrobot。
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ rqt_graph
如果代码的某一部分主题拼写错误，那么从图中就会很明显，因为节点不会被主题连接。
在另一个终端中，输入以下内容以查看图像。
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ rqt_image_view
在图像视图GUI中，您可以选择主题* /raspicam/image/compressed*来查看当前摄像头图像。对于测试，我将选择主题* /face_recognition_node/image/compressed*，图像目前将是空白的，但是当我们请求人脸识别操作时，我们将能够查看结果。
在一个新的终端中，运行测试节点，对来自摄像头的图像执行人脸识别过程。
$ cd ~/test_ws
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ source devel/setup.bash
$ rosrun rodney_recognition_test rodney_recognition_test_node.py
您可以再次在同一个终端中输入最后一行来重新运行该过程。每次运行的结果将显示在rqt_image_view窗口中并在终端中输出。
当我运行测试时，摄像头视野中没有人时，图像查看器显示了房间的图像，终端报告空结果，内容如下：
()
[]
当摄像头视野中出现我自己时，终端和图像查看器显示如下
(1,)
['Phil']
在图像中测试两个人时，系统对这两个对象都进行了训练，我得到了以下结果
(1, 2,)
[Phil, Dave]
---
您可以通过在终端中输入Ctrl-C来关闭工作站和树莓派上的每个终端。
接下来我们将测试头部控制节点。将Arduino连接到树莓派的USB端口后，使用启动文件通过以下命令启动节点：
$ cd ~/rodney_ws/
$ source devel/setup.bash
$ roslaunch head_control test.launch
代码启动时，头部将移动到默认位置。
接下来，我将使用rqt_graph和我们的测试代码来测试系统。在工作站上，运行以下命令启动键盘节点
$ cd ~/test_ws
$ source devel/setup.bash
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ rosrun keyboard keyboard
在工作站的第二个终端中，运行以下命令启动我们的测试节点
$ cd ~/test_ws
$ source devel/setup.bash
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ rosrun rodney_head_test rodney_head_test_node
在第三个终端中，运行以下命令启动rqt_graph
$ cd ~/test_ws
$ export ROS_MASTER_URI=http://ubiquityrobot:11311
$ rqt_graph
从图中，您应该看到正在测试的节点和正在运行的测试代码。您还应该看到由主题链接的节点。任何断开的链接都表明代码中主题拼写错误。
工作站还应该运行一个标题为“ROS 键盘输入”的小窗口。确保该窗口具有焦点，然后按下键进行以下测试。在扫描头部运动期间，您可以按下“c”键取消动作。
- 键“
1” - 头部将移动到最大平移和倾斜位置（左和下） - 键“
2” - 头部将移动到最小平移和倾斜位置（右和上） - 键“
3” - 头部将移回零平移和倾斜位置 - 键“
4” - 头部将向上倾斜到45度位置 - 键“
5” - 头部将从当前位置向上移动5度 - 键“
6” - 头部将从当前位置逆时针（左）移动20度 - 键“
7” - 头部将从当前位置顺时针（右）移动20度并向下移动10度
关注点
在本文的这一部分中，我们已将人脸识别和头部控制功能添加到第一部分的代码中，以完成我们的设计目标1。
在下一篇文章中，我将为罗德尼添加面部表情和语音，以完成设计目标2。
一段短视频展示了罗德尼运行设计目标1和2组合测试的情况。
历史
- 2018年8月18日：首次发布
- 2018年10月7日：版本2：对下一篇文章将涵盖的内容进行了细微更改，并更改了章节、部分和子部分以匹配第1部分
- 2019年1月11日：版本3：现在使用压缩图像、ROS标准单位弧度和用于人脸识别和头部控制节点的动作