produce_window.go 43 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189
  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. "github.com/bluenviron/goroslib/v2"
  11. "github.com/bluenviron/goroslib/v2/pkg/msgs/geometry_msgs"
  12. "github.com/bluenviron/goroslib/v2/pkg/msgs/nav_msgs"
  13. "github.com/bluenviron/goroslib/v2/pkg/msgs/sensor_msgs"
  14. "github.com/bluenviron/goroslib/v2/pkg/msgs/tf2_msgs"
  15. "github.com/bluenviron/goroslib/v2/pkg/msgs/visualization_msgs"
  16. "math"
  17. "sync"
  18. "time"
  19. )
  20. // 所有共享变量
  21. // todo 如果触发器订阅了多个topic,需要定制化缓存指定字段;也可以将所有topic的所有字段值实时缓存,工作量太大,暂时搁置
  22. var (
  23. extendParam commonEntity.PjisuvParam
  24. // /cicv_location
  25. mutexOfCicvLocation sync.RWMutex
  26. // /tpperception
  27. mutexOfTpperception sync.RWMutex
  28. // /pj_control_pub
  29. mutexOfPjControlPub sync.RWMutex
  30. // /data_read
  31. mutexOfDataRead sync.RWMutex
  32. )
  33. // PrepareTimeWindowProducerQueue 负责监听所有主题并修改时间窗口
  34. func PrepareTimeWindowProducerQueue() {
  35. c_log.GlobalLogger.Info("订阅者 goroutine,启动。")
  36. var err error
  37. subscribers := make([]*goroslib.Subscriber, len(commonConfig.SubscribeTopics))
  38. subscribersTimes := make([]time.Time, len(commonConfig.SubscribeTopics))
  39. subscribersTimeMutexes := make([]sync.Mutex, len(commonConfig.SubscribeTopics))
  40. subscribersMutexes := make([]sync.Mutex, len(commonConfig.SubscribeTopics))
  41. for i, topic := range commonConfig.SubscribeTopics {
  42. // !!!扩展的定时任务监听,牛逼的设计!!!扩展性拉满啦
  43. if topic == masterConfig.TopicOfCicvExtend {
  44. for {
  45. time.Sleep(time.Duration(3500) * time.Millisecond)
  46. for _, f := range masterConfig.RuleOfCicvExtend {
  47. label := f(extendParam)
  48. if label != "" {
  49. saveTimeWindow(label, util.GetNowTimeCustom(), commonEntity.GetLastTimeWindow())
  50. break
  51. }
  52. }
  53. }
  54. }
  55. // 其他常规监听器
  56. c_log.GlobalLogger.Info("创建订阅者订阅话题:" + topic)
  57. // 1
  58. if topic == masterConfig.TopicOfAmrPose && len(masterConfig.RuleOfAmrPose) > 0 {
  59. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  60. Node: commonConfig.RosNode,
  61. Topic: topic,
  62. Callback: func(data *visualization_msgs.MarkerArray) {
  63. subscribersTimeMutexes[i].Lock()
  64. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  65. subscribersMutexes[i].Lock()
  66. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  67. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  68. var faultLabel string
  69. for _, f := range masterConfig.RuleOfAmrPose {
  70. faultLabel = f(data)
  71. if faultLabel != "" {
  72. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  73. subscribersTimes[i] = time.Now()
  74. break
  75. }
  76. }
  77. subscribersMutexes[i].Unlock()
  78. }
  79. subscribersTimeMutexes[i].Unlock()
  80. },
  81. })
  82. }
  83. // 2
  84. if topic == masterConfig.TopicOfBoundingBoxesFast && len(masterConfig.RuleOfBoundingBoxesFast) > 0 {
  85. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  86. Node: commonConfig.RosNode,
  87. Topic: topic,
  88. Callback: func(data *pjisuv_msgs.BoundingBoxArray) {
  89. subscribersTimeMutexes[i].Lock()
  90. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  91. subscribersMutexes[i].Lock()
  92. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  93. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  94. var faultLabel string
  95. for _, f := range masterConfig.RuleOfBoundingBoxesFast {
  96. faultLabel = f(data)
  97. if faultLabel != "" {
  98. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  99. subscribersTimes[i] = time.Now()
  100. break
  101. }
  102. }
  103. subscribersMutexes[i].Unlock()
  104. }
  105. subscribersTimeMutexes[i].Unlock()
  106. },
  107. })
  108. }
  109. // 3
  110. if topic == masterConfig.TopicOfCameraFault && len(masterConfig.RuleOfCameraFault) > 0 {
  111. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  112. Node: commonConfig.RosNode,
  113. Topic: topic,
  114. Callback: func(data *pjisuv_msgs.FaultVec) {
  115. subscribersTimeMutexes[i].Lock()
  116. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  117. subscribersMutexes[i].Lock()
  118. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  119. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  120. var faultLabel string
  121. for _, f := range masterConfig.RuleOfCameraFault {
  122. faultLabel = f(data)
  123. if faultLabel != "" {
  124. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  125. subscribersTimes[i] = time.Now()
  126. break
  127. }
  128. }
  129. subscribersMutexes[i].Unlock()
  130. }
  131. subscribersTimeMutexes[i].Unlock()
  132. },
  133. })
  134. }
  135. // 4
  136. if topic == masterConfig.TopicOfCanData && len(masterConfig.RuleOfCanData) > 0 {
  137. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  138. Node: commonConfig.RosNode,
  139. Topic: topic,
  140. Callback: func(data *pjisuv_msgs.Frame) {
  141. subscribersTimeMutexes[i].Lock()
  142. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  143. subscribersMutexes[i].Lock()
  144. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  145. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  146. var faultLabel string
  147. for _, f := range masterConfig.RuleOfCanData {
  148. faultLabel = f(data)
  149. if faultLabel != "" {
  150. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  151. subscribersTimes[i] = time.Now()
  152. break
  153. }
  154. }
  155. subscribersMutexes[i].Unlock()
  156. }
  157. subscribersTimeMutexes[i].Unlock()
  158. },
  159. })
  160. }
  161. // 5
  162. if topic == masterConfig.TopicOfCh128x1LslidarPointCloud && len(masterConfig.RuleOfCh128x1LslidarPointCloud) > 0 {
  163. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  164. Node: commonConfig.RosNode,
  165. Topic: topic,
  166. Callback: func(data *sensor_msgs.PointCloud2) {
  167. subscribersTimeMutexes[i].Lock()
  168. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  169. subscribersMutexes[i].Lock()
  170. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  171. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  172. var faultLabel string
  173. for _, f := range masterConfig.RuleOfCh128x1LslidarPointCloud {
  174. faultLabel = f(data)
  175. if faultLabel != "" {
  176. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  177. subscribersTimes[i] = time.Now()
  178. break
  179. }
  180. }
  181. subscribersMutexes[i].Unlock()
  182. }
  183. subscribersTimeMutexes[i].Unlock()
  184. },
  185. })
  186. }
  187. // 6
  188. if topic == masterConfig.TopicOfCh64wLLslidarPointCloud && len(masterConfig.RuleOfCh64wLLslidarPointCloud) > 0 {
  189. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  190. Node: commonConfig.RosNode,
  191. Topic: topic,
  192. Callback: func(data *sensor_msgs.PointCloud2) {
  193. subscribersTimeMutexes[i].Lock()
  194. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  195. subscribersMutexes[i].Lock()
  196. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  197. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  198. var faultLabel string
  199. for _, f := range masterConfig.RuleOfCh64wLLslidarPointCloud {
  200. faultLabel = f(data)
  201. if faultLabel != "" {
  202. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  203. subscribersTimes[i] = time.Now()
  204. break
  205. }
  206. }
  207. subscribersMutexes[i].Unlock()
  208. }
  209. subscribersTimeMutexes[i].Unlock()
  210. },
  211. })
  212. }
  213. // 7
  214. if topic == masterConfig.TopicOfCh64wLScan && len(masterConfig.RuleOfCh64wLScan) > 0 {
  215. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  216. Node: commonConfig.RosNode,
  217. Topic: topic,
  218. Callback: func(data *sensor_msgs.LaserScan) {
  219. subscribersTimeMutexes[i].Lock()
  220. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  221. subscribersMutexes[i].Lock()
  222. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  223. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  224. var faultLabel string
  225. for _, f := range masterConfig.RuleOfCh64wLScan {
  226. faultLabel = f(data)
  227. if faultLabel != "" {
  228. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  229. subscribersTimes[i] = time.Now()
  230. break
  231. }
  232. }
  233. subscribersMutexes[i].Unlock()
  234. }
  235. subscribersTimeMutexes[i].Unlock()
  236. },
  237. })
  238. }
  239. // 8
  240. if topic == masterConfig.TopicOfCh64wRLslidarPointCloud && len(masterConfig.RuleOfCh64wRLslidarPointCloud) > 0 {
  241. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  242. Node: commonConfig.RosNode,
  243. Topic: topic,
  244. Callback: func(data *sensor_msgs.PointCloud2) {
  245. subscribersTimeMutexes[i].Lock()
  246. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  247. subscribersMutexes[i].Lock()
  248. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  249. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  250. var faultLabel string
  251. for _, f := range masterConfig.RuleOfCh64wRLslidarPointCloud {
  252. faultLabel = f(data)
  253. if faultLabel != "" {
  254. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  255. subscribersTimes[i] = time.Now()
  256. break
  257. }
  258. }
  259. subscribersMutexes[i].Unlock()
  260. }
  261. subscribersTimeMutexes[i].Unlock()
  262. },
  263. })
  264. }
  265. // 9
  266. if topic == masterConfig.TopicOfCh64wRScan && len(masterConfig.RuleOfCh64wRScan) > 0 {
  267. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  268. Node: commonConfig.RosNode,
  269. Topic: topic,
  270. Callback: func(data *sensor_msgs.LaserScan) {
  271. subscribersTimeMutexes[i].Lock()
  272. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  273. subscribersMutexes[i].Lock()
  274. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  275. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  276. var faultLabel string
  277. for _, f := range masterConfig.RuleOfCh64wRScan {
  278. faultLabel = f(data)
  279. if faultLabel != "" {
  280. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  281. subscribersTimes[i] = time.Now()
  282. break
  283. }
  284. }
  285. subscribersMutexes[i].Unlock()
  286. }
  287. subscribersTimeMutexes[i].Unlock()
  288. },
  289. })
  290. }
  291. // 10
  292. if topic == masterConfig.TopicOfCicvLidarclusterMovingObjects && len(masterConfig.RuleOfCicvLidarclusterMovingObjects) > 0 {
  293. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  294. Node: commonConfig.RosNode,
  295. Topic: topic,
  296. Callback: func(data *pjisuv_msgs.PerceptionCicvMovingObjects) {
  297. subscribersTimeMutexes[i].Lock()
  298. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  299. subscribersMutexes[i].Lock()
  300. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  301. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  302. var faultLabel string
  303. for _, f := range masterConfig.RuleOfCicvLidarclusterMovingObjects {
  304. faultLabel = f(data)
  305. if faultLabel != "" {
  306. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  307. subscribersTimes[i] = time.Now()
  308. break
  309. }
  310. }
  311. subscribersMutexes[i].Unlock()
  312. }
  313. subscribersTimeMutexes[i].Unlock()
  314. },
  315. })
  316. }
  317. // 11
  318. if topic == masterConfig.TopicOfCicvAmrTrajectory && len(masterConfig.RuleOfCicvAmrTrajectory) > 0 {
  319. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  320. Node: commonConfig.RosNode,
  321. Topic: topic,
  322. Callback: func(data *pjisuv_msgs.Trajectory) {
  323. subscribersTimeMutexes[i].Lock()
  324. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  325. subscribersMutexes[i].Lock()
  326. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  327. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  328. var faultLabel string
  329. for _, f := range masterConfig.RuleOfCicvAmrTrajectory {
  330. faultLabel = f(data)
  331. if faultLabel != "" {
  332. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  333. subscribersTimes[i] = time.Now()
  334. break
  335. }
  336. }
  337. subscribersMutexes[i].Unlock()
  338. }
  339. subscribersTimeMutexes[i].Unlock()
  340. },
  341. })
  342. }
  343. // 12
  344. if topic == masterConfig.TopicOfCicvLocation && len(masterConfig.RuleOfCicvLocation) > 0 {
  345. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  346. Node: commonConfig.RosNode,
  347. Topic: topic,
  348. Callback: func(data *pjisuv_msgs.PerceptionLocalization) {
  349. // 更新共享变量
  350. mutexOfCicvLocation.RLock()
  351. {
  352. extendParam.VelocityYOfCicvLocation = data.VelocityX
  353. extendParam.VelocityYOfCicvLocation = data.VelocityY
  354. extendParam.YawOfCicvLocation = data.Yaw
  355. extendParam.AngularVelocityZOfCicvLocation = data.AngularVelocityZ
  356. }
  357. mutexOfCicvLocation.RUnlock()
  358. subscribersTimeMutexes[i].Lock()
  359. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  360. subscribersMutexes[i].Lock()
  361. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  362. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  363. var faultLabel string
  364. for _, f := range masterConfig.RuleOfCicvLocation {
  365. faultLabel = f(data)
  366. if faultLabel != "" {
  367. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  368. break
  369. }
  370. }
  371. subscribersMutexes[i].Unlock()
  372. }
  373. subscribersTimeMutexes[i].Unlock()
  374. },
  375. })
  376. }
  377. // 13
  378. if topic == masterConfig.TopicOfCloudClusters && len(masterConfig.RuleOfCloudClusters) > 0 {
  379. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  380. Node: commonConfig.RosNode,
  381. Topic: topic,
  382. Callback: func(data *pjisuv_msgs.AutowareCloudClusterArray) {
  383. subscribersTimeMutexes[i].Lock()
  384. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  385. subscribersMutexes[i].Lock()
  386. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  387. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  388. var faultLabel string
  389. for _, f := range masterConfig.RuleOfCloudClusters {
  390. faultLabel = f(data)
  391. if faultLabel != "" {
  392. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  393. subscribersTimes[i] = time.Now()
  394. break
  395. }
  396. }
  397. subscribersMutexes[i].Unlock()
  398. }
  399. subscribersTimeMutexes[i].Unlock()
  400. },
  401. })
  402. }
  403. // 14
  404. if topic == masterConfig.TopicOfHeartbeatInfo && len(masterConfig.RuleOfHeartbeatInfo) > 0 {
  405. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  406. Node: commonConfig.RosNode,
  407. Topic: topic,
  408. Callback: func(data *pjisuv_msgs.HeartBeatInfo) {
  409. subscribersTimeMutexes[i].Lock()
  410. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  411. subscribersMutexes[i].Lock()
  412. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  413. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  414. var faultLabel string
  415. for _, f := range masterConfig.RuleOfHeartbeatInfo {
  416. faultLabel = f(data)
  417. if faultLabel != "" {
  418. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  419. subscribersTimes[i] = time.Now()
  420. break
  421. }
  422. }
  423. subscribersMutexes[i].Unlock()
  424. }
  425. subscribersTimeMutexes[i].Unlock()
  426. },
  427. })
  428. }
  429. // 15
  430. if topic == masterConfig.TopicOfLidarPretreatmentCost && len(masterConfig.RuleOfLidarPretreatmentCost) > 0 {
  431. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  432. Node: commonConfig.RosNode,
  433. Topic: topic,
  434. Callback: func(data *geometry_msgs.Vector3Stamped) {
  435. subscribersTimeMutexes[i].Lock()
  436. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  437. subscribersMutexes[i].Lock()
  438. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  439. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  440. var faultLabel string
  441. for _, f := range masterConfig.RuleOfLidarPretreatmentCost {
  442. faultLabel = f(data)
  443. if faultLabel != "" {
  444. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  445. subscribersTimes[i] = time.Now()
  446. break
  447. }
  448. }
  449. subscribersMutexes[i].Unlock()
  450. }
  451. subscribersTimeMutexes[i].Unlock()
  452. },
  453. })
  454. }
  455. // 16
  456. if topic == masterConfig.TopicOfLidarPretreatmentOdometry && len(masterConfig.RuleOfLidarPretreatmentOdometry) > 0 {
  457. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  458. Node: commonConfig.RosNode,
  459. Topic: topic,
  460. Callback: func(data *nav_msgs.Odometry) {
  461. subscribersTimeMutexes[i].Lock()
  462. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  463. subscribersMutexes[i].Lock()
  464. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  465. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  466. var faultLabel string
  467. for _, f := range masterConfig.RuleOfLidarPretreatmentOdometry {
  468. faultLabel = f(data)
  469. if faultLabel != "" {
  470. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  471. subscribersTimes[i] = time.Now()
  472. break
  473. }
  474. }
  475. subscribersMutexes[i].Unlock()
  476. }
  477. subscribersTimeMutexes[i].Unlock()
  478. },
  479. })
  480. }
  481. // 17
  482. if topic == masterConfig.TopicOfLidarRoi && len(masterConfig.RuleOfLidarRoi) > 0 {
  483. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  484. Node: commonConfig.RosNode,
  485. Topic: topic,
  486. Callback: func(data *geometry_msgs.PolygonStamped) {
  487. subscribersTimeMutexes[i].Lock()
  488. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  489. subscribersMutexes[i].Lock()
  490. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  491. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  492. var faultLabel string
  493. for _, f := range masterConfig.RuleOfLidarRoi {
  494. faultLabel = f(data)
  495. if faultLabel != "" {
  496. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  497. subscribersTimes[i] = time.Now()
  498. break
  499. }
  500. }
  501. subscribersMutexes[i].Unlock()
  502. }
  503. subscribersTimeMutexes[i].Unlock()
  504. },
  505. })
  506. }
  507. // 18
  508. if topic == masterConfig.TopicOfLine1 && len(masterConfig.RuleOfLine1) > 0 {
  509. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  510. Node: commonConfig.RosNode,
  511. Topic: topic,
  512. Callback: func(data *nav_msgs.Path) {
  513. subscribersTimeMutexes[i].Lock()
  514. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  515. subscribersMutexes[i].Lock()
  516. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  517. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  518. var faultLabel string
  519. for _, f := range masterConfig.RuleOfLine1 {
  520. faultLabel = f(data)
  521. if faultLabel != "" {
  522. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  523. subscribersTimes[i] = time.Now()
  524. break
  525. }
  526. }
  527. subscribersMutexes[i].Unlock()
  528. }
  529. subscribersTimeMutexes[i].Unlock()
  530. },
  531. })
  532. }
  533. // 19
  534. if topic == masterConfig.TopicOfLine2 && len(masterConfig.RuleOfLine2) > 0 {
  535. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  536. Node: commonConfig.RosNode,
  537. Topic: topic,
  538. Callback: func(data *nav_msgs.Path) {
  539. subscribersTimeMutexes[i].Lock()
  540. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  541. subscribersMutexes[i].Lock()
  542. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  543. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  544. var faultLabel string
  545. for _, f := range masterConfig.RuleOfLine2 {
  546. faultLabel = f(data)
  547. if faultLabel != "" {
  548. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  549. subscribersTimes[i] = time.Now()
  550. break
  551. }
  552. }
  553. subscribersMutexes[i].Unlock()
  554. }
  555. subscribersTimeMutexes[i].Unlock()
  556. },
  557. })
  558. }
  559. // 20
  560. if topic == masterConfig.TopicOfMapPolygon && len(masterConfig.RuleOfMapPolygon) > 0 {
  561. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  562. Node: commonConfig.RosNode,
  563. Topic: topic,
  564. Callback: func(data *pjisuv_msgs.PolygonStamped) {
  565. subscribersTimeMutexes[i].Lock()
  566. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  567. subscribersMutexes[i].Lock()
  568. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  569. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  570. var faultLabel string
  571. for _, f := range masterConfig.RuleOfMapPolygon {
  572. faultLabel = f(data)
  573. if faultLabel != "" {
  574. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  575. subscribersTimes[i] = time.Now()
  576. break
  577. }
  578. }
  579. subscribersMutexes[i].Unlock()
  580. }
  581. subscribersTimeMutexes[i].Unlock()
  582. },
  583. })
  584. }
  585. // 21
  586. if topic == masterConfig.TopicOfObstacleDisplay && len(masterConfig.RuleOfObstacleDisplay) > 0 {
  587. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  588. Node: commonConfig.RosNode,
  589. Topic: topic,
  590. Callback: func(data *visualization_msgs.MarkerArray) {
  591. subscribersTimeMutexes[i].Lock()
  592. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  593. subscribersMutexes[i].Lock()
  594. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  595. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  596. var faultLabel string
  597. for _, f := range masterConfig.RuleOfObstacleDisplay {
  598. faultLabel = f(data)
  599. if faultLabel != "" {
  600. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  601. subscribersTimes[i] = time.Now()
  602. break
  603. }
  604. }
  605. subscribersMutexes[i].Unlock()
  606. }
  607. subscribersTimeMutexes[i].Unlock()
  608. },
  609. })
  610. }
  611. // 22
  612. if topic == masterConfig.TopicOfPjControlPub && len(masterConfig.RuleOfPjControlPub) > 0 {
  613. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  614. Node: commonConfig.RosNode,
  615. Topic: topic,
  616. Callback: func(data *pjisuv_msgs.CommonVehicleCmd) {
  617. // 更新共享变量
  618. mutexOfPjControlPub.RLock()
  619. {
  620. extendParam.NumCountPjiControlCommandOfPjControlPub++
  621. if extendParam.NumCountPjiControlCommandOfPjControlPub == 10 {
  622. extendParam.EgoSteeringCmdOfPjControlPub = append(extendParam.EgoSteeringCmdOfPjControlPub, data.ICPVCmdStrAngle)
  623. extendParam.EgoThrottleCmdOfPjControlPub = append(extendParam.EgoThrottleCmdOfPjControlPub, data.ICPVCmdAccPelPosAct)
  624. extendParam.NumCountPjiControlCommandOfPjControlPub = 0
  625. }
  626. }
  627. mutexOfPjControlPub.RUnlock()
  628. subscribersTimeMutexes[i].Lock()
  629. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  630. subscribersMutexes[i].Lock()
  631. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  632. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  633. var faultLabel string
  634. for _, f := range masterConfig.RuleOfPjControlPub {
  635. faultLabel = f(data)
  636. if faultLabel != "" {
  637. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  638. subscribersTimes[i] = time.Now()
  639. break
  640. }
  641. }
  642. subscribersMutexes[i].Unlock()
  643. }
  644. subscribersTimeMutexes[i].Unlock()
  645. },
  646. })
  647. }
  648. // 23
  649. if topic == masterConfig.TopicOfPointsCluster && len(masterConfig.RuleOfPointsCluster) > 0 {
  650. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  651. Node: commonConfig.RosNode,
  652. Topic: topic,
  653. Callback: func(data *sensor_msgs.PointCloud2) {
  654. subscribersTimeMutexes[i].Lock()
  655. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  656. subscribersMutexes[i].Lock()
  657. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  658. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  659. var faultLabel string
  660. for _, f := range masterConfig.RuleOfPointsCluster {
  661. faultLabel = f(data)
  662. if faultLabel != "" {
  663. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  664. subscribersTimes[i] = time.Now()
  665. break
  666. }
  667. }
  668. subscribersMutexes[i].Unlock()
  669. }
  670. subscribersTimeMutexes[i].Unlock()
  671. },
  672. })
  673. }
  674. // 24
  675. if topic == masterConfig.TopicOfPointsConcat && len(masterConfig.RuleOfPointsConcat) > 0 {
  676. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  677. Node: commonConfig.RosNode,
  678. Topic: topic,
  679. Callback: func(data *sensor_msgs.PointCloud2) {
  680. subscribersTimeMutexes[i].Lock()
  681. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  682. subscribersMutexes[i].Lock()
  683. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  684. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  685. var faultLabel string
  686. for _, f := range masterConfig.RuleOfPointsConcat {
  687. faultLabel = f(data)
  688. if faultLabel != "" {
  689. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  690. subscribersTimes[i] = time.Now()
  691. break
  692. }
  693. }
  694. subscribersMutexes[i].Unlock()
  695. }
  696. subscribersTimeMutexes[i].Unlock()
  697. },
  698. })
  699. }
  700. // 25
  701. if topic == masterConfig.TopicOfReferenceDisplay && len(masterConfig.RuleOfReferenceDisplay) > 0 {
  702. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  703. Node: commonConfig.RosNode,
  704. Topic: topic,
  705. Callback: func(data *nav_msgs.Path) {
  706. subscribersTimeMutexes[i].Lock()
  707. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  708. subscribersMutexes[i].Lock()
  709. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  710. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  711. var faultLabel string
  712. for _, f := range masterConfig.RuleOfReferenceDisplay {
  713. faultLabel = f(data)
  714. if faultLabel != "" {
  715. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  716. subscribersTimes[i] = time.Now()
  717. break
  718. }
  719. }
  720. subscribersMutexes[i].Unlock()
  721. }
  722. subscribersTimeMutexes[i].Unlock()
  723. },
  724. })
  725. }
  726. // 26
  727. if topic == masterConfig.TopicOfReferenceTrajectory && len(masterConfig.RuleOfReferenceTrajectory) > 0 {
  728. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  729. Node: commonConfig.RosNode,
  730. Topic: topic,
  731. Callback: func(data *pjisuv_msgs.Trajectory) {
  732. subscribersTimeMutexes[i].Lock()
  733. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  734. subscribersMutexes[i].Lock()
  735. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  736. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  737. var faultLabel string
  738. for _, f := range masterConfig.RuleOfReferenceTrajectory {
  739. faultLabel = f(data)
  740. if faultLabel != "" {
  741. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  742. subscribersTimes[i] = time.Now()
  743. break
  744. }
  745. }
  746. subscribersMutexes[i].Unlock()
  747. }
  748. subscribersTimeMutexes[i].Unlock()
  749. },
  750. })
  751. }
  752. // 27
  753. if topic == masterConfig.TopicOfRoiPoints && len(masterConfig.RuleOfRoiPoints) > 0 {
  754. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  755. Node: commonConfig.RosNode,
  756. Topic: topic,
  757. Callback: func(data *sensor_msgs.PointCloud2) {
  758. subscribersTimeMutexes[i].Lock()
  759. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  760. subscribersMutexes[i].Lock()
  761. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  762. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  763. var faultLabel string
  764. for _, f := range masterConfig.RuleOfRoiPoints {
  765. faultLabel = f(data)
  766. if faultLabel != "" {
  767. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  768. subscribersTimes[i] = time.Now()
  769. break
  770. }
  771. }
  772. subscribersMutexes[i].Unlock()
  773. }
  774. subscribersTimeMutexes[i].Unlock()
  775. },
  776. })
  777. }
  778. // 28
  779. if topic == masterConfig.TopicOfRoiPolygon && len(masterConfig.RuleOfRoiPolygon) > 0 {
  780. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  781. Node: commonConfig.RosNode,
  782. Topic: topic,
  783. Callback: func(data *nav_msgs.Path) {
  784. subscribersTimeMutexes[i].Lock()
  785. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  786. subscribersMutexes[i].Lock()
  787. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  788. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  789. var faultLabel string
  790. for _, f := range masterConfig.RuleOfRoiPolygon {
  791. faultLabel = f(data)
  792. if faultLabel != "" {
  793. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  794. subscribersTimes[i] = time.Now()
  795. break
  796. }
  797. }
  798. subscribersMutexes[i].Unlock()
  799. }
  800. subscribersTimeMutexes[i].Unlock()
  801. },
  802. })
  803. }
  804. // 29
  805. if topic == masterConfig.TopicOfTf && len(masterConfig.RuleOfTf) > 0 {
  806. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  807. Node: commonConfig.RosNode,
  808. Topic: topic,
  809. Callback: func(data *tf2_msgs.TFMessage) {
  810. subscribersTimeMutexes[i].Lock()
  811. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  812. subscribersMutexes[i].Lock()
  813. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  814. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  815. var faultLabel string
  816. for _, f := range masterConfig.RuleOfTf {
  817. faultLabel = f(data)
  818. if faultLabel != "" {
  819. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  820. subscribersTimes[i] = time.Now()
  821. break
  822. }
  823. }
  824. subscribersMutexes[i].Unlock()
  825. }
  826. subscribersTimeMutexes[i].Unlock()
  827. },
  828. })
  829. }
  830. // 30
  831. if topic == masterConfig.TopicOfTpperception && len(masterConfig.RuleOfTpperception) > 0 {
  832. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  833. Node: commonConfig.RosNode,
  834. Topic: topic,
  835. Callback: func(data *pjisuv_msgs.PerceptionObjects) {
  836. // 更新共享变量
  837. mutexOfTpperception.RLock()
  838. {
  839. for _, obj := range data.Objs {
  840. if obj.X <= 5 || math.Abs(float64(obj.Y)) >= 10 {
  841. continue
  842. }
  843. if _, ok := extendParam.ObjDicOfTpperception[obj.Id]; !ok {
  844. extendParam.ObjDicOfTpperception[obj.Id] = []float32{}
  845. }
  846. extendParam.ObjDicOfTpperception[obj.Id] = append(extendParam.ObjDicOfTpperception[obj.Id], obj.Y)
  847. }
  848. }
  849. mutexOfTpperception.RUnlock()
  850. subscribersTimeMutexes[i].Lock()
  851. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  852. subscribersMutexes[i].Lock()
  853. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  854. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  855. var faultLabel string
  856. for _, f := range masterConfig.RuleOfTpperception {
  857. faultLabel = f(data, extendParam)
  858. if faultLabel != "" {
  859. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  860. break
  861. }
  862. }
  863. subscribersMutexes[i].Unlock()
  864. }
  865. subscribersTimeMutexes[i].Unlock()
  866. },
  867. })
  868. }
  869. // 31
  870. if topic == masterConfig.TopicOfTpperceptionVis && len(masterConfig.RuleOfTpperceptionVis) > 0 {
  871. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  872. Node: commonConfig.RosNode,
  873. Topic: topic,
  874. Callback: func(data *visualization_msgs.MarkerArray) {
  875. subscribersTimeMutexes[i].Lock()
  876. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  877. subscribersMutexes[i].Lock()
  878. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  879. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  880. var faultLabel string
  881. for _, f := range masterConfig.RuleOfTpperceptionVis {
  882. faultLabel = f(data)
  883. if faultLabel != "" {
  884. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  885. subscribersTimes[i] = time.Now()
  886. break
  887. }
  888. }
  889. subscribersMutexes[i].Unlock()
  890. }
  891. subscribersTimeMutexes[i].Unlock()
  892. },
  893. })
  894. }
  895. // 32
  896. if topic == masterConfig.TopicOfTprouteplan && len(masterConfig.RuleOfTprouteplan) > 0 {
  897. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  898. Node: commonConfig.RosNode,
  899. Topic: topic,
  900. Callback: func(data *pjisuv_msgs.RoutePlan) {
  901. subscribersTimeMutexes[i].Lock()
  902. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  903. subscribersMutexes[i].Lock()
  904. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  905. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  906. var faultLabel string
  907. for _, f := range masterConfig.RuleOfTprouteplan {
  908. faultLabel = f(data)
  909. if faultLabel != "" {
  910. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  911. subscribersTimes[i] = time.Now()
  912. break
  913. }
  914. }
  915. subscribersMutexes[i].Unlock()
  916. }
  917. subscribersTimeMutexes[i].Unlock()
  918. },
  919. })
  920. }
  921. // 33
  922. if topic == masterConfig.TopicOfTrajectoryDisplay && len(masterConfig.RuleOfTrajectoryDisplay) > 0 {
  923. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  924. Node: commonConfig.RosNode,
  925. Topic: topic,
  926. Callback: func(data *nav_msgs.Path) {
  927. subscribersTimeMutexes[i].Lock()
  928. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  929. subscribersMutexes[i].Lock()
  930. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  931. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  932. var faultLabel string
  933. for _, f := range masterConfig.RuleOfTrajectoryDisplay {
  934. faultLabel = f(data)
  935. if faultLabel != "" {
  936. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  937. subscribersTimes[i] = time.Now()
  938. break
  939. }
  940. }
  941. subscribersMutexes[i].Unlock()
  942. }
  943. subscribersTimeMutexes[i].Unlock()
  944. },
  945. })
  946. }
  947. // 34
  948. if topic == masterConfig.TopicOfUngroundCloudpoints && len(masterConfig.RuleOfUngroundCloudpoints) > 0 {
  949. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  950. Node: commonConfig.RosNode,
  951. Topic: topic,
  952. Callback: func(data *sensor_msgs.PointCloud2) {
  953. subscribersTimeMutexes[i].Lock()
  954. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  955. subscribersMutexes[i].Lock()
  956. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  957. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  958. var faultLabel string
  959. for _, f := range masterConfig.RuleOfUngroundCloudpoints {
  960. faultLabel = f(data)
  961. if faultLabel != "" {
  962. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  963. subscribersTimes[i] = time.Now()
  964. break
  965. }
  966. }
  967. subscribersMutexes[i].Unlock()
  968. }
  969. subscribersTimeMutexes[i].Unlock()
  970. },
  971. })
  972. }
  973. // 35
  974. if topic == masterConfig.TopicOfCameraImage && len(masterConfig.RuleOfCameraImage) > 0 {
  975. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  976. Node: commonConfig.RosNode,
  977. Topic: topic,
  978. Callback: func(data *sensor_msgs.Image) {
  979. subscribersTimeMutexes[i].Lock()
  980. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  981. subscribersMutexes[i].Lock()
  982. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  983. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  984. var faultLabel string
  985. for _, f := range masterConfig.RuleOfCameraImage {
  986. faultLabel = f(data)
  987. if faultLabel != "" {
  988. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  989. subscribersTimes[i] = time.Now()
  990. break
  991. }
  992. }
  993. subscribersMutexes[i].Unlock()
  994. }
  995. subscribersTimeMutexes[i].Unlock()
  996. },
  997. })
  998. }
  999. // 36
  1000. if topic == masterConfig.TopicOfDataRead && len(masterConfig.RuleOfDataRead) > 0 {
  1001. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  1002. Node: commonConfig.RosNode,
  1003. Topic: topic,
  1004. Callback: func(data *pjisuv_msgs.Retrieval) {
  1005. // 更新共享变量
  1006. mutexOfDataRead.RLock()
  1007. {
  1008. extendParam.NumCountDataReadOfDataRead++
  1009. if extendParam.NumCountDataReadOfDataRead == 10 {
  1010. extendParam.EgoSteeringRealOfDataRead = append(extendParam.EgoSteeringRealOfDataRead, data.ActStrWhAng)
  1011. extendParam.EgoThrottleRealOfDataRead = append(extendParam.EgoThrottleRealOfDataRead, data.AccPed2)
  1012. extendParam.NumCountDataReadOfDataRead = 0
  1013. }
  1014. extendParam.StrgAngleRealValueOfDataRead = data.ActStrWhAng
  1015. }
  1016. mutexOfDataRead.RUnlock()
  1017. subscribersTimeMutexes[i].Lock()
  1018. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  1019. subscribersMutexes[i].Lock()
  1020. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  1021. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  1022. var faultLabel string
  1023. for _, f := range masterConfig.RuleOfDataRead {
  1024. faultLabel = f(data)
  1025. if faultLabel != "" {
  1026. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1027. subscribersTimes[i] = time.Now()
  1028. break
  1029. }
  1030. }
  1031. subscribersMutexes[i].Unlock()
  1032. }
  1033. subscribersTimeMutexes[i].Unlock()
  1034. },
  1035. })
  1036. }
  1037. // 37
  1038. if topic == masterConfig.TopicOfPjiGps && len(masterConfig.RuleOfPjiGps) > 0 {
  1039. subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
  1040. Node: commonConfig.RosNode,
  1041. Topic: topic,
  1042. Callback: func(data *pjisuv_msgs.PerceptionLocalization) {
  1043. subscribersTimeMutexes[i].Lock()
  1044. if time.Since(subscribersTimes[i]).Seconds() > 1 {
  1045. subscribersMutexes[i].Lock()
  1046. faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
  1047. lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
  1048. var faultLabel string
  1049. for _, f := range masterConfig.RuleOfPjiGps {
  1050. faultLabel = f(data)
  1051. if faultLabel != "" {
  1052. saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
  1053. subscribersTimes[i] = time.Now()
  1054. break
  1055. }
  1056. }
  1057. subscribersMutexes[i].Unlock()
  1058. }
  1059. subscribersTimeMutexes[i].Unlock()
  1060. },
  1061. })
  1062. }
  1063. if err != nil {
  1064. c_log.GlobalLogger.Info("创建订阅者报错:", err)
  1065. //TODO 如何回传日志
  1066. continue
  1067. }
  1068. }
  1069. select {
  1070. case signal := <-service.ChannelKillWindowProducer:
  1071. if signal == 1 {
  1072. commonConfig.RosNode.Close()
  1073. service.AddKillTimes("3")
  1074. return
  1075. }
  1076. }
  1077. }
  1078. func saveTimeWindow(faultLabel string, faultHappenTime string, lastTimeWindow *commonEntity.TimeWindow) {
  1079. masterTopics, slaveTopics := getTopicsOfNode(faultLabel)
  1080. if lastTimeWindow == nil || util.TimeCustom1GreaterTimeCustom2(faultHappenTime, lastTimeWindow.TimeWindowEnd) {
  1081. // 2-1 如果是不在旧故障窗口内,添加一个新窗口
  1082. newTimeWindow := commonEntity.TimeWindow{
  1083. FaultTime: faultHappenTime,
  1084. TimeWindowBegin: util.TimeCustomChange(faultHappenTime, -commonConfig.PlatformConfig.TaskBeforeTime),
  1085. TimeWindowEnd: util.TimeCustomChange(faultHappenTime, commonConfig.PlatformConfig.TaskAfterTime),
  1086. Length: commonConfig.PlatformConfig.TaskBeforeTime + commonConfig.PlatformConfig.TaskAfterTime + 1,
  1087. Labels: []string{faultLabel},
  1088. MasterTopics: masterTopics,
  1089. SlaveTopics: slaveTopics,
  1090. }
  1091. c_log.GlobalLogger.Infof("不在旧故障窗口内,向生产者队列添加一个新窗口,【Lable】=%v,【FaultTime】=%v,【Length】=%v", newTimeWindow.Labels, newTimeWindow.FaultTime, newTimeWindow.Length)
  1092. commonEntity.AddTimeWindowToTimeWindowProducerQueue(newTimeWindow)
  1093. } else {
  1094. // 2-2 如果在旧故障窗口内
  1095. commonEntity.TimeWindowProducerQueueMutex.RLock()
  1096. defer commonEntity.TimeWindowProducerQueueMutex.RUnlock()
  1097. // 2-2-1 更新故障窗口end时间
  1098. maxEnd := util.TimeCustomChange(lastTimeWindow.TimeWindowBegin, commonConfig.PlatformConfig.TaskMaxTime)
  1099. expectEnd := util.TimeCustomChange(faultHappenTime, commonConfig.PlatformConfig.TaskAfterTime)
  1100. if util.TimeCustom1GreaterTimeCustom2(expectEnd, maxEnd) {
  1101. lastTimeWindow.TimeWindowEnd = maxEnd
  1102. lastTimeWindow.Length = commonConfig.PlatformConfig.TaskMaxTime
  1103. } else {
  1104. if util.TimeCustom1GreaterTimeCustom2(expectEnd, lastTimeWindow.TimeWindowEnd) {
  1105. lastTimeWindow.TimeWindowEnd = expectEnd
  1106. lastTimeWindow.Length = util.CalculateDifferenceOfTimeCustom(lastTimeWindow.TimeWindowBegin, expectEnd)
  1107. }
  1108. }
  1109. // 2-2-2 更新label
  1110. labels := lastTimeWindow.Labels
  1111. lastTimeWindow.Labels = util.AppendIfNotExists(labels, faultLabel)
  1112. // 2-2-3 更新 topic
  1113. sourceMasterTopics := lastTimeWindow.MasterTopics
  1114. lastTimeWindow.MasterTopics = util.MergeSlice(sourceMasterTopics, masterTopics)
  1115. sourceSlaveTopics := lastTimeWindow.SlaveTopics
  1116. lastTimeWindow.SlaveTopics = util.MergeSlice(sourceSlaveTopics, slaveTopics)
  1117. c_log.GlobalLogger.Infof("在旧故障窗口内,更新生产者队列最新的窗口,【Lable】=%v,【FaultTime】=%v,【Length】=%v", lastTimeWindow.Labels, lastTimeWindow.FaultTime, lastTimeWindow.Length)
  1118. }
  1119. }
  1120. func getTopicsOfNode(faultLabel string) (masterTopics []string, slaveTopics []string) {
  1121. // 获取所有需要采集的topic
  1122. var faultCodeTopics []string
  1123. for _, code := range commonConfig.CloudConfig.Triggers {
  1124. if code.Label == faultLabel {
  1125. faultCodeTopics = code.Topics
  1126. }
  1127. }
  1128. // 根据不同节点采集的topic进行分配采集
  1129. for _, acceptTopic := range faultCodeTopics {
  1130. for _, host := range commonConfig.CloudConfig.Hosts {
  1131. for _, topic := range host.Topics {
  1132. if host.Name == commonConfig.CloudConfig.Hosts[0].Name && acceptTopic == topic {
  1133. masterTopics = append(masterTopics, acceptTopic)
  1134. }
  1135. if host.Name == commonConfig.CloudConfig.Hosts[1].Name && acceptTopic == topic {
  1136. slaveTopics = append(slaveTopics, acceptTopic)
  1137. }
  1138. }
  1139. }
  1140. }
  1141. return masterTopics, slaveTopics
  1142. }