trigger_init.go 20 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541
  1. package config
  2. import (
  3. "cicv-data-closedloop/aarch64/pjisuv/common/config"
  4. "cicv-data-closedloop/common/config/c_log"
  5. "cicv-data-closedloop/common/util"
  6. "cicv-data-closedloop/pjisuv_msgs"
  7. "cicv-data-closedloop/pjisuv_ticker"
  8. "github.com/bluenviron/goroslib/v2/pkg/msgs/geometry_msgs"
  9. "github.com/bluenviron/goroslib/v2/pkg/msgs/nav_msgs"
  10. "github.com/bluenviron/goroslib/v2/pkg/msgs/sensor_msgs"
  11. "github.com/bluenviron/goroslib/v2/pkg/msgs/tf2_msgs"
  12. "github.com/bluenviron/goroslib/v2/pkg/msgs/visualization_msgs"
  13. "plugin"
  14. "slices"
  15. "sync"
  16. )
  17. func InitTriggerConfig() {
  18. config.OssMutex.Lock()
  19. defer config.OssMutex.Unlock()
  20. triggerLocalPathsMapTriggerId := make(map[string]string)
  21. c_log.GlobalLogger.Info("主节点加载触发器插件 - 开始。")
  22. // 1 获取数采任务的触发器列表
  23. cloudTriggers := &config.PlatformConfig.TaskTriggers
  24. // 2 获取本地触发器列表(触发器目录的一级子目录为【触发器ID_触发器Label】)
  25. localTriggerIds := util.GetFirstLevelSubdirectories(config.CloudConfig.TriggersDir)
  26. // 3 对比触发器列表,本地没有的则下载
  27. for _, trigger := range *cloudTriggers {
  28. id := util.ToString(trigger.TriggerId)
  29. hasIdDir := slices.Contains(localTriggerIds, id) // 改成了 slices 工具库
  30. triggerLocalDir := config.CloudConfig.TriggersDir + id + "/"
  31. hasLabelSo, soPaths := util.CheckSoFilesInDirectory(triggerLocalDir)
  32. var triggerLocalPath string
  33. if hasIdDir && hasLabelSo { // 已存在的触发器需要判断是否大小一致
  34. triggerLocalPath = soPaths[0]
  35. ossSize, _ := util.GetOSSFileSize(config.OssBucket, trigger.TriggerScriptPath)
  36. localSize, _ := util.GetFileSize(triggerLocalPath)
  37. if ossSize == localSize {
  38. c_log.GlobalLogger.Info("触发器插件 ", triggerLocalPath, " 存在且与云端触发器大小一致。")
  39. triggerLocalPathsMapTriggerId[triggerLocalPath] = id
  40. continue
  41. }
  42. }
  43. label := util.GetFileNameWithoutExtension(config.CloudConfig.TriggersDir + trigger.TriggerScriptPath)
  44. triggerLocalPath = config.CloudConfig.TriggersDir + id + "/" + label + ".so"
  45. c_log.GlobalLogger.Info("开始下载触发器插件从 ", trigger.TriggerScriptPath, " 到 ", triggerLocalPath)
  46. _ = util.CreateParentDir(triggerLocalPath)
  47. for {
  48. if err := config.OssBucket.GetObjectToFile(trigger.TriggerScriptPath, triggerLocalPath); err != nil {
  49. c_log.GlobalLogger.Error("下载触发器插件失败,再次尝试:", err)
  50. continue
  51. }
  52. break
  53. }
  54. triggerLocalPathsMapTriggerId[triggerLocalPath] = id
  55. }
  56. success := 0
  57. // 加载所有触发器的文件
  58. for triggerLocalPath, triggerId := range triggerLocalPathsMapTriggerId {
  59. // 载入插件到数组
  60. open, err := plugin.Open(triggerLocalPath)
  61. if err != nil {
  62. c_log.GlobalLogger.Error("加载本地插件", triggerLocalPath, "失败。", err)
  63. continue
  64. }
  65. topic0, err := open.Lookup("Topic")
  66. if err != nil {
  67. c_log.GlobalLogger.Error("加载本地插件", triggerLocalPath, "中的Topic方法失败。", err)
  68. continue
  69. }
  70. topic1, ok := topic0.(func() string)
  71. if ok != true {
  72. c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func() string):", err)
  73. continue
  74. }
  75. topic2 := topic1()
  76. rule, err := open.Lookup("Rule")
  77. if err != nil {
  78. c_log.GlobalLogger.Error("加载本地插件", triggerLocalPath, "中的Rule方法失败。", err)
  79. continue
  80. }
  81. if pjisuv_ticker.TickerTopic == topic2 { // 定时任务触发器
  82. if f, ok1 := rule.(func(shareVars *sync.Map)); ok1 {
  83. RuleOfCicvTicker = append(RuleOfCicvTicker, f)
  84. goto JudgeDone
  85. }
  86. log(triggerLocalPath)
  87. continue
  88. } else if TopicOfAmrPose == topic2 { //1
  89. if f, ok1 := rule.(func(data *visualization_msgs.MarkerArray) string); ok1 {
  90. RuleOfAmrPose1 = append(RuleOfAmrPose1, f)
  91. goto JudgeDone
  92. }
  93. if f, ok3 := rule.(func(shareVars *sync.Map, data *visualization_msgs.MarkerArray) string); ok3 {
  94. RuleOfAmrPose3 = append(RuleOfAmrPose3, f)
  95. goto JudgeDone
  96. }
  97. log(triggerLocalPath)
  98. continue
  99. } else if TopicOfBoundingBoxesFast == topic2 { //2
  100. if f, ok1 := rule.(func(data *pjisuv_msgs.BoundingBoxArray) string); ok1 {
  101. RuleOfBoundingBoxesFast1 = append(RuleOfBoundingBoxesFast1, f)
  102. goto JudgeDone
  103. }
  104. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.BoundingBoxArray) string); ok3 {
  105. RuleOfBoundingBoxesFast3 = append(RuleOfBoundingBoxesFast3, f)
  106. goto JudgeDone
  107. }
  108. log(triggerLocalPath)
  109. continue
  110. } else if TopicOfCameraFault == topic2 { //3
  111. if f, ok1 := rule.(func(data *pjisuv_msgs.FaultVec) string); ok1 {
  112. RuleOfCameraFault1 = append(RuleOfCameraFault1, f)
  113. goto JudgeDone
  114. }
  115. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.FaultVec) string); ok3 {
  116. RuleOfCameraFault3 = append(RuleOfCameraFault3, f)
  117. goto JudgeDone
  118. }
  119. log(triggerLocalPath)
  120. continue
  121. } else if TopicOfCanData == topic2 { //4
  122. if f, ok1 := rule.(func(data *pjisuv_msgs.Frame) string); ok1 {
  123. RuleOfCanData1 = append(RuleOfCanData1, f)
  124. goto JudgeDone
  125. }
  126. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.Frame) string); ok3 {
  127. RuleOfCanData3 = append(RuleOfCanData3, f)
  128. goto JudgeDone
  129. }
  130. log(triggerLocalPath)
  131. continue
  132. } else if TopicOfCh128x1LslidarPointCloud == topic2 { //5
  133. if f, ok1 := rule.(func(data *sensor_msgs.PointCloud2) string); ok1 {
  134. RuleOfCh128x1LslidarPointCloud1 = append(RuleOfCh128x1LslidarPointCloud1, f)
  135. goto JudgeDone
  136. }
  137. if f, ok3 := rule.(func(shareVars *sync.Map, data *sensor_msgs.PointCloud2) string); ok3 {
  138. RuleOfCh128x1LslidarPointCloud3 = append(RuleOfCh128x1LslidarPointCloud3, f)
  139. goto JudgeDone
  140. }
  141. log(triggerLocalPath)
  142. continue
  143. } else if TopicOfCh64wLLslidarPointCloud == topic2 { //6
  144. if f, ok1 := rule.(func(data *sensor_msgs.PointCloud2) string); ok1 {
  145. RuleOfCh64wLLslidarPointCloud1 = append(RuleOfCh64wLLslidarPointCloud1, f)
  146. goto JudgeDone
  147. }
  148. if f, ok3 := rule.(func(shareVars *sync.Map, data *sensor_msgs.PointCloud2) string); ok3 {
  149. RuleOfCh64wLLslidarPointCloud3 = append(RuleOfCh64wLLslidarPointCloud3, f)
  150. goto JudgeDone
  151. }
  152. log(triggerLocalPath)
  153. continue
  154. } else if TopicOfCh64wLScan == topic2 { //7
  155. if f, ok1 := rule.(func(data *sensor_msgs.LaserScan) string); ok1 {
  156. RuleOfCh64wLScan1 = append(RuleOfCh64wLScan1, f)
  157. goto JudgeDone
  158. }
  159. if f, ok3 := rule.(func(shareVars *sync.Map, data *sensor_msgs.LaserScan) string); ok3 {
  160. RuleOfCh64wLScan3 = append(RuleOfCh64wLScan3, f)
  161. goto JudgeDone
  162. }
  163. log(triggerLocalPath)
  164. continue
  165. } else if TopicOfCh64wRLslidarPointCloud == topic2 { //8
  166. if f, ok1 := rule.(func(data *sensor_msgs.PointCloud2) string); ok1 {
  167. RuleOfCh64wRLslidarPointCloud1 = append(RuleOfCh64wRLslidarPointCloud1, f)
  168. goto JudgeDone
  169. }
  170. if f, ok3 := rule.(func(shareVars *sync.Map, data *sensor_msgs.PointCloud2) string); ok3 {
  171. RuleOfCh64wRLslidarPointCloud3 = append(RuleOfCh64wRLslidarPointCloud3, f)
  172. goto JudgeDone
  173. }
  174. log(triggerLocalPath)
  175. continue
  176. } else if TopicOfCh64wRScan == topic2 { //9
  177. if f, ok1 := rule.(func(data *sensor_msgs.LaserScan) string); ok1 {
  178. RuleOfCh64wRScan1 = append(RuleOfCh64wRScan1, f)
  179. goto JudgeDone
  180. }
  181. if f, ok3 := rule.(func(shareVars *sync.Map, data *sensor_msgs.LaserScan) string); ok3 {
  182. RuleOfCh64wRScan3 = append(RuleOfCh64wRScan3, f)
  183. goto JudgeDone
  184. }
  185. log(triggerLocalPath)
  186. continue
  187. } else if TopicOfCicvLidarclusterMovingObjects == topic2 { //10
  188. if f, ok1 := rule.(func(data *pjisuv_msgs.PerceptionCicvMovingObjects) string); ok1 {
  189. RuleOfCicvLidarclusterMovingObjects1 = append(RuleOfCicvLidarclusterMovingObjects1, f)
  190. goto JudgeDone
  191. }
  192. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.PerceptionCicvMovingObjects) string); ok3 {
  193. RuleOfCicvLidarclusterMovingObjects3 = append(RuleOfCicvLidarclusterMovingObjects3, f)
  194. goto JudgeDone
  195. }
  196. log(triggerLocalPath)
  197. continue
  198. } else if TopicOfCicvAmrTrajectory == topic2 { //11
  199. if f, ok1 := rule.(func(data *pjisuv_msgs.Trajectory) string); ok1 {
  200. RuleOfCicvAmrTrajectory1 = append(RuleOfCicvAmrTrajectory1, f)
  201. goto JudgeDone
  202. }
  203. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.Trajectory) string); ok3 {
  204. RuleOfCicvAmrTrajectory3 = append(RuleOfCicvAmrTrajectory3, f)
  205. goto JudgeDone
  206. }
  207. log(triggerLocalPath)
  208. continue
  209. } else if TopicOfCicvLocation == topic2 { //12
  210. if f, ok1 := rule.(func(data *pjisuv_msgs.PerceptionLocalization) string); ok1 {
  211. RuleOfCicvLocation1 = append(RuleOfCicvLocation1, f)
  212. goto JudgeDone
  213. }
  214. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string); ok3 {
  215. RuleOfCicvLocation3 = append(RuleOfCicvLocation3, f)
  216. goto JudgeDone
  217. }
  218. log(triggerLocalPath)
  219. continue
  220. } else if TopicOfCloudClusters == topic2 { //13
  221. if f, ok1 := rule.(func(data *pjisuv_msgs.AutowareCloudClusterArray) string); ok1 {
  222. RuleOfCloudClusters1 = append(RuleOfCloudClusters1, f)
  223. goto JudgeDone
  224. }
  225. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.AutowareCloudClusterArray) string); ok3 {
  226. RuleOfCloudClusters3 = append(RuleOfCloudClusters3, f)
  227. goto JudgeDone
  228. }
  229. log(triggerLocalPath)
  230. continue
  231. } else if TopicOfHeartbeatInfo == topic2 { //14
  232. if f, ok1 := rule.(func(data *pjisuv_msgs.HeartBeatInfo) string); ok1 {
  233. RuleOfHeartbeatInfo1 = append(RuleOfHeartbeatInfo1, f)
  234. goto JudgeDone
  235. }
  236. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.HeartBeatInfo) string); ok3 {
  237. RuleOfHeartbeatInfo3 = append(RuleOfHeartbeatInfo3, f)
  238. goto JudgeDone
  239. }
  240. log(triggerLocalPath)
  241. continue
  242. } else if TopicOfLidarPretreatmentCost == topic2 { //15
  243. if f, ok1 := rule.(func(data *geometry_msgs.Vector3Stamped) string); ok1 {
  244. RuleOfLidarPretreatmentCost1 = append(RuleOfLidarPretreatmentCost1, f)
  245. goto JudgeDone
  246. }
  247. if f, ok3 := rule.(func(shareVars *sync.Map, data *geometry_msgs.Vector3Stamped) string); ok3 {
  248. RuleOfLidarPretreatmentCost3 = append(RuleOfLidarPretreatmentCost3, f)
  249. goto JudgeDone
  250. }
  251. log(triggerLocalPath)
  252. continue
  253. } else if TopicOfLidarPretreatmentOdometry == topic2 { //16
  254. if f, ok1 := rule.(func(data *nav_msgs.Odometry) string); ok1 {
  255. RuleOfLidarPretreatmentOdometry1 = append(RuleOfLidarPretreatmentOdometry1, f)
  256. goto JudgeDone
  257. }
  258. if f, ok3 := rule.(func(shareVars *sync.Map, data *nav_msgs.Odometry) string); ok3 {
  259. RuleOfLidarPretreatmentOdometry3 = append(RuleOfLidarPretreatmentOdometry3, f)
  260. goto JudgeDone
  261. }
  262. log(triggerLocalPath)
  263. continue
  264. } else if TopicOfLidarRoi == topic2 { //17
  265. if f, ok1 := rule.(func(data *geometry_msgs.PolygonStamped) string); ok1 {
  266. RuleOfLidarRoi1 = append(RuleOfLidarRoi1, f)
  267. goto JudgeDone
  268. }
  269. if f, ok3 := rule.(func(shareVars *sync.Map, data *geometry_msgs.PolygonStamped) string); ok3 {
  270. RuleOfLidarRoi3 = append(RuleOfLidarRoi3, f)
  271. goto JudgeDone
  272. }
  273. log(triggerLocalPath)
  274. continue
  275. } else if TopicOfLine1 == topic2 { //18
  276. if f, ok1 := rule.(func(data *nav_msgs.Path) string); ok1 {
  277. RuleOfLine11 = append(RuleOfLine11, f)
  278. goto JudgeDone
  279. }
  280. if f, ok3 := rule.(func(shareVars *sync.Map, data *nav_msgs.Path) string); ok3 {
  281. RuleOfLine13 = append(RuleOfLine13, f)
  282. goto JudgeDone
  283. }
  284. log(triggerLocalPath)
  285. continue
  286. } else if TopicOfLine2 == topic2 { //19
  287. if f, ok1 := rule.(func(data *nav_msgs.Path) string); ok1 {
  288. RuleOfLine21 = append(RuleOfLine21, f)
  289. goto JudgeDone
  290. }
  291. if f, ok3 := rule.(func(shareVars *sync.Map, data *nav_msgs.Path) string); ok3 {
  292. RuleOfLine23 = append(RuleOfLine23, f)
  293. goto JudgeDone
  294. }
  295. log(triggerLocalPath)
  296. continue
  297. } else if TopicOfMapPolygon == topic2 { //20
  298. if f, ok1 := rule.(func(data *pjisuv_msgs.PolygonStamped) string); ok1 {
  299. RuleOfMapPolygon1 = append(RuleOfMapPolygon1, f)
  300. goto JudgeDone
  301. }
  302. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.PolygonStamped) string); ok3 {
  303. RuleOfMapPolygon3 = append(RuleOfMapPolygon3, f)
  304. goto JudgeDone
  305. }
  306. log(triggerLocalPath)
  307. continue
  308. } else if TopicOfObstacleDisplay == topic2 { //21
  309. if f, ok1 := rule.(func(data *visualization_msgs.MarkerArray) string); ok1 {
  310. RuleOfObstacleDisplay1 = append(RuleOfObstacleDisplay1, f)
  311. goto JudgeDone
  312. }
  313. if f, ok3 := rule.(func(shareVars *sync.Map, data *visualization_msgs.MarkerArray) string); ok3 {
  314. RuleOfObstacleDisplay3 = append(RuleOfObstacleDisplay3, f)
  315. goto JudgeDone
  316. }
  317. log(triggerLocalPath)
  318. continue
  319. } else if TopicOfPjControlPub == topic2 { //22
  320. if f, ok1 := rule.(func(data *pjisuv_msgs.CommonVehicleCmd) string); ok1 {
  321. RuleOfPjControlPub1 = append(RuleOfPjControlPub1, f)
  322. goto JudgeDone
  323. }
  324. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.CommonVehicleCmd) string); ok3 {
  325. RuleOfPjControlPub3 = append(RuleOfPjControlPub3, f)
  326. goto JudgeDone
  327. }
  328. log(triggerLocalPath)
  329. continue
  330. } else if TopicOfPointsCluster == topic2 { //23
  331. if f, ok1 := rule.(func(data *sensor_msgs.PointCloud2) string); ok1 {
  332. RuleOfPointsCluster1 = append(RuleOfPointsCluster1, f)
  333. goto JudgeDone
  334. }
  335. if f, ok3 := rule.(func(shareVars *sync.Map, data *sensor_msgs.PointCloud2) string); ok3 {
  336. RuleOfPointsCluster3 = append(RuleOfPointsCluster3, f)
  337. goto JudgeDone
  338. }
  339. log(triggerLocalPath)
  340. continue
  341. } else if TopicOfPointsConcat == topic2 { //24
  342. if f, ok1 := rule.(func(data *sensor_msgs.PointCloud2) string); ok1 {
  343. RuleOfPointsConcat1 = append(RuleOfPointsConcat1, f)
  344. goto JudgeDone
  345. }
  346. if f, ok3 := rule.(func(shareVars *sync.Map, data *sensor_msgs.PointCloud2) string); ok3 {
  347. RuleOfPointsConcat3 = append(RuleOfPointsConcat3, f)
  348. goto JudgeDone
  349. }
  350. log(triggerLocalPath)
  351. continue
  352. } else if TopicOfReferenceDisplay == topic2 { //25
  353. if f, ok1 := rule.(func(data *nav_msgs.Path) string); ok1 {
  354. RuleOfReferenceDisplay1 = append(RuleOfReferenceDisplay1, f)
  355. goto JudgeDone
  356. }
  357. if f, ok3 := rule.(func(shareVars *sync.Map, data *nav_msgs.Path) string); ok3 {
  358. RuleOfReferenceDisplay3 = append(RuleOfReferenceDisplay3, f)
  359. goto JudgeDone
  360. }
  361. log(triggerLocalPath)
  362. continue
  363. } else if TopicOfReferenceTrajectory == topic2 { //26
  364. if f, ok1 := rule.(func(data *pjisuv_msgs.Trajectory) string); ok1 {
  365. RuleOfReferenceTrajectory1 = append(RuleOfReferenceTrajectory1, f)
  366. goto JudgeDone
  367. }
  368. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.Trajectory) string); ok3 {
  369. RuleOfReferenceTrajectory3 = append(RuleOfReferenceTrajectory3, f)
  370. goto JudgeDone
  371. }
  372. log(triggerLocalPath)
  373. continue
  374. } else if TopicOfRoiPoints == topic2 { //27
  375. if f, ok1 := rule.(func(data *sensor_msgs.PointCloud2) string); ok1 {
  376. RuleOfRoiPoints1 = append(RuleOfRoiPoints1, f)
  377. goto JudgeDone
  378. }
  379. if f, ok3 := rule.(func(shareVars *sync.Map, data *sensor_msgs.PointCloud2) string); ok3 {
  380. RuleOfRoiPoints3 = append(RuleOfRoiPoints3, f)
  381. goto JudgeDone
  382. }
  383. log(triggerLocalPath)
  384. continue
  385. } else if TopicOfRoiPolygon == topic2 { //28
  386. if f, ok1 := rule.(func(data *nav_msgs.Path) string); ok1 {
  387. RuleOfRoiPolygon1 = append(RuleOfRoiPolygon1, f)
  388. goto JudgeDone
  389. }
  390. if f, ok3 := rule.(func(shareVars *sync.Map, data *nav_msgs.Path) string); ok3 {
  391. RuleOfRoiPolygon3 = append(RuleOfRoiPolygon3, f)
  392. goto JudgeDone
  393. }
  394. log(triggerLocalPath)
  395. continue
  396. } else if TopicOfTf == topic2 { //29
  397. if f, ok1 := rule.(func(data *tf2_msgs.TFMessage) string); ok1 {
  398. RuleOfTf1 = append(RuleOfTf1, f)
  399. goto JudgeDone
  400. }
  401. if f, ok3 := rule.(func(shareVars *sync.Map, data *tf2_msgs.TFMessage) string); ok3 {
  402. RuleOfTf3 = append(RuleOfTf3, f)
  403. goto JudgeDone
  404. }
  405. log(triggerLocalPath)
  406. continue
  407. } else if TopicOfTpperception == topic2 { //30
  408. if f, ok1 := rule.(func(data *pjisuv_msgs.PerceptionObjects) string); ok1 {
  409. RuleOfTpperception1 = append(RuleOfTpperception1, f)
  410. goto JudgeDone
  411. }
  412. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.PerceptionObjects) string); ok3 {
  413. RuleOfTpperception3 = append(RuleOfTpperception3, f)
  414. goto JudgeDone
  415. }
  416. log(triggerLocalPath)
  417. continue
  418. } else if TopicOfTpperceptionVis == topic2 { //31
  419. if f, ok1 := rule.(func(data *visualization_msgs.MarkerArray) string); ok1 {
  420. RuleOfTpperceptionVis1 = append(RuleOfTpperceptionVis1, f)
  421. goto JudgeDone
  422. }
  423. if f, ok3 := rule.(func(shareVars *sync.Map, data *visualization_msgs.MarkerArray) string); ok3 {
  424. RuleOfTpperceptionVis3 = append(RuleOfTpperceptionVis3, f)
  425. goto JudgeDone
  426. }
  427. log(triggerLocalPath)
  428. continue
  429. } else if TopicOfTprouteplan == topic2 { //32
  430. if f, ok1 := rule.(func(data *pjisuv_msgs.RoutePlan) string); ok1 {
  431. RuleOfTprouteplan1 = append(RuleOfTprouteplan1, f)
  432. goto JudgeDone
  433. }
  434. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.RoutePlan) string); ok3 {
  435. RuleOfTprouteplan3 = append(RuleOfTprouteplan3, f)
  436. goto JudgeDone
  437. }
  438. log(triggerLocalPath)
  439. continue
  440. } else if TopicOfTrajectoryDisplay == topic2 { //33
  441. if f, ok1 := rule.(func(data *nav_msgs.Path) string); ok1 {
  442. RuleOfTrajectoryDisplay1 = append(RuleOfTrajectoryDisplay1, f)
  443. goto JudgeDone
  444. }
  445. if f, ok3 := rule.(func(shareVars *sync.Map, data *nav_msgs.Path) string); ok3 {
  446. RuleOfTrajectoryDisplay3 = append(RuleOfTrajectoryDisplay3, f)
  447. goto JudgeDone
  448. }
  449. log(triggerLocalPath)
  450. continue
  451. } else if TopicOfUngroundCloudpoints == topic2 { //34
  452. if f, ok1 := rule.(func(data *sensor_msgs.PointCloud2) string); ok1 {
  453. RuleOfUngroundCloudpoints1 = append(RuleOfUngroundCloudpoints1, f)
  454. goto JudgeDone
  455. }
  456. if f, ok3 := rule.(func(shareVars *sync.Map, data *sensor_msgs.PointCloud2) string); ok3 {
  457. RuleOfUngroundCloudpoints3 = append(RuleOfUngroundCloudpoints3, f)
  458. goto JudgeDone
  459. }
  460. log(triggerLocalPath)
  461. continue
  462. } else if TopicOfCameraImage == topic2 { //35
  463. if f, ok1 := rule.(func(data *sensor_msgs.Image) string); ok1 {
  464. RuleOfCameraImage1 = append(RuleOfCameraImage1, f)
  465. goto JudgeDone
  466. }
  467. if f, ok3 := rule.(func(shareVars *sync.Map, data *sensor_msgs.Image) string); ok3 {
  468. RuleOfCameraImage3 = append(RuleOfCameraImage3, f)
  469. goto JudgeDone
  470. }
  471. log(triggerLocalPath)
  472. continue
  473. } else if TopicOfDataRead == topic2 { //36
  474. if f, ok1 := rule.(func(data *pjisuv_msgs.Retrieval) string); ok1 {
  475. RuleOfDataRead1 = append(RuleOfDataRead1, f)
  476. goto JudgeDone
  477. }
  478. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.Retrieval) string); ok3 {
  479. RuleOfDataRead3 = append(RuleOfDataRead3, f)
  480. goto JudgeDone
  481. }
  482. log(triggerLocalPath)
  483. continue
  484. } else if TopicOfPjiGps == topic2 { //37
  485. if f, ok1 := rule.(func(data *pjisuv_msgs.PerceptionLocalization) string); ok1 {
  486. RuleOfPjiGps1 = append(RuleOfPjiGps1, f)
  487. goto JudgeDone
  488. }
  489. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string); ok3 {
  490. RuleOfPjiGps3 = append(RuleOfPjiGps3, f)
  491. goto JudgeDone
  492. }
  493. log(triggerLocalPath)
  494. continue
  495. } else if TopicOfFaultInfo == topic2 { //38
  496. if f, ok1 := rule.(func(data *pjisuv_msgs.FaultVec) string); ok1 {
  497. RuleOfFaultInfo1 = append(RuleOfFaultInfo1, f)
  498. goto JudgeDone
  499. }
  500. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.FaultVec) string); ok3 {
  501. RuleOfFaultInfo3 = append(RuleOfFaultInfo3, f)
  502. goto JudgeDone
  503. }
  504. log(triggerLocalPath)
  505. continue
  506. } else if TopicOfPjVehicleFdbPub == topic2 { //39
  507. if f, ok1 := rule.(func(data *pjisuv_msgs.VehicleFdb) string); ok1 {
  508. RuleOfPjVehicleFdbPub1 = append(RuleOfPjVehicleFdbPub1, f)
  509. goto JudgeDone
  510. }
  511. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.VehicleFdb) string); ok3 {
  512. RuleOfPjVehicleFdbPub3 = append(RuleOfPjVehicleFdbPub3, f)
  513. goto JudgeDone
  514. }
  515. log(triggerLocalPath)
  516. continue
  517. } else {
  518. c_log.GlobalLogger.Error("未知的topic:", topic2)
  519. continue
  520. }
  521. JudgeDone:
  522. label, err := open.Lookup("Label")
  523. if err != nil {
  524. c_log.GlobalLogger.Error("加载本地插件 ", triggerLocalPath, " 中的 Label 方法失败。", err)
  525. continue
  526. }
  527. labelFunc := label.(func() string)
  528. labelString := labelFunc()
  529. LabelMapTriggerId.Store(labelString, triggerId)
  530. success++
  531. }
  532. c_log.GlobalLogger.Info("一共应加载", len(config.PlatformConfig.TaskTriggers), "个触发器,实际加载 ", success, " 个触发器。")
  533. }
  534. func log(triggerLocalPath string) {
  535. c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的 Rule 方法参数格式不正确。")
  536. }