produce_window.go 65 KB

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