trigger_init.go 21 KB

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