【ROS-3】实现图像目标检测-2

基于ZED2相机，实现视觉停障，只是简单的Demo。

实现思路：实时返回检测框中心的深度值。

#!/usr/bin/env python2
# -*- coding: utf-8 -*-

import sys
sys.path.append('../../../devel/lib/python2.7/dist-packages/darknet_ros_msgs/')
from darknet_ros_msgs.msg import BoundingBoxes, ObjectCount
from sensor_msgs.msg import Image, CompressedImage, CameraInfo
import rospy
import cv2
import numpy as np
from cv_bridge import CvBridge


x = 0
y = 0
len_box = 0
objectcount = 0


def boxdata(data):
    global x, y, len_box
    box = data.bounding_boxes
    len_box = len(box)
    # print(len_box)
    for i in range(len_box):
        xmin = box[i].xmin
        xmax = box[i].xmax
        ymin = box[i].ymin
        ymax = box[i].ymax
        # center
        x = xmin + (xmax - xmin)/2
        y = ymin + (ymax - ymin)/2


def depthdata(data):
    bridge = CvBridge()
    # 深度图
    depth_image = bridge.imgmsg_to_cv2(data)
    depth_image = np.array(depth_image, dtype=np.float)
    depth_image = depth_image * 1000
    depth_image = np.round(depth_image).astype(np.uint16)
    # print(len_box)
    if objectcount == 0:
        distance = 0
        # print("------------------------")

    else:
        real_z = depth_image[y, x] * 0.001
        # real_x = (x - ppx)/fx * real_z
        # real_y = (y - ppy)/fy * real_z
        # rospy.loginfo("potion:x=%f,y=%f,z=%f", real_x, real_y, real_z)
        distance = round(real_z, 2)
        print(distance)
        # rospy.loginfo("distance:", distance)
    if 0.5 < distance < 2:
        print("detect obstacle! distance =", distance)
    # 显示
    cv2.imshow('depth', depth_image)
    cv2.waitKey(2)


def countdata(data):
    global objectcount
    objectcount = data.count


if __name__ == '__main__':
    global fx, fy, ppx, ppy
    # fx = 261.3017
    # fy = 261.3017
    # ppx = 321.8623
    # ppy = 180.1331
    rospy.init_node('listener')
    rospy.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes, boxdata, queue_size=1)
    rospy.Subscriber('/zed2/zed_node/depth/depth_registered', Image, depthdata, queue_size=1)
    rospy.Subscriber('/darknet_ros/found_object', ObjectCount, countdata, queue_size=1)
    rospy.spin()