produce_window.go 70 KB

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