def __init__(self, s_ParamPath):
"""
:param s_log_ParamPath: 配置文件路径
"""
self.log = initLog(s_ParamPath)
self.list_channel = get_channel_param(s_ParamPath)
# REDIS服务
self.data_bus_server = DataBus(strHost)
# 初始化状态服务
self.__st_sys_state = None
self.q_sys_state = Queue()
self.q_model_state = Queue()
self.status_server = StateServer(self.data_bus_server, self.log, self.q_sys_state, self.q_model_state, self)
# 初始化用户进程
# self.dict_q_sub = {}
# for s_channel in self.list_channel:
# self.dict_q_sub[s_channel] = Queue()
self.q_pub_data = Queue()
self.list_th_sub_data = []
self.th_pub_data = None
self.pro_usr = None
self.log.info('mod init successful!')
def usr_process(self, data_bus_server):
"""重写方法"""
# 功能接口状态类，用于接收接口类中的系统状态，同时上报功能进程的相关信息
sys_state = UsrSysState(self.q_sys_state, self.q_model_state, self.log)
if sys_state.get_sys_online():
self.log.info("read online process start, pid={} ".format(
os.getpid()))
# 从配置文件中获取雷达参数
st_lidar_param = get_lidarDev_param(self.s_Parampath)
sys_status_channel = 'src_data' # 数据发送频道名称
data_bus_server = DataBus(strHost)
# dict_src_data为保存接收各个基站数据的字典，键为每个基站的序号，
# 值为对应基站的数据,为BsData类的对象,BsData类保存了数据的各种信息
dict_src_data = {}
listBaseStationId = []
listNorthAngleT = []
for i in range(len(st_lidar_param.getListAttachedIp())):
dict_src_data["{}".format(i)] = BsData()
listBaseStationId.append(0)
listNorthAngleT.append(st_lidar_param.getAngleNorthT(i))
listSortNorthAngleT = sorted(listNorthAngleT)
for i in range(len(st_lidar_param.getListAttachedIp())):
if st_lidar_param.getHighSpeed():
listBaseStationId[i] = i + 1
else:
listBaseStationId[i] = listSortNorthAngleT.index(
listNorthAngleT[i]) + 1
timeForce10Fps = QTime() # TODO
timeForce10Fps.start() # TODO
timeExcept = QTime() # TODO
send_data = SendData()
getCoordinateFlag = True
bGetBS1data = False
BUFSIZE = 4096
nPort = st_lidar_param.getUdpServicePort()
ip_port = ('', nPort)
server = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # udp协议
server.bind(ip_port)
self.log.info("bind ip: {}".format(ip_port))
listAttachedIp = st_lidar_param.getListAttachedIp()
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)
bTimeMatch = st_lidar_param.boolTimeMatch()
while sys_state.get_sys_state():
try:
t0 = time.time()
data, client_addr = server.recvfrom(BUFSIZE)
self.log.info(
"step 1: get data from {} success, time is {} ".format(
(client_addr), getTimeStamp(time.time())))
tfirst = time.time()
headData = parsingHeadData(data)
if headData is None:
continue
if headData[2] != 0xA0:
if client_addr[0] in listAttachedIp and headData[
2] == 0xE1 and headData[3] == 0x02:
i = listAttachedIp.index(client_addr[0])
t3 = time.time()
npBoxInfo, listDevInfo = parseBaseStationData_np(
data)
if npBoxInfo is not None:
self.log.info(
"step 2: parse BaseStationData success, use {} ms"
.format(round((time.time() - t3) * 1000,
3)))
if getCoordinateFlag:
npBoxInfo[:,
6] = (npBoxInfo[:, 6] + 180 -
fAngleNorthT + 360) % 360
npBoxInfo[:, 0:
2] = stTransform.lonlat_to_xy_of_draw(
npBoxInfo[:, 19:21])
else:
npBoxInfo[:, 6] = (
npBoxInfo[:, 6] + 180 -
st_lidar_param.getAngleNorthT(i) +
360) % 360
else:
self.log.warning(
'parse BaseStationData_np failed...npBoxInfo is None! '
)
continue
ts = time.time()
dict_src_data["{}".format(i)].time = time.time()
if dict_src_data["{}".format(i)].new_data is None:
new_data = recvData()
new_data.boxInfo = npBoxInfo[:, :19]
new_data.time_Stamp = listDevInfo[0]
new_data.equip_id = listDevInfo[1]
new_data.frame_id = listDevInfo[2]
dict_src_data["{}".format(
i)].new_data = new_data
else:
dict_src_data["{}".format(
i)].new_data.boxInfo = npBoxInfo[:, :19]
dict_src_data["{}".format(
i)].new_data.time_Stamp = listDevInfo[0]
dict_src_data["{}".format(
i)].new_data.equip_id = listDevInfo[1]
dict_src_data["{}".format(
i)].new_data.frame_id = listDevInfo[2]
dict_src_data["{}".format(i)].online_flag = True
if bTimeMatch:
#=====================time match=====================#
if i == 0:
bGetBS1data = True
data_put = recvData()
data_put.boxInfo = npBoxInfo[:, :19]
data_put.frame_id = listDevInfo[2]
data_put.time_Stamp = listDevInfo[0]
data_put.equip_id = listDevInfo[1]
dict_src_data["{}".format(
i)].data_catch.append(data_put)
if len(dict_src_data["{}".format(
i)].data_catch) > 10:
dict_src_data["{}".format(
i)].data_catch.pop(0)
#=====================time match=====================#
self.log.info(
"step 3: put into dict_src_data over,time is : {}"
.format((time.time() - ts) * 1000))
if (time.time() - dict_src_data["0"].time) > 0.5:
dict_src_data["0"].online_flag = False
else:
self.log.error(
"reached ip address is not in listAttachedIp")
continue
if bTimeMatch:
self.log.info("use Time Match ! ")
#=====================time match=====================#
ngetFlag = False
if dict_src_data[
"0"].online_flag and bGetBS1data and len(
dict_src_data["0"].data_catch) > 2:
bGetBS1data = False
tBSData1 = dict_src_data["0"].data_catch[
-3].time_Stamp
dict_src_data["0"].new_data = dict_src_data[
"0"].data_catch[-3]
for i in range(len(dict_src_data)):
if i == 0 or len(dict_src_data["{}".format(
i)].data_catch) == 0:
continue
listTError = [
abs(listBSData.time_Stamp - tBSData1)
for listBSData in dict_src_data[
"{}".format(i)].data_catch
]
minTError = min(listTError)
self.log.info(
"BS{} time error is {},index is {}".format(
i, minTError,
listTError.index(minTError)))
if minTError < 0.05:
dict_src_data["{}".format(
i)].new_data = copy.deepcopy(
dict_src_data["{}".format(i)].
data_catch[listTError.index(
minTError)])
else:
dict_src_data["{}".format(
i)].new_data = None
timeForce10Fps.restart()
ngetFlag = True
elif not dict_src_data[
"0"].online_flag and timeForce10Fps.elapsed(
) > 100:
timeForce10Fps.restart()
ngetFlag = True
#=====================time match=====================#
else:
self.log.info("use Newest Frame ! ")
ngetFlag = False
if dict_src_data["0"].new_data is not None:
timeForce10Fps.restart()
ngetFlag = True
elif not dict_src_data[
"0"].online_flag and timeForce10Fps.elapsed(
) > 100:
timeForce10Fps.restart()
ngetFlag = True
# ngetFlag标志为True，往下进行，否则continue
if not ngetFlag:
if timeExcept.elapsed() > 0:
timeExcept.restart()
continue
except:
self.log.error("except! udpServerProcess!")
pass
# ngetFlag标志为True，往下进行 往下执行
try:
self.log.info("step 4: start concat, time is : {}".format(
getTimeStamp(time.time())))
t1 = time.time()
list_matrix_base = []
index = 0
# nFrameID = None
# nEqupId = None
list_data = []
for src_data in dict_src_data.values():
# 当前基站处于离线状态或者当前帧无数据:
if not src_data.online_flag or src_data.new_data is None:
if (time.time() - src_data.time
) > 0.5 and src_data.new_data is None:
self.log.warning(
"station No:{} has no Data".format(index +
1))
if src_data.online_flag:
src_data.online_flag = False
self.log.warning(
"station No:{} is offline".format(
index + 1))
# 保存单基站数据发送给平台 以基站号作为键将信息保存在字典，用于发送信息时根据基站号取出相应数据
send_data.nSingleFrame["{}".format(
listBaseStationId[index])] = 0
send_data.nSingleStamp["{}".format(
listBaseStationId[index])] = time.time()
send_data.nSingleSource["{}".format(
listBaseStationId[index]
)] = listBaseStationId[index]
send_data.recvTime["{}".format(
listBaseStationId[index])] = time.time()
send_data.stationState["{}".format(
listBaseStationId[index])] = 1
if not src_data.online_flag:
send_data.stationState["{}".format(
listBaseStationId[index])] = 2
index += 1
list_matrix_base.append(None)
continue
# if nFrameID is None:
# nFrameID = src_data.new_data.frame_id
# nEqupId = src_data.new_data.equip_id
# send_data.nEqupId = nEqupId
# 当前基站为第一号基站(入口基站)
# if index == 0:
# # 保存入口基站信息发送给平台 以基站号作为键将信息保存在字典，用于发送信息时根据基站号取出相应数据
# send_data.n0Frame = (src_data.new_data.frame_id[2]*256 + src_data.new_data.frame_id[3])
# send_data.n0TimeStamp = src_data.new_data.time_Stamp
# 保存单基站数据发送给平台 以基站号作为键将信息保存在字典，用于发送信息时根据基站号取出相应数据
send_data.nSingleFrame["{}".format(
listBaseStationId[index])] = (
src_data.new_data.frame_id[2] * 256 +
src_data.new_data.frame_id[3])
send_data.nSingleStamp["{}".format(
listBaseStationId[index]
)] = src_data.new_data.time_Stamp
send_data.nSingleSource["{}".format(
listBaseStationId[index]
)] = listBaseStationId[index]
send_data.recvTime["{}".format(
listBaseStationId[index])] = src_data.time
send_data.stationState["{}".format(
listBaseStationId[index])] = 0
data_base = src_data.new_data.boxInfo
data_base[:, 12] = listBaseStationId[index]
list_matrix_base.append(src_data.new_data.boxInfo)
list_data.append(index)
index += 1
##### 不同路口ip转发的数据进行坐标转换 ####
npStationdata0 = None
for i in range(len(list_matrix_base)):
if list_matrix_base[i] is None:
continue
npStationdata1 = list_matrix_base[i]
if npStationdata0 is None:
npStationdata0 = npStationdata1
continue
else:
npStationdata0 = np.concatenate(
(npStationdata0, npStationdata1), axis=0)
self.log.info(
"step 5: concatenate over ,use time: {} ms".format(
round((time.time() - t1) * 1000, 3)))
# print(npStationdata0)
if npStationdata0 is None:
self.log.warning("all baseStation has no data ")
continue
##### 拼接转发数据，准备转发 ####
tm = time.time()
BaseStationInfoList = []
for i in send_data.nSingleFrame.keys():
dict_tmp = {}
dict_tmp[
"baseStationSource"] = send_data.nSingleSource[
"{}".format(i)]
dict_tmp["state"] = send_data.stationState["{}".format(
i)]
dict_tmp["sourceframeNo"] = send_data.nSingleFrame[
"{}".format(i)]
dict_tmp["sourcetimeStamp"] = getTimeStamp(
send_data.nSingleStamp["{}".format(i)])
dict_tmp["sourceRecievetimeStamp"] = getTimeStamp(
send_data.recvTime["{}".format(i)])
dict_tmp["sourcelongitude"] = round(
st_lidar_param.getLidarLongitude(int(i) - 1), 7)
dict_tmp["sourcelatitude"] = round(
st_lidar_param.getLidarLatitude(int(i) - 1), 7)
dict_tmp["sourceangle"] = int(
st_lidar_param.getAngleNorthT(int(i) - 1))
BaseStationInfoList.append(dict_tmp)
npStationdata = getByteofNdarray(npStationdata0)
# entrance_info = [send_data.n0Frame, send_data.n0TimeStamp ]
dict_send_message = {}
dict_send_message["targetInfo"] = npStationdata
dict_send_message["baseStation"] = BaseStationInfoList
# dict_send_message["entrance_info"] = entrance_info
req = json.dumps(dict_send_message)
# req = npStationdata0.tobytes()
data_bus_server.publish(sys_status_channel, req)
self.log.info(
"step 6: send message ,use time: {} ms".format(
round((time.time() - tm) * 1000, 3)))
# 将使用过的基站的new_data置为None
for i in list_data:
dict_src_data["{}".format(i)].new_data = None
# print("dict_src_data", dict_src_data)
# if npStationdata0 is None or npStationdata0.shape[0] == 0:
# continue
self.log.info(
"step 7: the whole process use time: {} ms".format(
round((time.time() - t0) * 1000, 3)))
except:
traceback.print_exc()
continue
self.log.error('usr_process {} exit...'.format(self.log.name))
server.close()
else:
self.log.info("read offline process start, pid={} ".format(
os.getpid()))
sys_status_channel = 'src_data' # 数据发送频道名称
data_bus_server = DataBus(strHost)
t0 = time.time()
# strSelectFilePath = '/data/org_data_0602/orgData1.csv'
strSelectFilePath = '/data/data0430-08.txt'
strDataFile = strSelectFilePath
self.log.info("Start get data from {}!".format(strDataFile))
if 'orgData' in strDataFile:
dictFrame, nStartFrameId, nEndFrameId = get_dict_from_dir(
strDataFile)
elif '.txt' in strDataFile:
dictFrame, nStartFrameId, nEndFrameId = get_dict_from_txt(
strDataFile)
elif '.json' in strDataFile:
dictFrame, nStartFrameId, nEndFrameId = get_dict_from_json(
strDataFile)
else:
dictFrame, nStartFrameId, nEndFrameId = get_dict_from_csv(
strDataFile)
# 功能接口状态类，用于接收接口类中的系统状态，同时上报功能进程的相关信息
sys_state = UsrSysState(self.q_sys_state, self.q_model_state,
self.log)
while sys_state.get_sys_state():
try:
nFrameIdTemp = nStartFrameId
while nFrameIdTemp <= nEndFrameId:
npFrameData = dictFrame[nFrameIdTemp]
print("npFrameData", npFrameData)
nFrameIdTemp = int((nFrameIdTemp + 1) % MAX_FRAME_ID)
npStationdata = getByteofNdarray(npFrameData)
dict_send_message = {}
dict_send_message["targetInfo"] = npStationdata
dict_send_message["baseStation"] = []
req = json.dumps(dict_send_message)
data_bus_server.publish(sys_status_channel, req)
self.log.info("step 6: send message ")
time.sleep(0.1)
else:
self.log.info("over!! ")
continue
except:
traceback.print_exc()
continue
from Common.CommonDefine import *
from Common.Interface.DataBus import DataBus
if __name__ == '__main__':
sys_status_channel = 'channel1'
data_bus_server = DataBus(strHost)
t = time.time()
while True:
if time.time() - t > 100:
break
bytesSend = struct.pack('>3BQ', 1, 1, 1,
int(math.floor(time.time() * 1e6)))
data_bus_server.publish(sys_status_channel, bytesSend)
time.sleep(0.1)
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))
class Interface(object):
def __init__(self, s_ParamPath):
"""
:param s_log_ParamPath: 配置文件路径
"""
self.log = initLog(s_ParamPath)
self.list_channel = get_channel_param(s_ParamPath)
# REDIS服务
self.data_bus_server = DataBus(strHost)
# 初始化状态服务
self.__st_sys_state = None
self.q_sys_state = Queue()
self.q_model_state = Queue()
self.status_server = StateServer(self.data_bus_server, self.log, self.q_sys_state, self.q_model_state, self)
# 初始化用户进程
# self.dict_q_sub = {}
# for s_channel in self.list_channel:
# self.dict_q_sub[s_channel] = Queue()
self.q_pub_data = Queue()
self.list_th_sub_data = []
self.th_pub_data = None
self.pro_usr = None
self.log.info('mod init successful!')
def update_state(self):
sys_state = self.status_server.get_sys_state()
if self.__st_sys_state is None:
self.__st_sys_state = copy.deepcopy(sys_state)
if self.__st_sys_state.n_sys_state == 1:
self.start_usr_process()
elif self.__st_sys_state.n_sys_state == 3:
self.exit_cur_mod()
else:
if self.__st_sys_state.n_sys_state == 0 and sys_state.n_sys_state == 1:
self.start_usr_process()
elif self.__st_sys_state.n_sys_state == 0 and sys_state.n_sys_state == 3:
self.exit_cur_mod()
elif self.__st_sys_state.n_sys_state == 1 and sys_state.n_sys_state == 2:
self.stop_usr_process()
elif self.__st_sys_state.n_sys_state == 1 and sys_state.n_sys_state == 3:
self.exit_cur_mod()
if (self.__st_sys_state.n_sys_state == 1 and sys_state.n_sys_state == 0) or \
sys_state.n_sys_state < 0 or sys_state.n_sys_state > 3:
self.status_server.set_sys_state(self.__st_sys_state)
self.log.error('an error state:{}'.format(sys_state.n_sys_state))
else:
self.__st_sys_state = copy.deepcopy(sys_state)
if self.__st_sys_state.n_sys_state == 2:
self.__st_sys_state.n_sys_state = 0
self.log.debug('cur state:{}'.format(self.__st_sys_state.n_sys_state))
def start_usr_process(self):
"""启动用户进程"""
if self.pro_usr is not None and self.pro_usr.is_alive():
self.pro_usr.terminate()
self.pro_usr = Process(target=self.usr_process, args=(self.data_bus_server,))
self.pro_usr.start()
# for s_channel in self.list_channel:
# th_suber = threading.Thread(target=self.__th_data_subscriber, args=(s_channel,))
# th_suber.daemon = True
# th_suber.start()
# self.list_th_sub_data.append(th_suber)
self.th_pub_data = threading.Thread(target=self.__th_data_publisher)
self.th_pub_data.daemon = True
self.th_pub_data.start()
self.log.info('start usr process!')
def stop_usr_process(self):
"""结束用户进程"""
if self.pro_usr is not None and self.pro_usr.is_alive():
self.pro_usr.terminate()
self.pro_usr = None
self.log.info('usr process killed!')
while self.q_pub_data.qsize() > 0:
try:
self.q_pub_data.get(True, 0.1)
except:
continue
# for s_channel in self.dict_q_sub:
# q_sub_data = self.dict_q_sub[s_channel]
# while q_sub_data.qsize() > 0:
# try:
# q_sub_data.get(True, 0.1)
# except:
# continue
self.log.info('stop usr process!')
def usr_process(self, data_bus_server):
"""用户进程，在继承接口类后需要重写方法"""
pass
# def __th_data_subscriber(self, s_channel):
# """模块数据订阅线程"""
# time.sleep(0.1)
# list_sub = self.data_bus_server.get_subscriber([s_channel])
# dict_sub_data = {'channel': '', 'data': None, 'time': time.time()}
# while self.__st_sys_state.b_mod_running and self.__st_sys_state.n_sys_state == 1:
# try:
# list_msg = list_sub[0].parse_response(False, 0.1)
# dict_sub_data['channel'] = str(list_msg[1], encoding="utf-8")
# dict_sub_data['data'] = list_msg[2]
# dict_sub_data['time'] = time.time()
# self.dict_q_sub[s_channel].put(dict_sub_data)
# time.sleep(0.09)
# except:
# continue
# list_sub[0].close()
# self.log.warning('data-server th_sub_{} exit...'.format(s_channel))
def __th_data_publisher(self):
"""模块数据发布线程"""
time.sleep(0.1)
while self.__st_sys_state.b_mod_running and self.__st_sys_state.n_sys_state == 1:
try:
pub_data = self.q_pub_data.get(True, 0.01)
self.data_bus_server.publish(pub_data['channel'], pub_data['data'])
except:
pass
time.sleep(0.005)
self.log.warning('data-server th_data_pub exit...')
def exit_cur_mod(self):
"""模块数据发布线程"""
self.status_server.exit_state_server()
if self.pro_usr is not None and self.pro_usr.is_alive():
self.pro_usr.terminate()
while self.q_pub_data.qsize() > 0:
try:
self.q_pub_data.get(True, 0.01)
except:
continue
# for s_channel in self.dict_q_sub:
# q_sub_data = self.dict_q_sub[s_channel]
# while q_sub_data.qsize() > 0:
# try:
# q_sub_data.get(True, 0.1)
# except:
# continue
while self.q_sys_state.qsize() > 0:
try:
self.q_sys_state.get(True, 0.01)
except:
continue
while self.q_model_state.qsize() > 0:
try:
self.q_model_state.get(True, 0.01)
except:
continue
self.log.info('process exit...')
def join(self):
self.status_server.join()
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))