Program flow not correct with ROS

edit

This is an AI object detection routine for the Oak-D camera. It worked perfectly before I tried to add ROS to the program so that the camera AI image could be adjusted according to the machine speed. The camera will be instructed to use one of three slices of the image according to the number in "distance_value".

This uses Ubuntu 20.04, Python 3.8, OpenCV, and ROS Noetic. It's to run on a Raspberry Pi but this version will run on a laptop. I don't think I'm calling my functions properly or at the right time. When I run it, it's stuck in a loop and doesn't show an image until I hit CTRL-C, then it must be ended with CTRL-Z. Can an expert please help?

#!/usr/bin/env python3
"""
The code is edited from docs (https://docs.luxonis.com/projects/api/en/latest/samples/Yolo/tiny_yolo/)
We add parsing from JSON files that contain configuration.
"""
from pathlib import Path
import sys
import cv2
import depthai as dai
import numpy as np
import time
import argparse
import json
import blobconverter
import rospy
from std_msgs.msg import String
from time import sleep
# import RPi.GPIO as GPIO
# from gpiozero import Servo

z = ([0,0,0,0,0,0,0,0])
r = ([0,0,0,0,0,0,0,0])

# parse arguments
parser = argparse.ArgumentParser()
parser.add_argument("-m", "--model", help="Provide model name or model path for inference",
                    default='/home/steve/wide-weeds/openvino/result_openvino_2021.4_6shave.blob', type=str)
parser.add_argument("-c", "--config", help="Provide config path for inference",
                    default='/home/steve/wide-weeds/openvino/result.json', type=str)
args = parser.parse_args()

# parse config
configPath = Path(args.config)
if not configPath.exists():
    raise ValueError("Path {} does not exist!".format(configPath))

with configPath.open() as f:
    config = json.load(f)
nnConfig = config.get("nn_config", {})

# parse input shape
if "input_size" in nnConfig:
    W, H = tuple(map(int, nnConfig.get("input_size").split('x')))

# extract metadata
metadata = nnConfig.get("NN_specific_metadata", {})
classes = metadata.get("classes", {})
coordinates = metadata.get("coordinates", {})
anchors = metadata.get("anchors", {})
anchorMasks = metadata.get("anchor_masks", {})
iouThreshold = metadata.get("iou_threshold", {})
confidenceThreshold = metadata.get("confidence_threshold", {})

print(metadata)

# parse labels
nnMappings = config.get("mappings", {})
labels = nnMappings.get("labels", {})

# get model path
nnPath = args.model
if not Path(nnPath).exists():
    print("No blob found at {}. Looking into DepthAI model zoo.".format(nnPath))
    nnPath = str(blobconverter.from_zoo(args.model, shaves = 6, zoo_type = "depthai", use_cache=True))
# sync outputs
syncNN = True

# Create pipeline
pipeline = dai.Pipeline()

# Define sources and outputs
camRgb = pipeline.create(dai.node.ColorCamera)
detectionNetwork = pipeline.create(dai.node.YoloDetectionNetwork)

# Properties
camRgb.setPreviewSize(1280, 672)
camRgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1080_P)
camRgb.setInterleaved(False)
camRgb.setColorOrder(dai.ColorCameraProperties.ColorOrder.BGR)
# camRgb.setVideoSize(1920, 320)
camRgb.setFps(15)
# Faster video not needed, I think, at this point.

manip = pipeline.create(dai.node.ImageManip)
camRgb.preview.link(manip.inputImage)
configIn = pipeline.create(dai.node.XLinkIn)
xout = pipeline.create(dai.node.XLinkOut)

configIn.setStreamName('config')
xout.setStreamName("out1")
manip.out.link(xout.input)

# choose from one of three slices ...