def process(self, net_img, net_out, net_dep):
pred_car = self.yolo.predict(net_out[:3])
# --------------- data[5] is depth --------------- #
'''
if net_dep: # net_dep != None
x = int(net_dep.shape[1] * pred_car[0, 2])
y = int(net_dep.shape[0] * pred_car[0, 1])
pred_car[0, 5] = net_dep[y, x]
else:
pred_car[0, 5] = -1
'''
# ---------------- data[5] is azi ---------------- #
x = pred_car[0, -24:]
prob = np.exp(x) / np.sum(np.exp(x), axis=0)
c = sum(_cos_offset * prob)
s = sum(_sin_offset * prob)
vec_ang = math.atan2(s, c)
pred_car[0, 5] = vec_ang
# ------------------------------------------------- #
yolo_gluon.switch_print(
'cam to pub: %f' %
(rospy.get_rostime() - self.net_img_time).to_sec(), verbose)
self.ros_publish_array(self.car_pub, self.mat_car, pred_car[0])
self.visualize(pred_car, net_img)
def run(self):
print(global_variable.green)
print('Start Single Thread Video Node')
print(global_variable.reset_color)
h, w = self.yolo.size
rate = rospy.Rate(30)
while not rospy.is_shutdown():
if self.image is None:
print('Wait For Image')
rate.sleep()
continue
try:
# -------------------- additional image info-------------------- #
now = rospy.get_rostime()
net_img_time, net_img_seq = self.img_cb_time, self.img_cb_seq
net_img, net_dep = copy.copy(self.image), copy.copy(self.depth_image)
yolo_gluon.switch_print('cam to net: %f' % (now - net_img_time).to_sec(), verbose)
net_out = self.inference(cv2.resize(net_img, (w, h)))
# -------------------- additional image info-------------------- #
now = rospy.get_rostime()
self.net_img_time, self.net_img_seq = net_img_time, net_img_seq
yolo_gluon.switch_print('net done time: %f' % (now - net_img_time).to_sec(), verbose)
self.process(net_img, net_out, net_dep)
rate.sleep()
except Exception as e:
rospy.signal_shutdown('main_thread Error')
print(global_variable.red + e + global_variable.reset_color)
def video(args):
global net_img_g, net_out_g, net_thread_start
h, w = LPD.size
if args.trt: # tensorRT Inference can't in thread
import numpy as np
from yolo_modules.tensorrt_module import do_inference_wrapper
from yolo_modules.tensorrt_module import get_engine_wrapper
engine_wrapper = get_engine_wrapper(args.version)
net_out_shape = (1, LPD.LP_slice_point[-1], h / 2**LPD.num_downsample,
w / 2**LPD.num_downsample)
else:
import mxnet
# mx_resize = mxnet.image.ForceResizeAug((w, h), interp=2) # not always available
net = yolo_gluon.init_executor(LPD.export_file, (h, w), LPD.ctx[0])
yolo_gluon.test_inference_rate(net, (1, 3, h, w),
cycles=100,
ctx=LPD.ctx[0])
rospy.init_node("LP_Detection_Video_Node", anonymous=True)
threading.Thread(target=_video_thread).start()
net_thread_start = True
while not rospy.is_shutdown():
if 'img_g' not in globals() or img_g is None:
print('Wait For Image')
time.sleep(1.0)
continue
if not net_thread_start:
time.sleep(0.01)
continue
try:
net_start_time = time.time() # tic
net_img = img_g.copy() # (480, 640, 3), np.float32
net_img = cv2.resize(net_img, (w, h)) # (320, 512, 3)
if args.trt:
# if cuMemcpyHtoDAsync failed: invalid argument, check input image size
trt_outputs = do_inference_wrapper(engine_wrapper, net_img)
net_out = np.array(trt_outputs).reshape(
net_out_shape) # list to np.array
else:
nd_img = yolo_gluon.cv_img_2_ndarray(
net_img, LPD.ctx[0]) #, mxnet_resize=mx_resize)
net_out = net.forward(is_train=False, data=nd_img)[0]
net_out[-1].wait_to_read()
yolo_gluon.switch_print(time.time() - net_start_time,
video_verbose)
net_img_g = net_img
net_out_g = net_out
net_thread_start = False
except Exception as e:
print(global_variable.red)
rospy.signal_shutdown('main_thread Error')
print(e)
print(global_variable.reset_color)
sys.exit(0)
def __call__(self):
print(global_variable.green)
print('Start Double Thread Video Node')
ctx = self.ctx[0]
h, w = self.yolo.size
threading.Thread(target=self._video_thread).start()
self.net_thread_start = True
while not rospy.is_shutdown():
if self.image is None:
print('Net thread Wait For Image')
time.sleep(1.0)
continue
if not self.net_thread_start:
time.sleep(0.01)
continue
# -------------------- additional image info-------------------- #
net_img_time, net_img_seq = self.img_cb_time, self.img_cb_seq
now = rospy.get_rostime()
yolo_gluon.switch_print(
'cam to net: %f' % (now - net_img_time).to_sec(), verbose)
net_dep = copy.copy(self.depth_image)
net_img = copy.copy(self.image)
net_img = cv2.resize(net_img, (w, h))
# -------------------- image -------------------- #
if self.trt:
pass
else:
nd_img = yolo_gluon.cv_img_2_ndarray(net_img, ctx)
# if self.yolo.use_fp16:
# nd_img = nd_img.astype('float16')
# nd_img = yolo_gluon.nd_white_balance(nd_img, bgr=[1.0, 1.0, 1.0])
net_out = self.yolo.net.forward(is_train=False, data=nd_img)
net_out[0].wait_to_read()
# -------------------- additional image info-------------------- #
self.net_img_time = net_img_time
self.net_img_seq = net_img_seq
now = rospy.get_rostime()
yolo_gluon.switch_print(
'net done time: %f' % (now - net_img_time).to_sec(), verbose)
# -------------------- image -------------------- #
self.net_dep = net_dep
self.net_img = net_img
self.net_out = net_out
self.net_thread_start = False
def __call__(self):
print(global_variable.green)
print('Start Double Thread Video Node')
h, w = self.yolo.size
threading.Thread(target=self._video_thread).start()
self.net_thread_start = True
while not rospy.is_shutdown():
if self.image is None:
print('Net thread Wait For Image')
time.sleep(1.0)
continue
if not self.net_thread_start:
time.sleep(0.01)
continue
try:
# ------------------ additional image info In ------------------ #
now = rospy.get_rostime()
net_img_time, net_img_seq = self.img_cb_time, self.img_cb_seq
net_img, net_dep = copy.copy(self.image), copy.copy(self.depth_image)
yolo_gluon.switch_print('cam to net: %f' % (now - net_img_time).to_sec(), verbose)
net_out = self.inference(cv2.resize(net_img, (w, h)))
# -------------------- additional image info-------------------- #
now = rospy.get_rostime()
yolo_gluon.switch_print('net done time: %f' % (now - net_img_time).to_sec(), verbose)
# ------------------ additional image info Out ------------------ #
self.net_img_time, self.net_img_seq = net_img_time, net_img_seq
self.net_dep, self.net_img, self.net_out = net_dep, net_img, net_out
self.net_thread_start = False
except Exception as e:
rospy.signal_shutdown('main_thread Error')
print(global_variable.red + e + global_variable.reset_color)
sys.exit(0)