def th_win_sub(win_sys_sub):
while True:
try:
list_msg = win_sys_sub[0].parse_response(False, 0.1)
channel = str(list_msg[1], encoding="utf-8")
print(list_msg[2])
except:
continue
if __name__ == '__main__':
winSys_state_channel = 'winSys_state'
sys_state_channel = 'win_state'
data_bus_server = DataBus(strHost)
win_sys_sub = data_bus_server.get_subscriber([winSys_state_channel])
# th_win = threading.Thread(target=th_win_sub, args=(win_sys_sub,))
# th_win.daemon = True
# th_win.start()
# t = time.time()
while True:
# if time.time() - t > 100:
# break
winState = int(input())
dict_win_state = {'winState': winState,
'onlineState': 1,
'paramUpdate': 0,
'actCode': '',
'offlineFile': ''}
data_bus_server.publish(sys_state_channel, json.dumps(dict_win_state))
time.sleep(0.1)
def usr_process(self, data_bus_server):
"""重写方法"""
self.log.info("track process start, pid={} ".format(os.getpid()))
# 从配置文件中获取算法参数
st_alg_param = get_alg_param(self.s_Parampath)
sys_status_channel = 'track_data' # 数据发送频道名称
data_bus_server = DataBus(strHost)
nFrameIdTemp = 0
# #####***** 加载全域车道中心线信息 *****#####
global left_center, right_center, medium_center, bmp_img
mot_tracker_merge = Sort(left_center, medium_center, right_center,
bmp_img, st_alg_param.DisplayFrames,
st_alg_param.nMinHits)
# 功能接口状态类，用于接收接口类中的系统状态，同时上报功能进程的相关信息
sys_state = UsrSysState(self.q_sys_state, self.q_model_state, self.log)
# 获取用户订阅的subscriber列表，每个频道对应一个消息，为列表中的一个
list_sub = data_bus_server.get_subscriber(self.list_channel)
if sys_state.get_sys_online():
while sys_state.get_sys_state():
try:
try:
t0 = time.time()
list_msg = list_sub[0].parse_response(False, 0.01)
channel = str(list_msg[1], encoding="utf-8")
self.log.info(
'step 1:get data from {}'.format(channel))
trunAlg_start = time.time()
except:
self.log.warning('step 0:waitting for data')
continue
try:
t1 = time.time()
recv_data = json.loads(list_msg[2])
npStationdata0 = parseNdarrayofByte(
recv_data["targetInfo"]).reshape((-1, 19))
basestation_info = recv_data["baseStation"]
self.log.info(
'step 2:parse {} success,use time:{} ms'.format(
(channel), round((time.time() - t1) * 1000,
3)))
except:
self.log.error('step 2:parse wrong...')
npStationdata0 = None
continue
# print(npStationdata0)
tWait = time.time()
if npStationdata0 is not None:
nFrameIdTemp = int((nFrameIdTemp + 1) % MAX_FRAME_ID)
# print("nFrameIdTemp",nFrameIdTemp)
self.log.info('nFrameIdTemp: {}'.format(nFrameIdTemp))
# trackers_cov_old_for_sort = npStationdata0[:, [0, 1, 3, 4, 6, 2, 5, 7, 10, 9, 8, 11, 12, 13,14,15,16,17,18,19]]
# trackers_cov_old_for_sort[:, 4] = trackers_cov_old_for_sort[:, 4] * np.pi / 180
trackers_cov_old_for_sort = npStationdata0
# box_with_min_max, box_corners = get_min_max(trackers_cov_old_for_sort)
# #####***** 2021-0509 my add *****#####
# #####***** 在去重前进行车道航向角的规定 *****#####
add_dimension = np.zeros(
(trackers_cov_old_for_sort.shape[0], 1))
trackers_cov_old_for_sort = np.concatenate(
(trackers_cov_old_for_sort, add_dimension), axis=1)
trackers_cov_old_for_sort = cal_box_lane(
bmp_img, trackers_cov_old_for_sort)
# #####***** flag是标志位, 1表示第7列是航向角, 0表示第5列是航向角 *****#####
flag = 1
trackers_cov_old_for_sort = cal_map_angle(
trackers_cov_old_for_sort, left_center,
medium_center, right_center, flag)
box_with_min_max, box_corners = get_min_max(
trackers_cov_old_for_sort,
st_alg_param.fConeThreshold)
mathch, un_match1, un_match2, cost_matrix = fusion_box(
box_corners, box_corners, 0)
trackers_cov_old_for_sort = box_with_min_max
save_indice = []
delete_indice = []
for i in range(mathch.shape[0]):
match_row_indice = mathch[i][0]
match_column_indice = mathch[i][1]
if mathch[i][0] in delete_indice:
continue
if trackers_cov_old_for_sort[match_row_indice][
17] < trackers_cov_old_for_sort[
match_column_indice][17]:
# # #####***** 当目标的航向角不与坐标轴平行或者垂直时 *****#####
trackers_cov_old_for_sort[match_row_indice][
7] = trackers_cov_old_for_sort[
match_column_indice][7]
save_indice.append(match_row_indice)
delete_indice.append(match_column_indice)
trackers_cov_old_for_sort_new = trackers_cov_old_for_sort[
save_indice, :]
# # #####***** 但是对于场景中有的误检在目标旁边，采用距离去除 *****#####
un_match_object = trackers_cov_old_for_sort[
un_match1.tolist(), :]
un_match_object = fix_cluster(un_match_object, 1)
new_trackers_cov_old_for_sort = np.concatenate(
(trackers_cov_old_for_sort_new, un_match_object),
axis=0)
# print('my Time:', round((time.time()-tWait) *1000, 2), 'ms')
npStationdata0 = new_trackers_cov_old_for_sort
#tracker###
# #####***** 2021-0509 my add *****#####
box_final = np.zeros((len(npStationdata0), 17))
box_final[:, [
0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
15, 16
]] = npStationdata0[:, [
0, 1, 3, 4, 6, 2, 5, 7, 8, 9, 12, 13, 14, 15, 16,
18, 19
]]
self.log.info('step 3:fusion time:{}ms'.format(
round((time.time() - tWait) * 1000, 2)))
tWait = time.time()
trackers_all = mot_tracker_merge.update(
box_final, st_alg_param) # 将所有的跟踪到的物体都使用其进行跟踪
self.log.info('step 4:tracker time:{}ms'.format(
round((time.time() - tWait) * 1000, 2)))
trackers_covall = np.zeros((trackers_all.shape[0], 20))
trackers_covall[:, [
0, 1, 3, 4, 6, 2, 5, 7, 8, 9, 10, 11, 12, 13, 14,
15, 16, 17, 18, 19
]] = trackers_all[:, [
0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
15, 16, 17, 18, 19
]]
trackers_covall[:, 6] = trackers_covall[:, 6] * PI_rad
box_datall = box_np_ops.center_to_corner_box3d(
trackers_covall[:, :3],
trackers_covall[:, 3:6],
trackers_covall[:, 6],
origin=[0.5, 0.5, 0.5],
axis=2)
boxes_corners_trackersall_ = box_datall.reshape(-1, 24)
trackers_cov_ = np.hstack(
(trackers_covall, boxes_corners_trackersall_))
trunAlg = math.floor(
(time.time() - trunAlg_start) * 1000)
self.log.info(
'step 5:trunAlg time:{}ms'.format(trunAlg))
dict_result_send = {}
dict_result_send["deviceId"] = 158
dict_result_send["algorithmDur"] = trunAlg
dict_result_send["globalFrameNo"] = nFrameIdTemp
dict_result_send["globalTimeStamp"] = time.time()
dict_result_send[
"participantNum"] = trackers_cov_.shape[0]
dict_result_send[
"e1FrameParticipant"] = getByteofNdarray(
trackers_cov_)
dict_result_send["stationNum"] = len(basestation_info)
dict_result_send["sourceInfoList"] = basestation_info
self.log.info('e1FrameParticipant: {}'.format(
trackers_cov_.shape[0]))
bytesSend = json.dumps(dict_result_send)
try:
data_bus_server.publish(sys_status_channel,
bytesSend)
self.log.info('step 6:publish result_data success')
except:
self.log.error('step 6:publish result_data failed')
self.log.info(
'step 7:the whole track time use {} ms!'.format(
round((time.time() - t0) * 1000, 3)))
# time.sleep(1)
else:
print("npStationdata0 is None")
continue
except:
self.log.error('unexpected error')
pass
else:
try:
self.log.info("track offline process start, pid={} ".format(
os.getpid()))
st_lidar_param = get_lidarDev_param(self.s_Parampath2)
fLongitudeT = st_lidar_param.getLidarLongitude(0)
fLatitudeT = st_lidar_param.getLidarLatitude(0)
fAngleNorthT = st_lidar_param.getAngleNorthT(0)
stTransform = Lon_Lat_Meter_Transform(fLongitudeT, fLatitudeT,
fAngleNorthT)
while sys_state.get_sys_state():
try:
t0 = time.time()
list_msg = list_sub[0].parse_response(False, 0.01)
channel = str(list_msg[1], encoding="utf-8")
self.log.info(
'step 1:get data from {}'.format(channel))
trunAlg_start = time.time()
t1 = time.time()
recv_data = json.loads(list_msg[2])
npStationdata0 = parseNdarrayofByte(
recv_data["targetInfo"]).reshape((-1, 35))
basestation_info = recv_data["baseStation"]
self.log.info(
'step 2:parse {} success,use time:{} ms'.format(
(channel), round((time.time() - t1) * 1000,
3)))
except:
continue
e1FrameParticipant = parse_offline_data(
npStationdata0, stTransform, fAngleNorthT)
dict_result_send = {}
dict_result_send["deviceId"] = 158
dict_result_send["algorithmDur"] = 0
dict_result_send["globalFrameNo"] = 1
dict_result_send["globalTimeStamp"] = time.time()
dict_result_send["participantNum"] = npStationdata0.shape[
0]
dict_result_send["e1FrameParticipant"] = getByteofNdarray(
e1FrameParticipant)
dict_result_send["stationNum"] = len(basestation_info)
dict_result_send["sourceInfoList"] = basestation_info
self.log.info('e1FrameParticipant: {}'.format(
e1FrameParticipant.shape[0]))
bytesSend = json.dumps(dict_result_send)
try:
data_bus_server.publish(sys_status_channel, bytesSend)
self.log.info('step 3:publish result_data success')
except:
self.log.error('step 3:publish result_data failed')
except:
self.log.error('track offline process wrong...')
npStationdata0 = None
self.log.error('usr_process {} exit...'.format(self.log.name))