produce_window.go 43 KB

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