produce_window.go 67 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448144914501451145214531454145514561457145814591460146114621463146414651466146714681469147014711472147314741475147614771478147914801481148214831484148514861487148814891490149114921493149414951496149714981499150015011502150315041505150615071508150915101511151215131514151515161517151815191520152115221523152415251526152715281529153015311532153315341535153615371538153915401541154215431544154515461547154815491550155115521553155415551556155715581559156015611562156315641565156615671568156915701571157215731574157515761577157815791580158115821583158415851586158715881589159015911592159315941595159615971598159916001601160216031604160516061607160816091610161116121613161416151616161716181619162016211622162316241625162616271628162916301631163216331634163516361637163816391640164116421643164416451646164716481649165016511652165316541655165616571658165916601661166216631664166516661667166816691670167116721673167416751676167716781679168016811682168316841685168616871688168916901691169216931694169516961697169816991700170117021703170417051706170717081709171017111712171317141715171617171718171917201721172217231724172517261727172817291730173117321733173417351736173717381739174017411742174317441745174617471748174917501751175217531754175517561757175817591760176117621763176417651766176717681769177017711772177317741775177617771778177917801781178217831784178517861787178817891790179117921793179417951796179717981799180018011802180318041805180618071808180918101811181218131814181518161817181818191820182118221823182418251826182718281829183018311832183318341835183618371838183918401841184218431844184518461847184818491850185118521853185418551856185718581859186018611862186318641865186618671868186918701871187218731874187518761877187818791880188118821883188418851886188718881889
  1. package service
  2. import (
  3. commonConfig "cicv-data-closedloop/aarch64/pjisuv/common/config"
  4. "cicv-data-closedloop/aarch64/pjisuv/common/service"
  5. masterConfig "cicv-data-closedloop/aarch64/pjisuv/master/config"
  6. "cicv-data-closedloop/common/config/c_log"
  7. commonEntity "cicv-data-closedloop/common/entity"
  8. "cicv-data-closedloop/common/util"
  9. "cicv-data-closedloop/pjisuv_msgs"
  10. "cicv-data-closedloop/pjisuv_param"
  11. "github.com/bluenviron/goroslib/v2"
  12. "github.com/bluenviron/goroslib/v2/pkg/msgs/geometry_msgs"
  13. "github.com/bluenviron/goroslib/v2/pkg/msgs/nav_msgs"
  14. "github.com/bluenviron/goroslib/v2/pkg/msgs/sensor_msgs"
  15. "github.com/bluenviron/goroslib/v2/pkg/msgs/tf2_msgs"
  16. "github.com/bluenviron/goroslib/v2/pkg/msgs/visualization_msgs"
  17. "math"
  18. "sync"
  19. "time"
  20. )
  21. // 所有共享变量
  22. var (
  23. pjisuvParam = pjisuv_param.PjisuvParam{
  24. ObjDicOfTpperception: make(map[uint32][]float32),
  25. ObjTypeDicOfTpperception: make(map[uint32]uint8),
  26. ObjSpeedDicOfTpperception: make(map[uint32]float64),
  27. }
  28. // 保存时间窗口需要锁,防止数据竟态
  29. saveTimeWindowMutex sync.Mutex
  30. // /cicv_location
  31. mutexOfCicvLocation sync.RWMutex
  32. // /tpperception
  33. mutexOfTpperception sync.RWMutex
  34. // /pj_control_pub
  35. mutexOfPjControlPub sync.RWMutex
  36. // /data_read
  37. mutexOfDataRead sync.RWMutex
  38. // /pj_vehicle_fdb_pub
  39. mutexOfCicvAmrTrajectory sync.RWMutex
  40. latestTimeWindowEnd = util.GetTimeCustom(time.Now())
  41. // 每个触发器5秒触发一次
  42. triggerInterval = 3.0
  43. )
  44. // 负责监听所有主题并修改时间窗口
  45. func ProduceWindow() {
  46. c_log.GlobalLogger.Info("订阅者 goroutine,启动。")
  47. var err error
  48. subscribers := make([]*goroslib.Subscriber, len(commonConfig.SubscribeTopics))
  49. subscribersTimes := make([]time.Time, len(commonConfig.SubscribeTopics))
  50. subscribersTimeMutexes := make([]sync.Mutex, len(commonConfig.SubscribeTopics))
  51. subscribersMutexes := make([]sync.Mutex, len(commonConfig.SubscribeTopics))
  52. for i, topic := range commonConfig.SubscribeTopics {
  53. for {
  54. // 增加了可扩展性
  55. if topic == masterConfig.TopicOfCicvExtend {
  56. go func() {
  57. for {
  58. time.Sleep(time.Duration(3500) * time.Millisecond)
  59. for _, f := range masterConfig.RuleOfCicvExtend {
  60. label := f(pjisuvParam)
  61. if label != "" {
  62. saveTimeWindow(label, util.GetNowTimeCustom(), commonEntity.GetLastTimeWindow())
  63. subscribersTimes[i] = time.Now()
  64. break
  65. }
  66. }
  67. }
  68. }()
  69. }
  70. // 其他常规监听器
  71. c_log.GlobalLogger.Info("创建订阅者订阅话题:" + topic)
  72. // 1
  73. if topic == masterConfig.TopicOfAmrPose && (len(masterConfig.RuleOfAmrPose1) > 0 || len(masterConfig.RuleOfAmrPose2) > 0) {
  74. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  75. Node: commonConfig.RosNode,
  76. Topic: topic,
  77. Callback: func(data *visualization_msgs.MarkerArray) {
  78. subscribersTimeMutexes[i].Lock()
  79. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  80. subscribersMutexes[i].Lock()
  81. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  82. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  83. faultLabel := ""
  84. if len(masterConfig.RuleOfAmrPose1) > 0 {
  85. for _, f := range masterConfig.RuleOfAmrPose1 {
  86. faultLabel = f(data)
  87. if faultLabel != "" {
  88. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  89. subscribersTimes[i] = time.Now()
  90. goto TriggerSuccess
  91. }
  92. }
  93. }
  94. if len(masterConfig.RuleOfAmrPose2) > 0 {
  95. for _, f := range masterConfig.RuleOfAmrPose2 {
  96. faultLabel = f(data, &pjisuvParam)
  97. if faultLabel != "" {
  98. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  99. subscribersTimes[i] = time.Now()
  100. goto TriggerSuccess
  101. }
  102. }
  103. }
  104. TriggerSuccess:
  105. subscribersMutexes[i].Unlock()
  106. }
  107. subscribersTimeMutexes[i].Unlock()
  108. },
  109. })
  110. }
  111. // 2
  112. if topic == masterConfig.TopicOfBoundingBoxesFast && (len(masterConfig.RuleOfBoundingBoxesFast1) > 0 || len(masterConfig.RuleOfBoundingBoxesFast2) > 0) {
  113. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  114. Node: commonConfig.RosNode,
  115. Topic: topic,
  116. Callback: func(data *pjisuv_msgs.BoundingBoxArray) {
  117. subscribersTimeMutexes[i].Lock()
  118. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  119. subscribersMutexes[i].Lock()
  120. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  121. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  122. faultLabel := ""
  123. if len(masterConfig.RuleOfBoundingBoxesFast1) > 0 {
  124. for _, f := range masterConfig.RuleOfBoundingBoxesFast1 {
  125. faultLabel = f(data)
  126. if faultLabel != "" {
  127. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  128. subscribersTimes[i] = time.Now()
  129. goto TriggerSuccess
  130. }
  131. }
  132. }
  133. if len(masterConfig.RuleOfBoundingBoxesFast2) > 0 {
  134. for _, f := range masterConfig.RuleOfBoundingBoxesFast2 {
  135. faultLabel = f(data, &pjisuvParam)
  136. if faultLabel != "" {
  137. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  138. subscribersTimes[i] = time.Now()
  139. goto TriggerSuccess
  140. }
  141. }
  142. }
  143. TriggerSuccess:
  144. subscribersMutexes[i].Unlock()
  145. }
  146. subscribersTimeMutexes[i].Unlock()
  147. },
  148. })
  149. }
  150. // 3
  151. if topic == masterConfig.TopicOfCameraFault && (len(masterConfig.RuleOfCameraFault1) > 0 || len(masterConfig.RuleOfCameraFault2) > 0) {
  152. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  153. Node: commonConfig.RosNode,
  154. Topic: topic,
  155. Callback: func(data *pjisuv_msgs.FaultVec) {
  156. subscribersTimeMutexes[i].Lock()
  157. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  158. subscribersMutexes[i].Lock()
  159. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  160. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  161. faultLabel := ""
  162. if len(masterConfig.RuleOfCameraFault1) > 0 {
  163. for _, f := range masterConfig.RuleOfCameraFault1 {
  164. faultLabel = f(data)
  165. if faultLabel != "" {
  166. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  167. subscribersTimes[i] = time.Now()
  168. goto TriggerSuccess
  169. }
  170. }
  171. }
  172. if len(masterConfig.RuleOfCameraFault2) > 0 {
  173. for _, f := range masterConfig.RuleOfCameraFault2 {
  174. faultLabel = f(data, &pjisuvParam)
  175. if faultLabel != "" {
  176. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  177. subscribersTimes[i] = time.Now()
  178. goto TriggerSuccess
  179. }
  180. }
  181. }
  182. TriggerSuccess:
  183. subscribersMutexes[i].Unlock()
  184. }
  185. subscribersTimeMutexes[i].Unlock()
  186. },
  187. })
  188. }
  189. // 4
  190. if topic == masterConfig.TopicOfCanData && (len(masterConfig.RuleOfCanData1) > 0 || len(masterConfig.RuleOfCanData2) > 0) {
  191. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  192. Node: commonConfig.RosNode,
  193. Topic: topic,
  194. Callback: func(data *pjisuv_msgs.Frame) {
  195. subscribersTimeMutexes[i].Lock()
  196. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  197. subscribersMutexes[i].Lock()
  198. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  199. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  200. faultLabel := ""
  201. if len(masterConfig.RuleOfCanData1) > 0 {
  202. for _, f := range masterConfig.RuleOfCanData1 {
  203. faultLabel = f(data)
  204. if faultLabel != "" {
  205. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  206. subscribersTimes[i] = time.Now()
  207. goto TriggerSuccess
  208. }
  209. }
  210. }
  211. if len(masterConfig.RuleOfCanData2) > 0 {
  212. for _, f := range masterConfig.RuleOfCanData2 {
  213. faultLabel = f(data, &pjisuvParam)
  214. if faultLabel != "" {
  215. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  216. subscribersTimes[i] = time.Now()
  217. goto TriggerSuccess
  218. }
  219. }
  220. }
  221. TriggerSuccess:
  222. subscribersMutexes[i].Unlock()
  223. }
  224. subscribersTimeMutexes[i].Unlock()
  225. },
  226. })
  227. }
  228. // 5
  229. if topic == masterConfig.TopicOfCh128x1LslidarPointCloud && (len(masterConfig.RuleOfCh128x1LslidarPointCloud1) > 0 || len(masterConfig.RuleOfCh128x1LslidarPointCloud2) > 0) {
  230. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  231. Node: commonConfig.RosNode,
  232. Topic: topic,
  233. Callback: func(data *sensor_msgs.PointCloud2) {
  234. subscribersTimeMutexes[i].Lock()
  235. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  236. subscribersMutexes[i].Lock()
  237. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  238. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  239. faultLabel := ""
  240. if len(masterConfig.RuleOfCh128x1LslidarPointCloud1) > 0 {
  241. for _, f := range masterConfig.RuleOfCh128x1LslidarPointCloud1 {
  242. faultLabel = f(data)
  243. if faultLabel != "" {
  244. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  245. subscribersTimes[i] = time.Now()
  246. goto TriggerSuccess
  247. }
  248. }
  249. }
  250. if len(masterConfig.RuleOfCh128x1LslidarPointCloud2) > 0 {
  251. for _, f := range masterConfig.RuleOfCh128x1LslidarPointCloud2 {
  252. faultLabel = f(data, &pjisuvParam)
  253. if faultLabel != "" {
  254. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  255. subscribersTimes[i] = time.Now()
  256. goto TriggerSuccess
  257. }
  258. }
  259. }
  260. TriggerSuccess:
  261. subscribersMutexes[i].Unlock()
  262. }
  263. subscribersTimeMutexes[i].Unlock()
  264. },
  265. })
  266. }
  267. // 6
  268. if topic == masterConfig.TopicOfCh64wLLslidarPointCloud && (len(masterConfig.RuleOfCh64wLLslidarPointCloud1) > 0 || len(masterConfig.RuleOfCh64wLLslidarPointCloud2) > 1) {
  269. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  270. Node: commonConfig.RosNode,
  271. Topic: topic,
  272. Callback: func(data *sensor_msgs.PointCloud2) {
  273. subscribersTimeMutexes[i].Lock()
  274. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  275. subscribersMutexes[i].Lock()
  276. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  277. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  278. faultLabel := ""
  279. if len(masterConfig.RuleOfCh64wLLslidarPointCloud1) > 0 {
  280. for _, f := range masterConfig.RuleOfCh64wLLslidarPointCloud1 {
  281. faultLabel = f(data)
  282. if faultLabel != "" {
  283. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  284. subscribersTimes[i] = time.Now()
  285. goto TriggerSuccess
  286. }
  287. }
  288. }
  289. if len(masterConfig.RuleOfCh64wLLslidarPointCloud2) > 0 {
  290. for _, f := range masterConfig.RuleOfCh64wLLslidarPointCloud2 {
  291. faultLabel = f(data, &pjisuvParam)
  292. if faultLabel != "" {
  293. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  294. subscribersTimes[i] = time.Now()
  295. goto TriggerSuccess
  296. }
  297. }
  298. }
  299. TriggerSuccess:
  300. subscribersMutexes[i].Unlock()
  301. }
  302. subscribersTimeMutexes[i].Unlock()
  303. },
  304. })
  305. }
  306. // 7
  307. if topic == masterConfig.TopicOfCh64wLScan && (len(masterConfig.RuleOfCh64wLScan1) > 0 || len(masterConfig.RuleOfCh64wLScan2) > 0) {
  308. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  309. Node: commonConfig.RosNode,
  310. Topic: topic,
  311. Callback: func(data *sensor_msgs.LaserScan) {
  312. subscribersTimeMutexes[i].Lock()
  313. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  314. subscribersMutexes[i].Lock()
  315. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  316. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  317. faultLabel := ""
  318. if len(masterConfig.RuleOfCh64wLScan1) > 0 {
  319. for _, f := range masterConfig.RuleOfCh64wLScan1 {
  320. faultLabel = f(data)
  321. if faultLabel != "" {
  322. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  323. subscribersTimes[i] = time.Now()
  324. goto TriggerSuccess
  325. }
  326. }
  327. }
  328. if len(masterConfig.RuleOfCh64wLScan2) > 0 {
  329. for _, f := range masterConfig.RuleOfCh64wLScan2 {
  330. faultLabel = f(data, &pjisuvParam)
  331. if faultLabel != "" {
  332. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  333. subscribersTimes[i] = time.Now()
  334. goto TriggerSuccess
  335. }
  336. }
  337. }
  338. TriggerSuccess:
  339. subscribersMutexes[i].Unlock()
  340. }
  341. subscribersTimeMutexes[i].Unlock()
  342. },
  343. })
  344. }
  345. // 8
  346. if topic == masterConfig.TopicOfCh64wRLslidarPointCloud && (len(masterConfig.RuleOfCh64wRLslidarPointCloud1) > 0 || len(masterConfig.RuleOfCh64wRLslidarPointCloud2) > 0) {
  347. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  348. Node: commonConfig.RosNode,
  349. Topic: topic,
  350. Callback: func(data *sensor_msgs.PointCloud2) {
  351. subscribersTimeMutexes[i].Lock()
  352. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  353. subscribersMutexes[i].Lock()
  354. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  355. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  356. faultLabel := ""
  357. if len(masterConfig.RuleOfCh64wRLslidarPointCloud1) > 0 {
  358. for _, f := range masterConfig.RuleOfCh64wRLslidarPointCloud1 {
  359. faultLabel = f(data)
  360. if faultLabel != "" {
  361. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  362. subscribersTimes[i] = time.Now()
  363. goto TriggerSuccess
  364. }
  365. }
  366. }
  367. if len(masterConfig.RuleOfCh64wRLslidarPointCloud2) > 0 {
  368. for _, f := range masterConfig.RuleOfCh64wRLslidarPointCloud2 {
  369. faultLabel = f(data, &pjisuvParam)
  370. if faultLabel != "" {
  371. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  372. subscribersTimes[i] = time.Now()
  373. goto TriggerSuccess
  374. }
  375. }
  376. }
  377. TriggerSuccess:
  378. subscribersMutexes[i].Unlock()
  379. }
  380. subscribersTimeMutexes[i].Unlock()
  381. },
  382. })
  383. }
  384. // 9
  385. if topic == masterConfig.TopicOfCh64wRScan && (len(masterConfig.RuleOfCh64wRScan1) > 0 || len(masterConfig.RuleOfCh64wRScan2) > 0) {
  386. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  387. Node: commonConfig.RosNode,
  388. Topic: topic,
  389. Callback: func(data *sensor_msgs.LaserScan) {
  390. subscribersTimeMutexes[i].Lock()
  391. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  392. subscribersMutexes[i].Lock()
  393. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  394. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  395. faultLabel := ""
  396. if len(masterConfig.RuleOfCh64wRScan1) > 0 {
  397. for _, f := range masterConfig.RuleOfCh64wRScan1 {
  398. faultLabel = f(data)
  399. if faultLabel != "" {
  400. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  401. subscribersTimes[i] = time.Now()
  402. goto TriggerSuccess
  403. }
  404. }
  405. }
  406. if len(masterConfig.RuleOfCh64wRScan2) > 0 {
  407. for _, f := range masterConfig.RuleOfCh64wRScan2 {
  408. faultLabel = f(data, &pjisuvParam)
  409. if faultLabel != "" {
  410. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  411. subscribersTimes[i] = time.Now()
  412. goto TriggerSuccess
  413. }
  414. }
  415. }
  416. TriggerSuccess:
  417. subscribersMutexes[i].Unlock()
  418. }
  419. subscribersTimeMutexes[i].Unlock()
  420. },
  421. })
  422. }
  423. // 10
  424. if topic == masterConfig.TopicOfCicvLidarclusterMovingObjects && (len(masterConfig.RuleOfCicvLidarclusterMovingObjects1) > 0 || len(masterConfig.RuleOfCicvLidarclusterMovingObjects2) > 0) {
  425. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  426. Node: commonConfig.RosNode,
  427. Topic: topic,
  428. Callback: func(data *pjisuv_msgs.PerceptionCicvMovingObjects) {
  429. subscribersTimeMutexes[i].Lock()
  430. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  431. subscribersMutexes[i].Lock()
  432. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  433. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  434. faultLabel := ""
  435. if len(masterConfig.RuleOfCicvLidarclusterMovingObjects1) > 0 {
  436. for _, f := range masterConfig.RuleOfCicvLidarclusterMovingObjects1 {
  437. faultLabel = f(data)
  438. if faultLabel != "" {
  439. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  440. subscribersTimes[i] = time.Now()
  441. goto TriggerSuccess
  442. }
  443. }
  444. }
  445. if len(masterConfig.RuleOfCicvLidarclusterMovingObjects2) > 0 {
  446. for _, f := range masterConfig.RuleOfCicvLidarclusterMovingObjects2 {
  447. faultLabel = f(data, &pjisuvParam)
  448. if faultLabel != "" {
  449. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  450. subscribersTimes[i] = time.Now()
  451. goto TriggerSuccess
  452. }
  453. }
  454. }
  455. TriggerSuccess:
  456. subscribersMutexes[i].Unlock()
  457. }
  458. subscribersTimeMutexes[i].Unlock()
  459. },
  460. })
  461. }
  462. // 11 有共享变量的订阅者必须被创建
  463. if topic == masterConfig.TopicOfCicvAmrTrajectory {
  464. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  465. Node: commonConfig.RosNode,
  466. Topic: topic,
  467. Callback: func(data *pjisuv_msgs.Trajectory) {
  468. subscribersTimeMutexes[i].Lock()
  469. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  470. subscribersMutexes[i].Lock()
  471. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  472. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  473. faultLabel := ""
  474. if len(masterConfig.RuleOfCicvAmrTrajectory1) > 0 {
  475. for _, f := range masterConfig.RuleOfCicvAmrTrajectory1 {
  476. faultLabel = f(data)
  477. if faultLabel != "" {
  478. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  479. subscribersTimes[i] = time.Now()
  480. goto TriggerSuccess
  481. }
  482. }
  483. }
  484. if len(masterConfig.RuleOfCicvAmrTrajectory2) > 0 {
  485. for _, f := range masterConfig.RuleOfCicvAmrTrajectory2 {
  486. faultLabel = f(data, &pjisuvParam)
  487. if faultLabel != "" {
  488. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  489. subscribersTimes[i] = time.Now()
  490. goto TriggerSuccess
  491. }
  492. }
  493. }
  494. TriggerSuccess:
  495. subscribersMutexes[i].Unlock()
  496. }
  497. subscribersTimeMutexes[i].Unlock()
  498. // 触发后更新共享变量
  499. mutexOfCicvAmrTrajectory.RLock()
  500. {
  501. var currentCurvateres []float64
  502. for _, point := range data.Trajectoryinfo.Trajectorypoints {
  503. currentCurvateres = append(currentCurvateres, math.Abs(float64(point.Curvature)))
  504. }
  505. pjisuvParam.LastCurvaturesOfCicvAmrTrajectory = currentCurvateres
  506. }
  507. mutexOfCicvAmrTrajectory.RUnlock()
  508. },
  509. })
  510. }
  511. // 12 有共享变量的订阅者必须被创建
  512. if topic == masterConfig.TopicOfCicvLocation {
  513. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  514. Node: commonConfig.RosNode,
  515. Topic: topic,
  516. Callback: func(data *pjisuv_msgs.PerceptionLocalization) {
  517. // 更新共享变量
  518. mutexOfCicvLocation.RLock()
  519. {
  520. pjisuvParam.VelocityXOfCicvLocation = data.VelocityX
  521. pjisuvParam.VelocityYOfCicvLocation = data.VelocityY
  522. pjisuvParam.VelocityZOfCicvLocation = data.VelocityZ
  523. pjisuvParam.YawOfCicvLocation = data.Yaw
  524. pjisuvParam.AngularVelocityZOfCicvLocation = data.AngularVelocityZ
  525. pjisuvParam.PositionXOfCicvLocation = data.PositionX
  526. pjisuvParam.PositionYOfCicvLocation = data.PositionY
  527. }
  528. mutexOfCicvLocation.RUnlock()
  529. subscribersTimeMutexes[i].Lock()
  530. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  531. subscribersMutexes[i].Lock()
  532. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  533. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  534. faultLabel := ""
  535. if len(masterConfig.RuleOfCicvLocation1) > 0 {
  536. for _, f := range masterConfig.RuleOfCicvLocation1 {
  537. faultLabel = f(data)
  538. if faultLabel != "" {
  539. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  540. subscribersTimes[i] = time.Now()
  541. goto TriggerSuccess
  542. }
  543. }
  544. }
  545. if len(masterConfig.RuleOfCicvLocation2) > 0 {
  546. for _, f := range masterConfig.RuleOfCicvLocation2 {
  547. faultLabel = f(data, &pjisuvParam)
  548. if faultLabel != "" {
  549. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  550. subscribersTimes[i] = time.Now()
  551. goto TriggerSuccess
  552. }
  553. }
  554. }
  555. TriggerSuccess:
  556. subscribersMutexes[i].Unlock()
  557. }
  558. subscribersTimeMutexes[i].Unlock()
  559. },
  560. })
  561. }
  562. // 13
  563. if topic == masterConfig.TopicOfCloudClusters && (len(masterConfig.RuleOfCloudClusters1) > 0 || len(masterConfig.RuleOfCloudClusters2) > 0) {
  564. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  565. Node: commonConfig.RosNode,
  566. Topic: topic,
  567. Callback: func(data *pjisuv_msgs.AutowareCloudClusterArray) {
  568. subscribersTimeMutexes[i].Lock()
  569. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  570. subscribersMutexes[i].Lock()
  571. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  572. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  573. faultLabel := ""
  574. if len(masterConfig.RuleOfCloudClusters1) > 0 {
  575. for _, f := range masterConfig.RuleOfCloudClusters1 {
  576. faultLabel = f(data)
  577. if faultLabel != "" {
  578. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  579. subscribersTimes[i] = time.Now()
  580. goto TriggerSuccess
  581. }
  582. }
  583. }
  584. if len(masterConfig.RuleOfCloudClusters2) > 0 {
  585. for _, f := range masterConfig.RuleOfCloudClusters2 {
  586. faultLabel = f(data, &pjisuvParam)
  587. if faultLabel != "" {
  588. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  589. subscribersTimes[i] = time.Now()
  590. goto TriggerSuccess
  591. }
  592. }
  593. }
  594. TriggerSuccess:
  595. subscribersMutexes[i].Unlock()
  596. }
  597. subscribersTimeMutexes[i].Unlock()
  598. },
  599. })
  600. }
  601. // 14
  602. if topic == masterConfig.TopicOfHeartbeatInfo && (len(masterConfig.RuleOfHeartbeatInfo1) > 0 || len(masterConfig.RuleOfHeartbeatInfo2) > 0) {
  603. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  604. Node: commonConfig.RosNode,
  605. Topic: topic,
  606. Callback: func(data *pjisuv_msgs.HeartBeatInfo) {
  607. subscribersTimeMutexes[i].Lock()
  608. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  609. subscribersMutexes[i].Lock()
  610. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  611. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  612. faultLabel := ""
  613. if len(masterConfig.RuleOfHeartbeatInfo1) > 0 {
  614. for _, f := range masterConfig.RuleOfHeartbeatInfo1 {
  615. faultLabel = f(data)
  616. if faultLabel != "" {
  617. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  618. subscribersTimes[i] = time.Now()
  619. goto TriggerSuccess
  620. }
  621. }
  622. }
  623. if len(masterConfig.RuleOfHeartbeatInfo2) > 0 {
  624. for _, f := range masterConfig.RuleOfHeartbeatInfo2 {
  625. faultLabel = f(data, &pjisuvParam)
  626. if faultLabel != "" {
  627. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  628. subscribersTimes[i] = time.Now()
  629. goto TriggerSuccess
  630. }
  631. }
  632. }
  633. TriggerSuccess:
  634. subscribersMutexes[i].Unlock()
  635. }
  636. subscribersTimeMutexes[i].Unlock()
  637. },
  638. })
  639. }
  640. // 15
  641. if topic == masterConfig.TopicOfLidarPretreatmentCost && (len(masterConfig.RuleOfLidarPretreatmentCost1) > 0 || len(masterConfig.RuleOfLidarPretreatmentCost2) > 0) {
  642. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  643. Node: commonConfig.RosNode,
  644. Topic: topic,
  645. Callback: func(data *geometry_msgs.Vector3Stamped) {
  646. subscribersTimeMutexes[i].Lock()
  647. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  648. subscribersMutexes[i].Lock()
  649. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  650. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  651. faultLabel := ""
  652. if len(masterConfig.RuleOfLidarPretreatmentCost1) > 0 {
  653. for _, f := range masterConfig.RuleOfLidarPretreatmentCost1 {
  654. faultLabel = f(data)
  655. if faultLabel != "" {
  656. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  657. subscribersTimes[i] = time.Now()
  658. goto TriggerSuccess
  659. }
  660. }
  661. }
  662. if len(masterConfig.RuleOfLidarPretreatmentCost2) > 0 {
  663. for _, f := range masterConfig.RuleOfLidarPretreatmentCost2 {
  664. faultLabel = f(data, &pjisuvParam)
  665. if faultLabel != "" {
  666. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  667. subscribersTimes[i] = time.Now()
  668. goto TriggerSuccess
  669. }
  670. }
  671. }
  672. TriggerSuccess:
  673. subscribersMutexes[i].Unlock()
  674. }
  675. subscribersTimeMutexes[i].Unlock()
  676. },
  677. })
  678. }
  679. // 16
  680. if topic == masterConfig.TopicOfLidarPretreatmentOdometry && (len(masterConfig.RuleOfLidarPretreatmentOdometry1) > 0 || len(masterConfig.RuleOfLidarPretreatmentOdometry2) > 0) {
  681. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  682. Node: commonConfig.RosNode,
  683. Topic: topic,
  684. Callback: func(data *nav_msgs.Odometry) {
  685. subscribersTimeMutexes[i].Lock()
  686. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  687. subscribersMutexes[i].Lock()
  688. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  689. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  690. faultLabel := ""
  691. if len(masterConfig.RuleOfLidarPretreatmentOdometry1) > 0 {
  692. for _, f := range masterConfig.RuleOfLidarPretreatmentOdometry1 {
  693. faultLabel = f(data)
  694. if faultLabel != "" {
  695. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  696. subscribersTimes[i] = time.Now()
  697. goto TriggerSuccess
  698. }
  699. }
  700. }
  701. if len(masterConfig.RuleOfLidarPretreatmentOdometry2) > 0 {
  702. for _, f := range masterConfig.RuleOfLidarPretreatmentOdometry2 {
  703. faultLabel = f(data, &pjisuvParam)
  704. if faultLabel != "" {
  705. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  706. subscribersTimes[i] = time.Now()
  707. goto TriggerSuccess
  708. }
  709. }
  710. }
  711. TriggerSuccess:
  712. subscribersMutexes[i].Unlock()
  713. }
  714. subscribersTimeMutexes[i].Unlock()
  715. },
  716. })
  717. }
  718. // 17
  719. if topic == masterConfig.TopicOfLidarRoi && (len(masterConfig.RuleOfLidarRoi1) > 0 || len(masterConfig.RuleOfLidarRoi2) > 0) {
  720. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  721. Node: commonConfig.RosNode,
  722. Topic: topic,
  723. Callback: func(data *geometry_msgs.PolygonStamped) {
  724. subscribersTimeMutexes[i].Lock()
  725. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  726. subscribersMutexes[i].Lock()
  727. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  728. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  729. faultLabel := ""
  730. if len(masterConfig.RuleOfLidarRoi1) > 0 {
  731. for _, f := range masterConfig.RuleOfLidarRoi1 {
  732. faultLabel = f(data)
  733. if faultLabel != "" {
  734. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  735. subscribersTimes[i] = time.Now()
  736. goto TriggerSuccess
  737. }
  738. }
  739. }
  740. if len(masterConfig.RuleOfLidarRoi2) > 0 {
  741. for _, f := range masterConfig.RuleOfLidarRoi2 {
  742. faultLabel = f(data, &pjisuvParam)
  743. if faultLabel != "" {
  744. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  745. subscribersTimes[i] = time.Now()
  746. goto TriggerSuccess
  747. }
  748. }
  749. }
  750. TriggerSuccess:
  751. subscribersMutexes[i].Unlock()
  752. }
  753. subscribersTimeMutexes[i].Unlock()
  754. },
  755. })
  756. }
  757. // 18
  758. if topic == masterConfig.TopicOfLine1 && (len(masterConfig.RuleOfLine11) > 0 || len(masterConfig.RuleOfLine12) > 0) {
  759. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  760. Node: commonConfig.RosNode,
  761. Topic: topic,
  762. Callback: func(data *nav_msgs.Path) {
  763. subscribersTimeMutexes[i].Lock()
  764. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  765. subscribersMutexes[i].Lock()
  766. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  767. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  768. faultLabel := ""
  769. if len(masterConfig.RuleOfLine11) > 0 {
  770. for _, f := range masterConfig.RuleOfLine11 {
  771. faultLabel = f(data)
  772. if faultLabel != "" {
  773. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  774. subscribersTimes[i] = time.Now()
  775. goto TriggerSuccess
  776. }
  777. }
  778. }
  779. if len(masterConfig.RuleOfLine12) > 0 {
  780. for _, f := range masterConfig.RuleOfLine12 {
  781. faultLabel = f(data, &pjisuvParam)
  782. if faultLabel != "" {
  783. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  784. subscribersTimes[i] = time.Now()
  785. goto TriggerSuccess
  786. }
  787. }
  788. }
  789. TriggerSuccess:
  790. subscribersMutexes[i].Unlock()
  791. }
  792. subscribersTimeMutexes[i].Unlock()
  793. },
  794. })
  795. }
  796. // 19
  797. if topic == masterConfig.TopicOfLine2 && (len(masterConfig.RuleOfLine21) > 0 || len(masterConfig.RuleOfLine22) > 0) {
  798. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  799. Node: commonConfig.RosNode,
  800. Topic: topic,
  801. Callback: func(data *nav_msgs.Path) {
  802. subscribersTimeMutexes[i].Lock()
  803. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  804. subscribersMutexes[i].Lock()
  805. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  806. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  807. faultLabel := ""
  808. if len(masterConfig.RuleOfLine21) > 0 {
  809. for _, f := range masterConfig.RuleOfLine21 {
  810. faultLabel = f(data)
  811. if faultLabel != "" {
  812. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  813. subscribersTimes[i] = time.Now()
  814. goto TriggerSuccess
  815. }
  816. }
  817. }
  818. if len(masterConfig.RuleOfLine22) > 0 {
  819. for _, f := range masterConfig.RuleOfLine22 {
  820. faultLabel = f(data, &pjisuvParam)
  821. if faultLabel != "" {
  822. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  823. subscribersTimes[i] = time.Now()
  824. goto TriggerSuccess
  825. }
  826. }
  827. }
  828. TriggerSuccess:
  829. subscribersMutexes[i].Unlock()
  830. }
  831. subscribersTimeMutexes[i].Unlock()
  832. },
  833. })
  834. }
  835. // 20
  836. if topic == masterConfig.TopicOfMapPolygon && (len(masterConfig.RuleOfMapPolygon1) > 0 || len(masterConfig.RuleOfMapPolygon2) > 0) {
  837. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  838. Node: commonConfig.RosNode,
  839. Topic: topic,
  840. Callback: func(data *pjisuv_msgs.PolygonStamped) {
  841. subscribersTimeMutexes[i].Lock()
  842. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  843. subscribersMutexes[i].Lock()
  844. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  845. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  846. faultLabel := ""
  847. if len(masterConfig.RuleOfMapPolygon1) > 0 {
  848. for _, f := range masterConfig.RuleOfMapPolygon1 {
  849. faultLabel = f(data)
  850. if faultLabel != "" {
  851. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  852. subscribersTimes[i] = time.Now()
  853. goto TriggerSuccess
  854. }
  855. }
  856. }
  857. if len(masterConfig.RuleOfMapPolygon2) > 0 {
  858. for _, f := range masterConfig.RuleOfMapPolygon2 {
  859. faultLabel = f(data, &pjisuvParam)
  860. if faultLabel != "" {
  861. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  862. subscribersTimes[i] = time.Now()
  863. goto TriggerSuccess
  864. }
  865. }
  866. }
  867. TriggerSuccess:
  868. subscribersMutexes[i].Unlock()
  869. }
  870. subscribersTimeMutexes[i].Unlock()
  871. },
  872. })
  873. }
  874. // 21
  875. if topic == masterConfig.TopicOfObstacleDisplay && (len(masterConfig.RuleOfObstacleDisplay1) > 0 || len(masterConfig.RuleOfObstacleDisplay2) > 0) {
  876. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  877. Node: commonConfig.RosNode,
  878. Topic: topic,
  879. Callback: func(data *visualization_msgs.MarkerArray) {
  880. subscribersTimeMutexes[i].Lock()
  881. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  882. subscribersMutexes[i].Lock()
  883. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  884. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  885. faultLabel := ""
  886. if len(masterConfig.RuleOfObstacleDisplay1) > 0 {
  887. for _, f := range masterConfig.RuleOfObstacleDisplay1 {
  888. faultLabel = f(data)
  889. if faultLabel != "" {
  890. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  891. subscribersTimes[i] = time.Now()
  892. goto TriggerSuccess
  893. }
  894. }
  895. }
  896. if len(masterConfig.RuleOfObstacleDisplay2) > 0 {
  897. for _, f := range masterConfig.RuleOfObstacleDisplay2 {
  898. faultLabel = f(data, &pjisuvParam)
  899. if faultLabel != "" {
  900. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  901. subscribersTimes[i] = time.Now()
  902. goto TriggerSuccess
  903. }
  904. }
  905. }
  906. TriggerSuccess:
  907. subscribersMutexes[i].Unlock()
  908. }
  909. subscribersTimeMutexes[i].Unlock()
  910. },
  911. })
  912. }
  913. // 22 有共享变量的订阅者必须被创建
  914. if topic == masterConfig.TopicOfPjControlPub {
  915. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  916. Node: commonConfig.RosNode,
  917. Topic: topic,
  918. Callback: func(data *pjisuv_msgs.CommonVehicleCmd) {
  919. // 更新共享变量
  920. mutexOfPjControlPub.RLock()
  921. {
  922. pjisuvParam.NumCountPjiControlCommandOfPjControlPub++
  923. if pjisuvParam.NumCountPjiControlCommandOfPjControlPub == 10 {
  924. pjisuvParam.EgoSteeringCmdOfPjControlPub = append(pjisuvParam.EgoSteeringCmdOfPjControlPub, data.ICPVCmdStrAngle)
  925. pjisuvParam.EgoThrottleCmdOfPjControlPub = append(pjisuvParam.EgoThrottleCmdOfPjControlPub, data.ICPVCmdAccPelPosAct)
  926. pjisuvParam.NumCountPjiControlCommandOfPjControlPub = 0
  927. }
  928. }
  929. mutexOfPjControlPub.RUnlock()
  930. subscribersTimeMutexes[i].Lock()
  931. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  932. subscribersMutexes[i].Lock()
  933. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  934. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  935. faultLabel := ""
  936. if len(masterConfig.RuleOfPjControlPub1) > 0 {
  937. for _, f := range masterConfig.RuleOfPjControlPub1 {
  938. faultLabel = f(data)
  939. if faultLabel != "" {
  940. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  941. subscribersTimes[i] = time.Now()
  942. goto TriggerSuccess
  943. }
  944. }
  945. }
  946. if len(masterConfig.RuleOfPjControlPub2) > 0 {
  947. for _, f := range masterConfig.RuleOfPjControlPub2 {
  948. faultLabel = f(data, &pjisuvParam)
  949. if faultLabel != "" {
  950. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  951. subscribersTimes[i] = time.Now()
  952. goto TriggerSuccess
  953. }
  954. }
  955. }
  956. TriggerSuccess:
  957. subscribersMutexes[i].Unlock()
  958. }
  959. subscribersTimeMutexes[i].Unlock()
  960. },
  961. })
  962. }
  963. // 23
  964. if topic == masterConfig.TopicOfPointsCluster && (len(masterConfig.RuleOfPointsCluster1) > 0 || len(masterConfig.RuleOfPointsCluster2) > 0) {
  965. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  966. Node: commonConfig.RosNode,
  967. Topic: topic,
  968. Callback: func(data *sensor_msgs.PointCloud2) {
  969. subscribersTimeMutexes[i].Lock()
  970. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  971. subscribersMutexes[i].Lock()
  972. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  973. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  974. faultLabel := ""
  975. if len(masterConfig.RuleOfPointsCluster1) > 0 {
  976. for _, f := range masterConfig.RuleOfPointsCluster1 {
  977. faultLabel = f(data)
  978. if faultLabel != "" {
  979. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  980. subscribersTimes[i] = time.Now()
  981. goto TriggerSuccess
  982. }
  983. }
  984. }
  985. if len(masterConfig.RuleOfPointsCluster2) > 0 {
  986. for _, f := range masterConfig.RuleOfPointsCluster2 {
  987. faultLabel = f(data, &pjisuvParam)
  988. if faultLabel != "" {
  989. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  990. subscribersTimes[i] = time.Now()
  991. goto TriggerSuccess
  992. }
  993. }
  994. }
  995. TriggerSuccess:
  996. subscribersMutexes[i].Unlock()
  997. }
  998. subscribersTimeMutexes[i].Unlock()
  999. },
  1000. })
  1001. }
  1002. // 24
  1003. if topic == masterConfig.TopicOfPointsConcat && (len(masterConfig.RuleOfPointsConcat1) > 0 || len(masterConfig.RuleOfPointsConcat2) > 0) {
  1004. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  1005. Node: commonConfig.RosNode,
  1006. Topic: topic,
  1007. Callback: func(data *sensor_msgs.PointCloud2) {
  1008. subscribersTimeMutexes[i].Lock()
  1009. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  1010. subscribersMutexes[i].Lock()
  1011. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  1012. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  1013. faultLabel := ""
  1014. if len(masterConfig.RuleOfPointsConcat1) > 0 {
  1015. for _, f := range masterConfig.RuleOfPointsConcat1 {
  1016. faultLabel = f(data)
  1017. if faultLabel != "" {
  1018. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1019. subscribersTimes[i] = time.Now()
  1020. goto TriggerSuccess
  1021. }
  1022. }
  1023. }
  1024. if len(masterConfig.RuleOfPointsConcat2) > 0 {
  1025. for _, f := range masterConfig.RuleOfPointsConcat2 {
  1026. faultLabel = f(data, &pjisuvParam)
  1027. if faultLabel != "" {
  1028. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1029. subscribersTimes[i] = time.Now()
  1030. goto TriggerSuccess
  1031. }
  1032. }
  1033. }
  1034. TriggerSuccess:
  1035. subscribersMutexes[i].Unlock()
  1036. }
  1037. subscribersTimeMutexes[i].Unlock()
  1038. },
  1039. })
  1040. }
  1041. // 25
  1042. if topic == masterConfig.TopicOfReferenceDisplay && (len(masterConfig.RuleOfReferenceDisplay1) > 0 || len(masterConfig.RuleOfReferenceDisplay2) > 0) {
  1043. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  1044. Node: commonConfig.RosNode,
  1045. Topic: topic,
  1046. Callback: func(data *nav_msgs.Path) {
  1047. subscribersTimeMutexes[i].Lock()
  1048. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  1049. subscribersMutexes[i].Lock()
  1050. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  1051. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  1052. faultLabel := ""
  1053. if len(masterConfig.RuleOfReferenceDisplay1) > 0 {
  1054. for _, f := range masterConfig.RuleOfReferenceDisplay1 {
  1055. faultLabel = f(data)
  1056. if faultLabel != "" {
  1057. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1058. subscribersTimes[i] = time.Now()
  1059. goto TriggerSuccess
  1060. }
  1061. }
  1062. }
  1063. if len(masterConfig.RuleOfReferenceDisplay2) > 0 {
  1064. for _, f := range masterConfig.RuleOfReferenceDisplay2 {
  1065. faultLabel = f(data, &pjisuvParam)
  1066. if faultLabel != "" {
  1067. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1068. subscribersTimes[i] = time.Now()
  1069. goto TriggerSuccess
  1070. }
  1071. }
  1072. }
  1073. TriggerSuccess:
  1074. subscribersMutexes[i].Unlock()
  1075. }
  1076. subscribersTimeMutexes[i].Unlock()
  1077. },
  1078. })
  1079. }
  1080. // 26
  1081. if topic == masterConfig.TopicOfReferenceTrajectory && (len(masterConfig.RuleOfReferenceTrajectory1) > 0 || len(masterConfig.RuleOfReferenceTrajectory2) > 0) {
  1082. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  1083. Node: commonConfig.RosNode,
  1084. Topic: topic,
  1085. Callback: func(data *pjisuv_msgs.Trajectory) {
  1086. subscribersTimeMutexes[i].Lock()
  1087. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  1088. subscribersMutexes[i].Lock()
  1089. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  1090. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  1091. faultLabel := ""
  1092. if len(masterConfig.RuleOfReferenceTrajectory1) > 0 {
  1093. for _, f := range masterConfig.RuleOfReferenceTrajectory1 {
  1094. faultLabel = f(data)
  1095. if faultLabel != "" {
  1096. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1097. subscribersTimes[i] = time.Now()
  1098. goto TriggerSuccess
  1099. }
  1100. }
  1101. }
  1102. if len(masterConfig.RuleOfReferenceTrajectory2) > 0 {
  1103. for _, f := range masterConfig.RuleOfReferenceTrajectory2 {
  1104. faultLabel = f(data, &pjisuvParam)
  1105. if faultLabel != "" {
  1106. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1107. subscribersTimes[i] = time.Now()
  1108. goto TriggerSuccess
  1109. }
  1110. }
  1111. }
  1112. TriggerSuccess:
  1113. subscribersMutexes[i].Unlock()
  1114. }
  1115. subscribersTimeMutexes[i].Unlock()
  1116. },
  1117. })
  1118. }
  1119. // 27
  1120. if topic == masterConfig.TopicOfRoiPoints && (len(masterConfig.RuleOfRoiPoints1) > 0 || len(masterConfig.RuleOfRoiPoints2) > 0) {
  1121. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  1122. Node: commonConfig.RosNode,
  1123. Topic: topic,
  1124. Callback: func(data *sensor_msgs.PointCloud2) {
  1125. subscribersTimeMutexes[i].Lock()
  1126. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  1127. subscribersMutexes[i].Lock()
  1128. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  1129. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  1130. faultLabel := ""
  1131. if len(masterConfig.RuleOfRoiPoints1) > 0 {
  1132. for _, f := range masterConfig.RuleOfRoiPoints1 {
  1133. faultLabel = f(data)
  1134. if faultLabel != "" {
  1135. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1136. subscribersTimes[i] = time.Now()
  1137. goto TriggerSuccess
  1138. }
  1139. }
  1140. }
  1141. if len(masterConfig.RuleOfRoiPoints2) > 0 {
  1142. for _, f := range masterConfig.RuleOfRoiPoints2 {
  1143. faultLabel = f(data, &pjisuvParam)
  1144. if faultLabel != "" {
  1145. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1146. subscribersTimes[i] = time.Now()
  1147. goto TriggerSuccess
  1148. }
  1149. }
  1150. }
  1151. TriggerSuccess:
  1152. subscribersMutexes[i].Unlock()
  1153. }
  1154. subscribersTimeMutexes[i].Unlock()
  1155. },
  1156. })
  1157. }
  1158. // 28
  1159. if topic == masterConfig.TopicOfRoiPolygon && (len(masterConfig.RuleOfRoiPolygon1) > 0 || len(masterConfig.RuleOfRoiPolygon2) > 0) {
  1160. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  1161. Node: commonConfig.RosNode,
  1162. Topic: topic,
  1163. Callback: func(data *nav_msgs.Path) {
  1164. subscribersTimeMutexes[i].Lock()
  1165. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  1166. subscribersMutexes[i].Lock()
  1167. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  1168. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  1169. faultLabel := ""
  1170. if len(masterConfig.RuleOfRoiPolygon1) > 0 {
  1171. for _, f := range masterConfig.RuleOfRoiPolygon1 {
  1172. faultLabel = f(data)
  1173. if faultLabel != "" {
  1174. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1175. subscribersTimes[i] = time.Now()
  1176. goto TriggerSuccess
  1177. }
  1178. }
  1179. }
  1180. if len(masterConfig.RuleOfRoiPolygon2) > 0 {
  1181. for _, f := range masterConfig.RuleOfRoiPolygon2 {
  1182. faultLabel = f(data, &pjisuvParam)
  1183. if faultLabel != "" {
  1184. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1185. subscribersTimes[i] = time.Now()
  1186. goto TriggerSuccess
  1187. }
  1188. }
  1189. }
  1190. TriggerSuccess:
  1191. subscribersMutexes[i].Unlock()
  1192. }
  1193. subscribersTimeMutexes[i].Unlock()
  1194. },
  1195. })
  1196. }
  1197. // 29
  1198. if topic == masterConfig.TopicOfTf && (len(masterConfig.RuleOfTf1) > 0 || len(masterConfig.RuleOfTf2) > 0) {
  1199. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  1200. Node: commonConfig.RosNode,
  1201. Topic: topic,
  1202. Callback: func(data *tf2_msgs.TFMessage) {
  1203. subscribersTimeMutexes[i].Lock()
  1204. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  1205. subscribersMutexes[i].Lock()
  1206. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  1207. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  1208. faultLabel := ""
  1209. if len(masterConfig.RuleOfTf1) > 0 {
  1210. for _, f := range masterConfig.RuleOfTf1 {
  1211. faultLabel = f(data)
  1212. if faultLabel != "" {
  1213. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1214. subscribersTimes[i] = time.Now()
  1215. goto TriggerSuccess
  1216. }
  1217. }
  1218. }
  1219. if len(masterConfig.RuleOfTf2) > 0 {
  1220. for _, f := range masterConfig.RuleOfTf2 {
  1221. faultLabel = f(data, &pjisuvParam)
  1222. if faultLabel != "" {
  1223. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1224. subscribersTimes[i] = time.Now()
  1225. goto TriggerSuccess
  1226. }
  1227. }
  1228. }
  1229. TriggerSuccess:
  1230. subscribersMutexes[i].Unlock()
  1231. }
  1232. subscribersTimeMutexes[i].Unlock()
  1233. },
  1234. })
  1235. }
  1236. // 30 有共享变量的订阅者必须被创建
  1237. if topic == masterConfig.TopicOfTpperception {
  1238. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  1239. Node: commonConfig.RosNode,
  1240. Topic: topic,
  1241. Callback: func(data *pjisuv_msgs.PerceptionObjects) {
  1242. // 更新共享变量
  1243. mutexOfTpperception.RLock()
  1244. {
  1245. for _, obj := range data.Objs {
  1246. if obj.X <= 5 || math.Abs(float64(obj.Y)) >= 10 {
  1247. continue
  1248. }
  1249. // 检查 ObjDicOfTpperception 是否为 nil,如果是,则初始化它
  1250. if pjisuvParam.ObjDicOfTpperception == nil {
  1251. pjisuvParam.ObjDicOfTpperception = make(map[uint32][]float32)
  1252. }
  1253. if _, ok := pjisuvParam.ObjDicOfTpperception[obj.Id]; !ok {
  1254. pjisuvParam.ObjDicOfTpperception[obj.Id] = []float32{}
  1255. }
  1256. pjisuvParam.ObjDicOfTpperception[obj.Id] = append(pjisuvParam.ObjDicOfTpperception[obj.Id], obj.Y)
  1257. }
  1258. }
  1259. mutexOfTpperception.RUnlock()
  1260. subscribersTimeMutexes[i].Lock()
  1261. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  1262. subscribersMutexes[i].Lock()
  1263. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  1264. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  1265. faultLabel := ""
  1266. if len(masterConfig.RuleOfTpperception1) > 0 {
  1267. for _, f := range masterConfig.RuleOfTpperception1 {
  1268. faultLabel = f(data)
  1269. if faultLabel != "" {
  1270. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1271. subscribersTimes[i] = time.Now()
  1272. goto TriggerSuccess
  1273. }
  1274. }
  1275. }
  1276. if len(masterConfig.RuleOfTpperception2) > 0 {
  1277. for _, f := range masterConfig.RuleOfTpperception2 {
  1278. faultLabel = f(data, &pjisuvParam)
  1279. if faultLabel != "" {
  1280. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1281. subscribersTimes[i] = time.Now()
  1282. goto TriggerSuccess
  1283. }
  1284. }
  1285. }
  1286. TriggerSuccess:
  1287. subscribersMutexes[i].Unlock()
  1288. }
  1289. subscribersTimeMutexes[i].Unlock()
  1290. // -------- 触发后更新共享变量
  1291. mutexOfTpperception.RLock()
  1292. {
  1293. for _, obj := range data.Objs {
  1294. pjisuvParam.ObjTypeDicOfTpperception[obj.Id] = obj.Type
  1295. pjisuvParam.ObjSpeedDicOfTpperception[obj.Id] = math.Pow(math.Pow(float64(obj.Vxabs), 2)+math.Pow(float64(obj.Vyabs), 2), 0.5)
  1296. }
  1297. }
  1298. mutexOfTpperception.RUnlock()
  1299. },
  1300. })
  1301. }
  1302. // 31
  1303. if topic == masterConfig.TopicOfTpperceptionVis && (len(masterConfig.RuleOfTpperceptionVis1) > 0 || len(masterConfig.RuleOfTpperceptionVis2) > 0) {
  1304. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  1305. Node: commonConfig.RosNode,
  1306. Topic: topic,
  1307. Callback: func(data *visualization_msgs.MarkerArray) {
  1308. subscribersTimeMutexes[i].Lock()
  1309. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  1310. subscribersMutexes[i].Lock()
  1311. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  1312. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  1313. faultLabel := ""
  1314. if len(masterConfig.RuleOfTpperceptionVis1) > 0 {
  1315. for _, f := range masterConfig.RuleOfTpperceptionVis1 {
  1316. faultLabel = f(data)
  1317. if faultLabel != "" {
  1318. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1319. subscribersTimes[i] = time.Now()
  1320. goto TriggerSuccess
  1321. }
  1322. }
  1323. }
  1324. if len(masterConfig.RuleOfTpperceptionVis2) > 0 {
  1325. for _, f := range masterConfig.RuleOfTpperceptionVis2 {
  1326. faultLabel = f(data, &pjisuvParam)
  1327. if faultLabel != "" {
  1328. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1329. subscribersTimes[i] = time.Now()
  1330. goto TriggerSuccess
  1331. }
  1332. }
  1333. }
  1334. TriggerSuccess:
  1335. subscribersMutexes[i].Unlock()
  1336. }
  1337. subscribersTimeMutexes[i].Unlock()
  1338. },
  1339. })
  1340. }
  1341. // 32
  1342. if topic == masterConfig.TopicOfTprouteplan && (len(masterConfig.RuleOfTprouteplan1) > 0 || len(masterConfig.RuleOfTprouteplan2) > 0) {
  1343. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  1344. Node: commonConfig.RosNode,
  1345. Topic: topic,
  1346. Callback: func(data *pjisuv_msgs.RoutePlan) {
  1347. subscribersTimeMutexes[i].Lock()
  1348. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  1349. subscribersMutexes[i].Lock()
  1350. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  1351. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  1352. faultLabel := ""
  1353. if len(masterConfig.RuleOfTprouteplan1) > 0 {
  1354. for _, f := range masterConfig.RuleOfTprouteplan1 {
  1355. faultLabel = f(data)
  1356. if faultLabel != "" {
  1357. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1358. subscribersTimes[i] = time.Now()
  1359. goto TriggerSuccess
  1360. }
  1361. }
  1362. }
  1363. if len(masterConfig.RuleOfTprouteplan2) > 0 {
  1364. for _, f := range masterConfig.RuleOfTprouteplan2 {
  1365. faultLabel = f(data, &pjisuvParam)
  1366. if faultLabel != "" {
  1367. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1368. subscribersTimes[i] = time.Now()
  1369. goto TriggerSuccess
  1370. }
  1371. }
  1372. }
  1373. TriggerSuccess:
  1374. subscribersMutexes[i].Unlock()
  1375. }
  1376. subscribersTimeMutexes[i].Unlock()
  1377. },
  1378. })
  1379. }
  1380. // 33
  1381. if topic == masterConfig.TopicOfTrajectoryDisplay && (len(masterConfig.RuleOfTrajectoryDisplay1) > 0 || len(masterConfig.RuleOfTrajectoryDisplay2) > 0) {
  1382. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  1383. Node: commonConfig.RosNode,
  1384. Topic: topic,
  1385. Callback: func(data *nav_msgs.Path) {
  1386. subscribersTimeMutexes[i].Lock()
  1387. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  1388. subscribersMutexes[i].Lock()
  1389. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  1390. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  1391. faultLabel := ""
  1392. if len(masterConfig.RuleOfTrajectoryDisplay1) > 0 {
  1393. for _, f := range masterConfig.RuleOfTrajectoryDisplay1 {
  1394. faultLabel = f(data)
  1395. if faultLabel != "" {
  1396. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1397. subscribersTimes[i] = time.Now()
  1398. goto TriggerSuccess
  1399. }
  1400. }
  1401. }
  1402. if len(masterConfig.RuleOfTrajectoryDisplay2) > 0 {
  1403. for _, f := range masterConfig.RuleOfTrajectoryDisplay2 {
  1404. faultLabel = f(data, &pjisuvParam)
  1405. if faultLabel != "" {
  1406. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1407. subscribersTimes[i] = time.Now()
  1408. goto TriggerSuccess
  1409. }
  1410. }
  1411. }
  1412. TriggerSuccess:
  1413. subscribersMutexes[i].Unlock()
  1414. }
  1415. subscribersTimeMutexes[i].Unlock()
  1416. },
  1417. })
  1418. }
  1419. // 34
  1420. if topic == masterConfig.TopicOfUngroundCloudpoints && (len(masterConfig.RuleOfUngroundCloudpoints1) > 0 || len(masterConfig.RuleOfUngroundCloudpoints2) > 0) {
  1421. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  1422. Node: commonConfig.RosNode,
  1423. Topic: topic,
  1424. Callback: func(data *sensor_msgs.PointCloud2) {
  1425. subscribersTimeMutexes[i].Lock()
  1426. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  1427. subscribersMutexes[i].Lock()
  1428. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  1429. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  1430. faultLabel := ""
  1431. if len(masterConfig.RuleOfUngroundCloudpoints1) > 0 {
  1432. for _, f := range masterConfig.RuleOfUngroundCloudpoints1 {
  1433. faultLabel = f(data)
  1434. if faultLabel != "" {
  1435. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1436. subscribersTimes[i] = time.Now()
  1437. goto TriggerSuccess
  1438. }
  1439. }
  1440. }
  1441. if len(masterConfig.RuleOfUngroundCloudpoints2) > 0 {
  1442. for _, f := range masterConfig.RuleOfUngroundCloudpoints2 {
  1443. faultLabel = f(data, &pjisuvParam)
  1444. if faultLabel != "" {
  1445. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1446. subscribersTimes[i] = time.Now()
  1447. goto TriggerSuccess
  1448. }
  1449. }
  1450. }
  1451. TriggerSuccess:
  1452. subscribersMutexes[i].Unlock()
  1453. }
  1454. subscribersTimeMutexes[i].Unlock()
  1455. },
  1456. })
  1457. }
  1458. // 35
  1459. if topic == masterConfig.TopicOfCameraImage && (len(masterConfig.RuleOfCameraImage1) > 0 || len(masterConfig.RuleOfCameraImage2) > 0) {
  1460. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  1461. Node: commonConfig.RosNode,
  1462. Topic: topic,
  1463. Callback: func(data *sensor_msgs.Image) {
  1464. subscribersTimeMutexes[i].Lock()
  1465. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  1466. subscribersMutexes[i].Lock()
  1467. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  1468. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  1469. faultLabel := ""
  1470. if len(masterConfig.RuleOfCameraImage1) > 0 {
  1471. for _, f := range masterConfig.RuleOfCameraImage1 {
  1472. faultLabel = f(data)
  1473. if faultLabel != "" {
  1474. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1475. subscribersTimes[i] = time.Now()
  1476. goto TriggerSuccess
  1477. }
  1478. }
  1479. }
  1480. if len(masterConfig.RuleOfCameraImage2) > 0 {
  1481. for _, f := range masterConfig.RuleOfCameraImage2 {
  1482. faultLabel = f(data, &pjisuvParam)
  1483. if faultLabel != "" {
  1484. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1485. subscribersTimes[i] = time.Now()
  1486. goto TriggerSuccess
  1487. }
  1488. }
  1489. }
  1490. TriggerSuccess:
  1491. subscribersMutexes[i].Unlock()
  1492. }
  1493. subscribersTimeMutexes[i].Unlock()
  1494. },
  1495. })
  1496. }
  1497. // 36 有共享变量的订阅者必须被创建
  1498. if topic == masterConfig.TopicOfDataRead {
  1499. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  1500. Node: commonConfig.RosNode,
  1501. Topic: topic,
  1502. Callback: func(data *pjisuv_msgs.Retrieval) {
  1503. // 更新共享变量
  1504. mutexOfDataRead.RLock()
  1505. {
  1506. pjisuvParam.NumCountDataReadOfDataRead++
  1507. if pjisuvParam.NumCountDataReadOfDataRead == 10 {
  1508. pjisuvParam.EgoSteeringRealOfDataRead = append(pjisuvParam.EgoSteeringRealOfDataRead, data.ActStrWhAng)
  1509. pjisuvParam.EgoThrottleRealOfDataRead = append(pjisuvParam.EgoThrottleRealOfDataRead, data.AccPed2)
  1510. pjisuvParam.NumCountDataReadOfDataRead = 0
  1511. }
  1512. pjisuvParam.StrgAngleRealValueOfDataRead = data.ActStrWhAng
  1513. }
  1514. mutexOfDataRead.RUnlock()
  1515. subscribersTimeMutexes[i].Lock()
  1516. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  1517. subscribersMutexes[i].Lock()
  1518. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  1519. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  1520. faultLabel := ""
  1521. if len(masterConfig.RuleOfDataRead1) > 0 {
  1522. for _, f := range masterConfig.RuleOfDataRead1 {
  1523. faultLabel = f(data)
  1524. if faultLabel != "" {
  1525. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1526. subscribersTimes[i] = time.Now()
  1527. goto TriggerSuccess
  1528. }
  1529. }
  1530. }
  1531. if len(masterConfig.RuleOfDataRead2) > 0 {
  1532. for _, f := range masterConfig.RuleOfDataRead2 {
  1533. faultLabel = f(data, &pjisuvParam)
  1534. if faultLabel != "" {
  1535. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1536. subscribersTimes[i] = time.Now()
  1537. goto TriggerSuccess
  1538. }
  1539. }
  1540. }
  1541. TriggerSuccess:
  1542. subscribersMutexes[i].Unlock()
  1543. }
  1544. subscribersTimeMutexes[i].Unlock()
  1545. },
  1546. })
  1547. }
  1548. // 37
  1549. if topic == masterConfig.TopicOfPjiGps && (len(masterConfig.RuleOfPjiGps1) > 0 || len(masterConfig.RuleOfPjiGps2) > 0) {
  1550. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  1551. Node: commonConfig.RosNode,
  1552. Topic: topic,
  1553. Callback: func(data *pjisuv_msgs.PerceptionLocalization) {
  1554. subscribersTimeMutexes[i].Lock()
  1555. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  1556. subscribersMutexes[i].Lock()
  1557. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  1558. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  1559. faultLabel := ""
  1560. if len(masterConfig.RuleOfPjiGps1) > 0 {
  1561. for _, f := range masterConfig.RuleOfPjiGps1 {
  1562. faultLabel = f(data)
  1563. if faultLabel != "" {
  1564. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1565. subscribersTimes[i] = time.Now()
  1566. goto TriggerSuccess
  1567. }
  1568. }
  1569. }
  1570. if len(masterConfig.RuleOfPjiGps2) > 0 {
  1571. for _, f := range masterConfig.RuleOfPjiGps2 {
  1572. faultLabel = f(data, &pjisuvParam)
  1573. if faultLabel != "" {
  1574. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1575. subscribersTimes[i] = time.Now()
  1576. goto TriggerSuccess
  1577. }
  1578. }
  1579. }
  1580. TriggerSuccess:
  1581. subscribersMutexes[i].Unlock()
  1582. }
  1583. subscribersTimeMutexes[i].Unlock()
  1584. },
  1585. })
  1586. }
  1587. // 39
  1588. if topic == masterConfig.TopicOfPjVehicleFdbPub && (len(masterConfig.RuleOfPjVehicleFdbPub1) > 0 || len(masterConfig.RuleOfPjVehicleFdbPub2) > 0) {
  1589. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  1590. Node: commonConfig.RosNode,
  1591. Topic: topic,
  1592. Callback: func(data *pjisuv_msgs.VehicleFdb) {
  1593. subscribersTimeMutexes[i].Lock()
  1594. if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
  1595. subscribersMutexes[i].Lock()
  1596. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  1597. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  1598. faultLabel := ""
  1599. if len(masterConfig.RuleOfPjVehicleFdbPub1) > 0 {
  1600. for _, f := range masterConfig.RuleOfPjVehicleFdbPub1 {
  1601. faultLabel = f(data)
  1602. if faultLabel != "" {
  1603. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1604. subscribersTimes[i] = time.Now()
  1605. goto TriggerSuccess
  1606. }
  1607. }
  1608. }
  1609. if len(masterConfig.RuleOfPjVehicleFdbPub2) > 0 {
  1610. for _, f := range masterConfig.RuleOfPjVehicleFdbPub2 {
  1611. faultLabel = f(data, &pjisuvParam)
  1612. if faultLabel != "" {
  1613. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1614. subscribersTimes[i] = time.Now()
  1615. goto TriggerSuccess
  1616. }
  1617. }
  1618. }
  1619. TriggerSuccess:
  1620. subscribersMutexes[i].Unlock()
  1621. }
  1622. subscribersTimeMutexes[i].Unlock()
  1623. },
  1624. })
  1625. }
  1626. if err != nil {
  1627. c_log.GlobalLogger.Info("创建订阅者报错,可能由于节点未启动,再次尝试:", err)
  1628. time.Sleep(time.Duration(2) * time.Second)
  1629. continue
  1630. } else {
  1631. break
  1632. }
  1633. }
  1634. }
  1635. select {
  1636. case signal := <-service.ChannelKillWindowProducer:
  1637. if signal == 1 {
  1638. commonConfig.RosNode.Close()
  1639. service.AddKillTimes("3")
  1640. return
  1641. }
  1642. }
  1643. }
  1644. func saveTimeWindow(faultLabel string, faultHappenTime string, lastTimeWindow *commonEntity.TimeWindow) {
  1645. saveTimeWindowMutex.Lock()
  1646. defer saveTimeWindowMutex.Unlock()
  1647. masterTopics, slaveTopics := getTopicsOfNode(faultLabel)
  1648. if lastTimeWindow == nil || util.TimeCustom1GreaterTimeCustom2(faultHappenTime, lastTimeWindow.TimeWindowEnd) { // 如果是不在旧故障窗口内,添加一个新窗口
  1649. exceptBegin := util.TimeCustomChange(faultHappenTime, -commonConfig.PlatformConfig.TaskBeforeTime)
  1650. finalTimeWindowBegin := ""
  1651. if util.TimeCustom1LessEqualThanTimeCustom2(exceptBegin, latestTimeWindowEnd) { // 窗口最早时间不能早于上一个窗口结束时间
  1652. finalTimeWindowBegin = latestTimeWindowEnd
  1653. } else {
  1654. finalTimeWindowBegin = exceptBegin
  1655. }
  1656. latestTimeWindowEnd = util.TimeCustomChange(faultHappenTime, commonConfig.PlatformConfig.TaskAfterTime)
  1657. newTimeWindow := commonEntity.TimeWindow{
  1658. FaultTime: faultHappenTime,
  1659. TimeWindowBegin: finalTimeWindowBegin,
  1660. TimeWindowEnd: latestTimeWindowEnd,
  1661. Length: util.CalculateDifferenceOfTimeCustom(finalTimeWindowBegin, latestTimeWindowEnd),
  1662. Labels: []string{faultLabel},
  1663. MasterTopics: masterTopics,
  1664. SlaveTopics: slaveTopics,
  1665. }
  1666. c_log.GlobalLogger.Infof("不在旧故障窗口内,向生产者队列添加一个新窗口,【Lable】=%v,【FaultTime】=%v,【Length】=%v", newTimeWindow.Labels, newTimeWindow.FaultTime, newTimeWindow.Length)
  1667. commonEntity.AddTimeWindowToTimeWindowProducerQueue(newTimeWindow)
  1668. } else { // 如果在旧故障窗口内
  1669. commonEntity.TimeWindowProducerQueueMutex.RLock()
  1670. defer commonEntity.TimeWindowProducerQueueMutex.RUnlock()
  1671. // 更新故障窗口end时间
  1672. expectEnd := util.TimeCustomChange(faultHappenTime, commonConfig.PlatformConfig.TaskAfterTime) // 窗口期望关闭时间是触发时间加上后置时间
  1673. expectLength := util.CalculateDifferenceOfTimeCustom(lastTimeWindow.TimeWindowBegin, expectEnd)
  1674. if expectLength < commonConfig.PlatformConfig.TaskMaxTime {
  1675. latestTimeWindowEnd = expectEnd
  1676. lastTimeWindow.TimeWindowEnd = latestTimeWindowEnd
  1677. lastTimeWindow.Length = util.CalculateDifferenceOfTimeCustom(lastTimeWindow.TimeWindowBegin, lastTimeWindow.TimeWindowEnd)
  1678. }
  1679. // 更新label
  1680. labels := lastTimeWindow.Labels
  1681. lastTimeWindow.Labels = util.AppendIfNotExists(labels, faultLabel)
  1682. // 更新 topic
  1683. sourceMasterTopics := lastTimeWindow.MasterTopics
  1684. lastTimeWindow.MasterTopics = util.MergeSlice(sourceMasterTopics, masterTopics)
  1685. sourceSlaveTopics := lastTimeWindow.SlaveTopics
  1686. lastTimeWindow.SlaveTopics = util.MergeSlice(sourceSlaveTopics, slaveTopics)
  1687. c_log.GlobalLogger.Infof("在旧故障窗口内,更新生产者队列最新的窗口,【Lable】=%v,【FaultTime】=%v,【Length】=%v", lastTimeWindow.Labels, lastTimeWindow.FaultTime, lastTimeWindow.Length)
  1688. }
  1689. }
  1690. func getTopicsOfNode(faultLabel string) (masterTopics []string, slaveTopics []string) {
  1691. // 获取所有需要采集的topic
  1692. var faultCodeTopics []string
  1693. for _, code := range commonConfig.CloudConfig.Triggers {
  1694. if code.Label == faultLabel {
  1695. faultCodeTopics = code.Topics
  1696. }
  1697. }
  1698. // 根据不同节点采集的topic进行分配采集
  1699. for _, acceptTopic := range faultCodeTopics {
  1700. for _, host := range commonConfig.CloudConfig.Hosts {
  1701. for _, topic := range host.Topics {
  1702. if host.Name == commonConfig.CloudConfig.Hosts[0].Name && acceptTopic == topic {
  1703. masterTopics = append(masterTopics, acceptTopic)
  1704. }
  1705. if host.Name == commonConfig.CloudConfig.Hosts[1].Name && acceptTopic == topic {
  1706. slaveTopics = append(slaveTopics, acceptTopic)
  1707. }
  1708. }
  1709. }
  1710. }
  1711. return masterTopics, slaveTopics
  1712. }