produce_window.go 64 KB

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