trigger_init.go 21 KB

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