接下来我们就以机器视觉的任务为例，模拟实际机器人中节点的实现过程。
image-20220527101249842.png

安装opencv库

  1. sudo apt install python3-opencv

图片资源处理

将图片放在package下img目录下

image.png

在setup.py中配置

  1. data_files=[
  2.         ('share/ament_index/resource_index/packages',
  3.             ['resource/' + package_name]),
  4.         ('share/' + package_name, ['package.xml']),
  5.         (os.path.join('share', package_name), glob('img/*')),
  6.     ],

需要导入os和glob

  1. import os
  2. from glob import glob

加载图片

  1. from ament_index_python.packages import get_package_share_path
  4. img_dir_path = get_package_share_path('first_pkg')
  5. apple_path = img_dir_path / 'apple.jpg'
  7. image = cv2.imread(str(apple_path))  # 读取图像

完整代码

  1. from ament_index_python.packages import get_package_share_path
  3. import rclpy                            # ROS2 Python接口库
  4. from rclpy.node import Node             # ROS2 节点类
  6. import cv2                              # OpenCV图像处理库
  7. import numpy as np                      # Python数值计算库
  9. lower_red = np.array([0, 90, 128])     # 红色的HSV阈值下限
  10. upper_red = np.array([180, 255, 255])  # 红色的HSV阈值上限
  12. def object_detect(image):
  13.     hsv_img = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)                               # 图像从BGR颜色模型转换为HSV模型
  14.     mask_red = cv2.inRange(hsv_img, lower_red, upper_red)                          # 图像二值化
  16.     contours, hierarchy = cv2.findContours(mask_red, cv2.RETR_LIST, cv2.CHAIN_APPROX_NONE) # 图像中轮廓检测
  18.     for cnt in contours:                                                          # 去除一些轮廓面积太小的噪声
  19.         if cnt.shape[0] < 150:
  20.             continue
  22.         (x, y, w, h) = cv2.boundingRect(cnt)                                      # 得到苹果所在轮廓的左上角xy像素坐标及轮廓范围的宽和高
  23.         cv2.drawContours(image, [cnt], -1, (0, 255, 0), 2)                        # 将苹果的轮廓勾勒出来
  24.         cv2.circle(image, (int(x+w/2), int(y+h/2)), 5, (0, 255, 0), -1)           # 将苹果的图像中心点画出来
  26.     cv2.imshow("object", image)                                                    # 使用OpenCV显示处理后的图像效果
  27.     cv2.waitKey(0)
  28.     cv2.destroyAllWindows()
  30. def main(args=None):                                                              # ROS2节点主入口main函数
  31.     rclpy.init(args=args)                                                         # ROS2 Python接口初始化
  32.     node = Node("node_object")                                                     # 创建ROS2节点对象并进行初始化
  33.     node.get_logger().info("ROS2节点示例:检测图片中的苹果")
  35.     img_dir_path = get_package_share_path('first_pkg')
  36.     apple_path = img_dir_path / 'apple.jpg'
  38.     image = cv2.imread(str(apple_path))  # 读取图像
  39.     print(image)
  40.     object_detect(image)                                                            # 苹果检测
  41.     rclpy.spin(node)                                                               # 循环等待ROS2退出
  42.     node.destroy_node()                                                            # 销毁节点对象
  43.     rclpy.shutdown()                                                               # 关闭ROS2 Python接口

摄像头识别完整代码

  1. import rclpy                            # ROS2 Python接口库
  2. from rclpy.node import Node             # ROS2 节点类
  4. import cv2                              # OpenCV图像处理库
  5. import numpy as np                      # Python数值计算库
  7. lower_red = np.array([0, 90, 128])     # 红色的HSV阈值下限
  8. upper_red = np.array([180, 255, 255])  # 红色的HSV阈值上限
  10. def object_detect(image):
  11.     hsv_img = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)                       # 图像从BGR颜色模型转换为HSV模型
  12.     mask_red = cv2.inRange(hsv_img, lower_red, upper_red)                  # 图像二值化
  14.     contours, hierarchy = cv2.findContours(mask_red, cv2.RETR_LIST, cv2.CHAIN_APPROX_NONE) # 图像中轮廓检测
  16.     for cnt in contours:                                                   # 去除一些轮廓面积太小的噪声
  17.         if cnt.shape[0] < 150:
  18.             continue
  20.         (x, y, w, h) = cv2.boundingRect(cnt)                               # 得到苹果所在轮廓的左上角xy像素坐标及轮廓范围的宽和高
  21.         cv2.drawContours(image, [cnt], -1, (0, 255, 0), 2)                 # 将苹果的轮廓勾勒出来
  22.         cv2.circle(image, (int(x+w/2), int(y+h/2)), 5, (0, 255, 0), -1)    # 将苹果的图像中心点画出来
  24.     cv2.imshow("object", image)                                            # 使用OpenCV显示处理后的图像效果
  25.     cv2.waitKey(50)
  27. def main(args=None):                                                       # ROS2节点主入口main函数
  28.     rclpy.init(args=args)                                                  # ROS2 Python接口初始化
  29.     node = Node("node_object_webcam")                                      # 创建ROS2节点对象并进行初始化
  30.     node.get_logger().info("ROS2节点示例：检测图像中的苹果")
  32.     cap = cv2.VideoCapture(0)
  34.     while rclpy.ok():
  35.         ret, image = cap.read()          # 读取一帧图像
  37.         if ret == True:
  38.             object_detect(image)          # 苹果检测
  40.     node.destroy_node()                  # 销毁节点对象
  41.     rclpy.shutdown()                     # 关闭ROS2 Python接口