produce_window.go 65 KB

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