produce_window.go 67 KB

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