hailo-inference/ros_inference.py

import json
import os
import io
import time
import copy

from PIL import Image
from threading import Thread
from multiprocessing import Process

import ipdb

from detection_tools.utils.visualization_utils import \
            visualize_boxes_and_labels_on_image_array

from hailo_platform import (ConfigureParams, FormatType, HEF,
                            HailoStreamInterface, InferVStreams,
                            InputVStreamParams, OutputVStreamParams,
                            PcieDevice)

import numpy as np

import tensorflow as tf
from tensorflow.image import combined_non_max_suppression

import rclpy
from rclpy.node import Node

from std_msgs.msg import String
from sensor_msgs.msg import Image as ImageMsg
from vision_msgs.msg import Detection2DArray, Detection2D, BoundingBox2D, ObjectHypothesisWithPose
from geometry_msgs.msg import Pose2D

from cv_bridge import CvBridge

# Collect images from data files


class ImageMeta:
    def __init__(self, image_height, image_width, channels):
        self.image_height = image_height
        self.image_width = image_width
        self.channels = channels


class DataHandler:
    def __init__(self, path, image_meta):
        self.images_path = path
        self.image_meta = image_meta

    def load_data(self, preprocess_fn):
        names = []

        images_list = [img_name for img_name in os.listdir(self.images_path)
                       if os.path.splitext(os.path.join(self.images_path, img_name))[1] == '.jpg']
        dataset = np.zeros((1, self.image_meta.image_height, 
                               self.image_meta.image_width, 
                               self.image_meta.channels),
                           dtype=np.float32)

        for idx, img_name in enumerate(images_list):
            img = Image.open(os.path.join(self.images_path, img_name))
            img_preproc = preprocess_fn(img)
            dataset[idx, :, :, :] = np.array(img_preproc)
            names.append(img_name)
            break

        self.dataset = dataset
        self.names = names


    def _get_coco_labels(self):
        coco_names = json.load(open(os.path.join(os.path.dirname(__file__), 'coco_names.json')))
        coco_names = {int(k): {'id': int(k), 'name': str(v)} for (k, v) in coco_names.items()}
        return coco_names


    def get_labels(self, path):
        filename = os.path.join(os.path.dirname(__file__), path)
        names = json.load(open(filename))
        names = {int(k): {'id': int(k), 'name': str(v)} for (k, v) in names.items()}
        return names

def get_labels(path):
    filename = os.path.join(os.path.dirname(__file__), path)
    names = json.load(open(filename))
    names = {int(k): {'id': int(k), 'name': str(v)} for (k, v) in names.items()}
    return names

COCO_17_14 = {0: 0, 1: 1, 2: 2, 3: 3, 4: 4, 5: 5, 6: 6, 7: 7, 8: 8, 9: 9,
              10: 10, 11: 11, 12: 13, 13: 14, 14: 15, 15: 16, 16: 17, 17: 18,
              18: 19, 19: 20, 20: 21, 21: 22, 22: 23, 23: 24, 24: 25, 25: 27,
              26: 28, 27: 31, 28: 32, 29: 33, 30: 34, 31: 35, 32: 36, 33: 37,
              34: 38, 35: 39, 36: 40, 37: 41, 38: 42, 39: 43, 40: 44, 41: 46,
              42: 47, 43: 48, 44: 49, 45: 50, 46: 51, 47: 52, 48: 53, 49: 54,
              50: 55, 51: 56, 52: 57, 53: 58, 54: 59, 55: 60, 56: 61, 57: 62,
              58: 63, 59: 64, 60: 65, 61: 67, 62: 70, 63: 72, 64: 73, 65: 74,
              66: 75, 67: 76, 68: 77, 69: 78, 70: 79, 71: 80, 72: 81, 73: 82,
              74: 84, 75: 85, 76: 86, 77: 87, 78: 88, 79: 89, 80: 90}


class YoloProcessing:
    def __init__(self, imageMeta, classes):
        self.output_height = imageMeta.image_height
        self.output_width = imageMeta.image_width
        self.classes = classes

    def preproc(self, image, resize_side=256):

        '''
        imagenet-standard: aspect-preserving resize to 256px smaller-side,
        then central-crop to 224px
        '''
        new_width = int(image.width/image.height*resize_side)
        new_height = resize_side
        x, y = (new_width-self.output_width)/2, 0

        # Select area to crop
        area = (x, y, x+self.output_width, y+self.output_height)

        # Crop, show, and save image
        cropped_img = image.resize((new_width, new_height)).crop(area)
        return cropped_img

    # 20 x 20 -> 32
    # stride = 32
    def yolo_postprocess_numpy(self, net_out, anchors_for_stride, stride):
        """
        net_out is shape: [N, 19, 19, 255] or [N, 38, 38, 255] or [N, 76, 76, 255]
        first we reshape it to be as in gluon and then follow gluon's shapes.
        output_ind = 0 for stride 32, 1 for stride 16, 2 for stride 8.
        """

        # net_out = net_out.astype(np.float32) / 256
        num_classes = 4
        BS = net_out.shape[0]  # batch size
        H = net_out.shape[1]
        W = net_out.shape[2]

        num_anchors = anchors_for_stride.size // 2  # 2 params for each anchor.
        num_pred = 1 + 4 + num_classes  # 2 box centers, 2 box scales, 1 objness, num_classes class scores
        alloc_size = (128, 128)

        grid_x = np.arange(alloc_size[1])
        grid_y = np.arange(alloc_size[0])
        grid_x, grid_y = np.meshgrid(grid_x, grid_y)  # dims [128,128], [128,128]

        offsets = np.concatenate((grid_x[:, :, np.newaxis], grid_y[:, :, np.newaxis]), axis=-1)  # dim [128,128,2]
        offsets = np.expand_dims(np.expand_dims(offsets, 0), 0)  # dim [1,1,128,128,2]

        pred = net_out.transpose((0, 3, 1, 2))  # now dims are: [N,C,H,W] as in Gluon.
        pred = np.reshape(pred, (BS, num_anchors * num_pred, -1))  # dim [N, 255, HxW]
        # dim [N, 361, 255], we did it so that the 255 be the last dim and can be reshaped.
        pred = pred.transpose((0, 2, 1))
        pred = np.reshape(pred, (BS, -1, num_anchors, num_pred))  # dim [N, HxW, 3, 85]]

        raw_box_centers = pred[:, :, :, 0:2]  # dim [N, HxW, 3, 2]
        raw_box_scales = pred[:, :, :, 2:4]  # dim [N,HxW, 3, 2]

        objness = pred[:, :, :, 4:5]  # dim [N, HxW, 3, 1]
        class_pred = pred[:, :, :, 5:]  # dim [N, HxW, 3, 80]
        offsets = offsets[:, :, :H, :W, :]  # dim [1, 1, H, W, 2]
        offsets = np.reshape(offsets, (1, -1, 1, 2))  # dim [1, HxW, 1, 2]
        box_centers, box_scales, confidence, class_pred = self._yolo5_decode(
            raw_box_centers=raw_box_centers,
            raw_box_scales=raw_box_scales,
            objness=objness,
            class_pred=class_pred,
            anchors_for_stride=anchors_for_stride,
            offsets=offsets,
            stride=stride)

        class_score = class_pred * confidence  # dim [N, HxW, 3, 80]
        wh = box_scales / 2.0
        # dim [N, HxW, 3, 4]. scheme xmin, ymin, xmax, ymax
        bbox = np.concatenate((box_centers - wh, box_centers + wh), axis=-1)

        detection_boxes = np.reshape(bbox, (BS, -1, 1, 4))  # dim [N, num_detections, 1, 4]
        detection_scores = np.reshape(class_score, (BS, -1, num_classes))  # dim [N, num_detections, 80]

        # switching scheme from xmin, ymin, xmanx, ymax to ymin, xmin, ymax, xmax:
        detection_boxes_tmp = np.zeros(detection_boxes.shape)
        detection_boxes_tmp[:, :, :, 0] = detection_boxes[:, :, :, 1]
        detection_boxes_tmp[:, :, :, 1] = detection_boxes[:, :, :, 0]
        detection_boxes_tmp[:, :, :, 2] = detection_boxes[:, :, :, 3]
        detection_boxes_tmp[:, :, :, 3] = detection_boxes[:, :, :, 2]

        detection_boxes = detection_boxes_tmp  # now scheme is: ymin, xmin, ymax, xmax
        return detection_boxes.astype(np.float32), detection_scores.astype(np.float32)

    def _yolo5_decode(self, raw_box_centers, raw_box_scales, objness, class_pred, anchors_for_stride, offsets, stride):
        box_centers = (raw_box_centers * 2. - 0.5 + offsets) * stride
        box_scales = (raw_box_scales * 2) ** 2 * anchors_for_stride  # dim [N, HxW, 3, 2]
        return box_centers, box_scales, objness, class_pred

    def visualize_image(self, logits, image):
        labels = get_labels("data/daria_labels.json")
        image = visualize_boxes_and_labels_on_image_array(
            image,
            logits['detection_boxes'].numpy()[0],
            logits['detection_classes'][0],
            logits['detection_scores'].numpy()[0],
            labels,
            use_normalized_coordinates=True,
            max_boxes_to_draw=100,
            min_score_thresh=.5,
            agnostic_mode=False,
            line_thickness=4)

        Image.fromarray(np.uint8(image)).save('/home/maintenance/test.png')
        Image.fromarray(np.uint8(image)).show()


    def postprocessing(self, endnodes):
        """
        endnodes is a list of 3 output tensors:
        endnodes[0] - stride 32 of input
        endnodes[1] - stride 16 of input
        endnodes[2] - stride 8 of input
        Returns:
        a tensor with dims: [BS, Total_num_of_detections_in_image, 6]
        where:
            total_num_of_detections_in_image = H*W*((1/32^2) + (1/16^2) + (1/8^2))*num_anchors*num_classes,
            with H, W as input dims.
            If H=W=608, num_anchors=3, num_classes=80 (coco 2017), we get:
            total_num_of_detections = 1819440 ~ 1.8M detections per image for the NMS
        """
        H_input = 640
        W_input = 640
        anchors_list = [[10, 13, 16, 30, 33, 23], [30, 61, 62, 45, 59, 119], [116, 90, 156, 198, 373, 326]]
        # TODO make prettier
        strides = [8, 16, 32]

        for output_ind, output_branch in enumerate(endnodes):  # iterating over the output layers:
            stride = strides[::-1][output_ind]
            anchors_for_stride = np.array(anchors_list[::-1][output_ind])
            anchors_for_stride = np.reshape(anchors_for_stride, (1, 1, -1, 2))  # dim [1, 1, 3, 2]

            detection_boxes, detection_scores = self.yolo_postprocess_numpy(output_branch, 
                                                                       anchors_for_stride,
                                                                       stride)

            # detection_boxes is a [BS, num_detections, 1, 4] tensor, detection_scores is a
            # [BS, num_detections, num_classes] tensor
            detection_boxes = detection_boxes / H_input  # normalization of box coordinates to 1
            BS = endnodes[0].shape[0]
            H = H_input // stride
            W = W_input // stride
            num_anchors = anchors_for_stride.size // 2
            num_detections = H * W * num_anchors
            # detection_boxes.set_shape((BS, num_detections, 1, 4))
            # detection_scores.set_shape((BS, num_detections, num_classes))
            # concatenating the detections from the different output layers:
            if output_ind == 0:
                detection_boxes_full = detection_boxes
                detection_scores_full = detection_scores
            else:
                detection_boxes_full = tf.concat([detection_boxes_full, detection_boxes], axis=1)
                detection_scores_full = tf.concat([detection_scores_full, detection_scores], axis=1)

        score_threshold = 0.5
        nms_iou_threshold = 0.5
        labels_offset = 1

        (nmsed_boxes, nmsed_scores, nmsed_classes, num_detections) = \
            combined_non_max_suppression(boxes=detection_boxes_full,
                                         scores=detection_scores_full,
                                         score_threshold=score_threshold,
                                         iou_threshold=nms_iou_threshold,
                                         max_output_size_per_class=100,
                                         max_total_size=100)



        # adding offset to the class prediction and cast to integer
        def translate_coco_2017_to_2014(nmsed_classes):
            return np.vectorize(COCO_17_14.get)(nmsed_classes).astype(np.int32)

        nmsed_classes = tf.cast(tf.add(nmsed_classes, labels_offset), tf.int16)
        nmsed_classes = translate_coco_2017_to_2014(nmsed_classes)

        return {'detection_boxes': nmsed_boxes,
                'detection_scores': nmsed_scores,
                'detection_classes': nmsed_classes,
                'num_detections': num_detections}



class HailoHandler:
    def __init__(self, hef_path='hef/yolov5m.hef'):
        target = PcieDevice()
        self.hef = HEF(hef_path)

        # Configure network groups
        configure_params = ConfigureParams.create_from_hef(hef=self.hef,
                                        interface=HailoStreamInterface.PCIe)
        network_groups = target.configure(self.hef, configure_params)
        self.network_group = network_groups[0]

        self.input_vstreams_params = InputVStreamParams.make(self.network_group,
                                                        quantized=False,
                                                        format_type=FormatType.FLOAT32)

        self.output_vstreams_params = OutputVStreamParams.make(self.network_group, quantized=False, format_type=FormatType.FLOAT32)

        self.input_vstream_info = self.hef.get_input_vstream_infos()[0]
        self.output_vstream_infos = self.hef.get_output_vstream_infos()
        self.network_group_params = self.network_group.create_params()

    def run_hailo(self, dataset):

        input_data = {self.input_vstream_info.name: dataset}

        with InferVStreams(self.network_group, self.input_vstreams_params, self.output_vstreams_params) as infer_pipeline:
            with self.network_group.activate(self.network_group_params):
                infer_results = infer_pipeline.infer(input_data)

        out = [infer_results[i.name] for i in self.output_vstream_infos]
        return out

    def start_hailo_thread(self):
        self.hailo_async = True
        self.hailo_block = False
        self.input_data = None
        self._infer_results = None
        self.hailo_thread = Thread(target=self._hailo_async)
        self.hailo_thread.start()

    def _hailo_async(self):
        with InferVStreams(self.network_group, self.input_vstreams_params, self.output_vstreams_params)\
                as infer_pipeline:
            with self.network_group.activate(self.network_group_params):
                self._hailo_async_loop(infer_pipeline)


    def _hailo_async_loop(self, infer_pipeline):
        while self.hailo_async:
            if(not self.hailo_block and type(self.input_data) != type(None)):
                self._infer_results = None
                self.hailo_block = True
                infer_results = infer_pipeline.infer(self.input_data)
                self._infer_results = [infer_results[i.name] for i in self.output_vstream_infos]
                self.input_data = None
                self.hailo_block = False

    def hailo_input(self, input_data):
        while self.hailo_block:
            time.sleep(0.001)
        self.hailo_block = True
        self.input_data = input_data
        self.input_data = {self.input_vstream_info.name: input_data}
        self.hailo_block = False

    def hailo_output(self):
        while self.hailo_block:
            time.sleep(0.001)
        return self._infer_results


    def stop_hailo_thread(self):
        self.hailo_async = False
        self.hailo_thread.join()

class HailoNode(Node):

    def __init__(self):

        self._ros_init()
        self._metadata_init()
        self._object_init()
        self._thread_init()

    def __del__(self):
        self.hailo.stop_hailo_thread()
        self._thread_run = False
        self._post_process.join()

    def _ros_init(self):
        super().__init__('hailo_image_subscriber')
        self.sub = self.create_subscription(ImageMsg, '/r3_cam_left_0', self._image_callback, 10)
        self.pub = self.create_publisher(Detection2DArray, '/hailo_bounding_boxes', 10)
        self.pub_ping = self.create_publisher(String, '/ping', 1)

    def _metadata_init(self):
        # TODO into yaml file
        classes = 3
        self.image_meta = ImageMeta(640, 640, 3)
        self.processor = YoloProcessing(self.image_meta, classes)
        self.hailo_hef = 'hef/yolov5m_daria.hef'

    def _object_init(self):
        self.hailo = HailoHandler(self.hailo_hef)
        self.bridge = CvBridge()

    def _thread_init(self):
        self._thread_run = True
        self._new_input = False
        self.yolo_image = None
        self.hailo.start_hailo_thread()

        self.detections = None
        self.detections_new = False
        self.detections_mutex = False
        self._post_process = Thread(target=self._thread_postprocessing).start()
        self.publish_thread = Thread(target=self._thread_publish).start()

    def _image_callback(self, ros_image):
        image = self._convert_ros_to_pil(ros_image)
        self.yolo_image = self._preprocess(image)
        self.image_infer(self.yolo_image)
        self._new_input = True

    def _preprocess(self, image):
        image = self.processor.preproc(image)
        return self._dataset_from_image(image)

    def image_infer(self, data):
        self.hailo.hailo_input(data)

    def _thread_postprocessing(self):
        while self._thread_run:
            output = None
            while(output == None or not self._new_input):
                time.sleep(0.001)
                output = self.hailo.hailo_output()

            
            self._new_input = False
            now = time.time()
            self.detections_mutex = True
            self.detections = self.processor.postprocessing(output)
            self.detections_new = True
            self.detections_mutex = False
            print("postprocessing time: ", time.time() - now)
            self.processor.visualize_image(self.detections, self.yolo_image[0])
        
    def _thread_publish(self):
        while self._thread_run:
            while self.detections_mutex or not self.detections_new:
                time.sleep(0.001)
            self._publish_detection(self.detections)
            self.detections_new = False

    def _publish_ping(self, msg="ping"):
        s = String()
        s.data = msg
        self.pub_ping.publish(s)

    def _publish_detection(self, detections):

        labels = get_labels("data/daria_labels.json")

        detection_array = Detection2DArray()

        for bb in range(len(detections['detection_boxes'].numpy()[0])):
            boxes = detections['detection_boxes'].numpy()[0][bb]
            classes = detections['detection_classes'][0][bb]
            scores = detections['detection_scores'].numpy()[0][bb]
            if(scores > 0.01):
                bb = BoundingBox2D()
                bb.center = Pose2D()
                bb.center.x = float(boxes[0])
                bb.center.y = float(boxes[1])
                bb.size_x = float(boxes[2])
                bb.size_y = float(boxes[3])
                oh = ObjectHypothesisWithPose(id=str(labels[classes]), score=float(scores))
                detection = Detection2D(results=[oh], bbox=bb)
                detection_array.detections.append(detection)

        self.pub.publish(detection_array)

    def _convert_ros_to_pil(self, ros_image):
        try:
            img = self.bridge.imgmsg_to_cv2(ros_image, "rgb8")
            image = Image.fromarray(img)
        except CvBridgeError as e:
            print(e)
        return image

    def _dataset_from_image(self, image):
        dataset = np.zeros((1, self.image_meta.image_height, 
                               self.image_meta.image_width, 
                               self.image_meta.channels),
                           dtype=np.float32)
        dataset[0, :, :, :] = np.array(image)
        return dataset


def main(args=None):
    rclpy.init(args=args)
    hailo_node = HailoNode()
    rclpy.spin(hailo_node)


if __name__ == "__main__":
    main()