trigger_init.go 27 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698
  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. entity "cicv-data-closedloop/pjisuv_param"
  8. "cicv-data-closedloop/pjisuv_ticker"
  9. "github.com/bluenviron/goroslib/v2/pkg/msgs/geometry_msgs"
  10. "github.com/bluenviron/goroslib/v2/pkg/msgs/nav_msgs"
  11. "github.com/bluenviron/goroslib/v2/pkg/msgs/sensor_msgs"
  12. "github.com/bluenviron/goroslib/v2/pkg/msgs/tf2_msgs"
  13. "github.com/bluenviron/goroslib/v2/pkg/msgs/visualization_msgs"
  14. "plugin"
  15. "slices"
  16. "sync"
  17. )
  18. func InitTriggerConfig() {
  19. config.OssMutex.Lock()
  20. defer config.OssMutex.Unlock()
  21. triggerLocalPathsMapTriggerId := make(map[string]string)
  22. c_log.GlobalLogger.Info("主节点加载触发器插件 - 开始。")
  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. success := 0
  58. // 加载所有触发器的文件
  59. for triggerLocalPath, triggerId := range triggerLocalPathsMapTriggerId {
  60. // 载入插件到数组
  61. open, err := plugin.Open(triggerLocalPath)
  62. if err != nil {
  63. c_log.GlobalLogger.Error("加载本地插件", triggerLocalPath, "失败。", err)
  64. continue
  65. }
  66. topic0, err := open.Lookup("Topic")
  67. if err != nil {
  68. c_log.GlobalLogger.Error("加载本地插件", triggerLocalPath, "中的Topic方法失败。", err)
  69. continue
  70. }
  71. topic1, ok := topic0.(func() string)
  72. if ok != true {
  73. c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func() string):", err)
  74. continue
  75. }
  76. topic2 := topic1()
  77. rule, err := open.Lookup("Rule")
  78. if err != nil {
  79. c_log.GlobalLogger.Error("加载本地插件", triggerLocalPath, "中的Rule方法失败。", err)
  80. continue
  81. }
  82. if pjisuv_ticker.TickerTopic == topic2 { // 定时任务触发器
  83. if f, ok1 := rule.(func(shareVars *sync.Map) string); ok1 {
  84. RuleOfCicvTicker = append(RuleOfCicvTicker, f)
  85. goto JudgeDone
  86. }
  87. log(triggerLocalPath)
  88. continue
  89. } else if TopicOfAmrPose == topic2 { //1
  90. if f, ok1 := rule.(func(data *visualization_msgs.MarkerArray) string); ok1 {
  91. RuleOfAmrPose1 = append(RuleOfAmrPose1, f)
  92. goto JudgeDone
  93. }
  94. if f, ok2 := rule.(func(data *visualization_msgs.MarkerArray, param *entity.PjisuvParam) string); ok2 {
  95. RuleOfAmrPose2 = append(RuleOfAmrPose2, f)
  96. goto JudgeDone
  97. }
  98. if f, ok3 := rule.(func(shareVars *sync.Map, data *visualization_msgs.MarkerArray) string); ok3 {
  99. RuleOfAmrPose3 = append(RuleOfAmrPose3, f)
  100. goto JudgeDone
  101. }
  102. log(triggerLocalPath)
  103. continue
  104. } else if TopicOfBoundingBoxesFast == topic2 { //2
  105. if f, ok1 := rule.(func(data *pjisuv_msgs.BoundingBoxArray) string); ok1 {
  106. RuleOfBoundingBoxesFast1 = append(RuleOfBoundingBoxesFast1, f)
  107. goto JudgeDone
  108. }
  109. if f, ok2 := rule.(func(data *pjisuv_msgs.BoundingBoxArray, param *entity.PjisuvParam) string); ok2 {
  110. RuleOfBoundingBoxesFast2 = append(RuleOfBoundingBoxesFast2, 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, ok2 := rule.(func(data *pjisuv_msgs.FaultVec, param *entity.PjisuvParam) string); ok2 {
  125. RuleOfCameraFault2 = append(RuleOfCameraFault2, f)
  126. goto JudgeDone
  127. }
  128. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.FaultVec) string); ok3 {
  129. RuleOfCameraFault3 = append(RuleOfCameraFault3, f)
  130. goto JudgeDone
  131. }
  132. log(triggerLocalPath)
  133. continue
  134. } else if TopicOfCanData == topic2 { //4
  135. if f, ok1 := rule.(func(data *pjisuv_msgs.Frame) string); ok1 {
  136. RuleOfCanData1 = append(RuleOfCanData1, f)
  137. goto JudgeDone
  138. }
  139. if f, ok2 := rule.(func(data *pjisuv_msgs.Frame, param *entity.PjisuvParam) string); ok2 {
  140. RuleOfCanData2 = append(RuleOfCanData2, f)
  141. goto JudgeDone
  142. }
  143. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.Frame) string); ok3 {
  144. RuleOfCanData3 = append(RuleOfCanData3, f)
  145. goto JudgeDone
  146. }
  147. log(triggerLocalPath)
  148. continue
  149. } else if TopicOfCh128x1LslidarPointCloud == topic2 { //5
  150. if f, ok1 := rule.(func(data *sensor_msgs.PointCloud2) string); ok1 {
  151. RuleOfCh128x1LslidarPointCloud1 = append(RuleOfCh128x1LslidarPointCloud1, f)
  152. goto JudgeDone
  153. }
  154. if f, ok2 := rule.(func(data *sensor_msgs.PointCloud2, param *entity.PjisuvParam) string); ok2 {
  155. RuleOfCh128x1LslidarPointCloud2 = append(RuleOfCh128x1LslidarPointCloud2, f)
  156. goto JudgeDone
  157. }
  158. if f, ok3 := rule.(func(shareVars *sync.Map, data *sensor_msgs.PointCloud2) string); ok3 {
  159. RuleOfCh128x1LslidarPointCloud3 = append(RuleOfCh128x1LslidarPointCloud3, f)
  160. goto JudgeDone
  161. }
  162. log(triggerLocalPath)
  163. continue
  164. } else if TopicOfCh64wLLslidarPointCloud == topic2 { //6
  165. if f, ok1 := rule.(func(data *sensor_msgs.PointCloud2) string); ok1 {
  166. RuleOfCh64wLLslidarPointCloud1 = append(RuleOfCh64wLLslidarPointCloud1, f)
  167. goto JudgeDone
  168. }
  169. if f, ok2 := rule.(func(data *sensor_msgs.PointCloud2, param *entity.PjisuvParam) string); ok2 {
  170. RuleOfCh64wLLslidarPointCloud2 = append(RuleOfCh64wLLslidarPointCloud2, f)
  171. goto JudgeDone
  172. }
  173. if f, ok3 := rule.(func(shareVars *sync.Map, data *sensor_msgs.PointCloud2) string); ok3 {
  174. RuleOfCh64wLLslidarPointCloud3 = append(RuleOfCh64wLLslidarPointCloud3, f)
  175. goto JudgeDone
  176. }
  177. log(triggerLocalPath)
  178. continue
  179. } else if TopicOfCh64wLScan == topic2 { //7
  180. if f, ok1 := rule.(func(data *sensor_msgs.LaserScan) string); ok1 {
  181. RuleOfCh64wLScan1 = append(RuleOfCh64wLScan1, f)
  182. goto JudgeDone
  183. }
  184. if f, ok2 := rule.(func(data *sensor_msgs.LaserScan, param *entity.PjisuvParam) string); ok2 {
  185. RuleOfCh64wLScan2 = append(RuleOfCh64wLScan2, f)
  186. goto JudgeDone
  187. }
  188. if f, ok3 := rule.(func(shareVars *sync.Map, data *sensor_msgs.LaserScan) string); ok3 {
  189. RuleOfCh64wLScan3 = append(RuleOfCh64wLScan3, f)
  190. goto JudgeDone
  191. }
  192. log(triggerLocalPath)
  193. continue
  194. } else if TopicOfCh64wRLslidarPointCloud == topic2 { //8
  195. if f, ok1 := rule.(func(data *sensor_msgs.PointCloud2) string); ok1 {
  196. RuleOfCh64wRLslidarPointCloud1 = append(RuleOfCh64wRLslidarPointCloud1, f)
  197. goto JudgeDone
  198. }
  199. if f, ok2 := rule.(func(data *sensor_msgs.PointCloud2, param *entity.PjisuvParam) string); ok2 {
  200. RuleOfCh64wRLslidarPointCloud2 = append(RuleOfCh64wRLslidarPointCloud2, f)
  201. goto JudgeDone
  202. }
  203. if f, ok3 := rule.(func(shareVars *sync.Map, data *sensor_msgs.PointCloud2) string); ok3 {
  204. RuleOfCh64wRLslidarPointCloud3 = append(RuleOfCh64wRLslidarPointCloud3, f)
  205. goto JudgeDone
  206. }
  207. log(triggerLocalPath)
  208. continue
  209. } else if TopicOfCh64wRScan == topic2 { //9
  210. if f, ok1 := rule.(func(data *sensor_msgs.LaserScan) string); ok1 {
  211. RuleOfCh64wRScan1 = append(RuleOfCh64wRScan1, f)
  212. goto JudgeDone
  213. }
  214. if f, ok2 := rule.(func(data *sensor_msgs.LaserScan, param *entity.PjisuvParam) string); ok2 {
  215. RuleOfCh64wRScan2 = append(RuleOfCh64wRScan2, f)
  216. goto JudgeDone
  217. }
  218. if f, ok3 := rule.(func(shareVars *sync.Map, data *sensor_msgs.LaserScan) string); ok3 {
  219. RuleOfCh64wRScan3 = append(RuleOfCh64wRScan3, f)
  220. goto JudgeDone
  221. }
  222. log(triggerLocalPath)
  223. continue
  224. } else if TopicOfCicvLidarclusterMovingObjects == topic2 { //10
  225. if f, ok1 := rule.(func(data *pjisuv_msgs.PerceptionCicvMovingObjects) string); ok1 {
  226. RuleOfCicvLidarclusterMovingObjects1 = append(RuleOfCicvLidarclusterMovingObjects1, f)
  227. goto JudgeDone
  228. }
  229. if f, ok2 := rule.(func(data *pjisuv_msgs.PerceptionCicvMovingObjects, param *entity.PjisuvParam) string); ok2 {
  230. RuleOfCicvLidarclusterMovingObjects2 = append(RuleOfCicvLidarclusterMovingObjects2, f)
  231. goto JudgeDone
  232. }
  233. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.PerceptionCicvMovingObjects) string); ok3 {
  234. RuleOfCicvLidarclusterMovingObjects3 = append(RuleOfCicvLidarclusterMovingObjects3, f)
  235. goto JudgeDone
  236. }
  237. log(triggerLocalPath)
  238. continue
  239. } else if TopicOfCicvAmrTrajectory == topic2 { //11
  240. if f, ok1 := rule.(func(data *pjisuv_msgs.Trajectory) string); ok1 {
  241. RuleOfCicvAmrTrajectory1 = append(RuleOfCicvAmrTrajectory1, f)
  242. goto JudgeDone
  243. }
  244. if f, ok2 := rule.(func(data *pjisuv_msgs.Trajectory, param *entity.PjisuvParam) string); ok2 {
  245. RuleOfCicvAmrTrajectory2 = append(RuleOfCicvAmrTrajectory2, f)
  246. goto JudgeDone
  247. }
  248. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.Trajectory) string); ok3 {
  249. RuleOfCicvAmrTrajectory3 = append(RuleOfCicvAmrTrajectory3, f)
  250. goto JudgeDone
  251. }
  252. log(triggerLocalPath)
  253. continue
  254. } else if TopicOfCicvLocation == topic2 { //12
  255. if f, ok1 := rule.(func(data *pjisuv_msgs.PerceptionLocalization) string); ok1 {
  256. RuleOfCicvLocation1 = append(RuleOfCicvLocation1, f)
  257. goto JudgeDone
  258. }
  259. if f, ok2 := rule.(func(data *pjisuv_msgs.PerceptionLocalization, param *entity.PjisuvParam) string); ok2 {
  260. RuleOfCicvLocation2 = append(RuleOfCicvLocation2, f)
  261. goto JudgeDone
  262. }
  263. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string); ok3 {
  264. RuleOfCicvLocation3 = append(RuleOfCicvLocation3, f)
  265. goto JudgeDone
  266. }
  267. log(triggerLocalPath)
  268. continue
  269. } else if TopicOfCloudClusters == topic2 { //13
  270. if f, ok1 := rule.(func(data *pjisuv_msgs.AutowareCloudClusterArray) string); ok1 {
  271. RuleOfCloudClusters1 = append(RuleOfCloudClusters1, f)
  272. goto JudgeDone
  273. }
  274. if f, ok2 := rule.(func(data *pjisuv_msgs.AutowareCloudClusterArray, param *entity.PjisuvParam) string); ok2 {
  275. RuleOfCloudClusters2 = append(RuleOfCloudClusters2, f)
  276. goto JudgeDone
  277. }
  278. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.AutowareCloudClusterArray) string); ok3 {
  279. RuleOfCloudClusters3 = append(RuleOfCloudClusters3, f)
  280. goto JudgeDone
  281. }
  282. log(triggerLocalPath)
  283. continue
  284. } else if TopicOfHeartbeatInfo == topic2 { //14
  285. if f, ok1 := rule.(func(data *pjisuv_msgs.HeartBeatInfo) string); ok1 {
  286. RuleOfHeartbeatInfo1 = append(RuleOfHeartbeatInfo1, f)
  287. goto JudgeDone
  288. }
  289. if f, ok2 := rule.(func(data *pjisuv_msgs.HeartBeatInfo, param *entity.PjisuvParam) string); ok2 {
  290. RuleOfHeartbeatInfo2 = append(RuleOfHeartbeatInfo2, f)
  291. goto JudgeDone
  292. }
  293. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.HeartBeatInfo) string); ok3 {
  294. RuleOfHeartbeatInfo3 = append(RuleOfHeartbeatInfo3, f)
  295. goto JudgeDone
  296. }
  297. log(triggerLocalPath)
  298. continue
  299. } else if TopicOfLidarPretreatmentCost == topic2 { //15
  300. if f, ok1 := rule.(func(data *geometry_msgs.Vector3Stamped) string); ok1 {
  301. RuleOfLidarPretreatmentCost1 = append(RuleOfLidarPretreatmentCost1, f)
  302. goto JudgeDone
  303. }
  304. if f, ok2 := rule.(func(data *geometry_msgs.Vector3Stamped, param *entity.PjisuvParam) string); ok2 {
  305. RuleOfLidarPretreatmentCost2 = append(RuleOfLidarPretreatmentCost2, f)
  306. goto JudgeDone
  307. }
  308. if f, ok3 := rule.(func(shareVars *sync.Map, data *geometry_msgs.Vector3Stamped) string); ok3 {
  309. RuleOfLidarPretreatmentCost3 = append(RuleOfLidarPretreatmentCost3, f)
  310. goto JudgeDone
  311. }
  312. log(triggerLocalPath)
  313. continue
  314. } else if TopicOfLidarPretreatmentOdometry == topic2 { //16
  315. if f, ok1 := rule.(func(data *nav_msgs.Odometry) string); ok1 {
  316. RuleOfLidarPretreatmentOdometry1 = append(RuleOfLidarPretreatmentOdometry1, f)
  317. goto JudgeDone
  318. }
  319. if f, ok2 := rule.(func(data *nav_msgs.Odometry, param *entity.PjisuvParam) string); ok2 {
  320. RuleOfLidarPretreatmentOdometry2 = append(RuleOfLidarPretreatmentOdometry2, f)
  321. goto JudgeDone
  322. }
  323. if f, ok3 := rule.(func(shareVars *sync.Map, data *nav_msgs.Odometry) string); ok3 {
  324. RuleOfLidarPretreatmentOdometry3 = append(RuleOfLidarPretreatmentOdometry3, f)
  325. goto JudgeDone
  326. }
  327. log(triggerLocalPath)
  328. continue
  329. } else if TopicOfLidarRoi == topic2 { //17
  330. if f, ok1 := rule.(func(data *geometry_msgs.PolygonStamped) string); ok1 {
  331. RuleOfLidarRoi1 = append(RuleOfLidarRoi1, f)
  332. goto JudgeDone
  333. }
  334. if f, ok2 := rule.(func(data *geometry_msgs.PolygonStamped, param *entity.PjisuvParam) string); ok2 {
  335. RuleOfLidarRoi2 = append(RuleOfLidarRoi2, f)
  336. goto JudgeDone
  337. }
  338. if f, ok3 := rule.(func(shareVars *sync.Map, data *geometry_msgs.PolygonStamped) string); ok3 {
  339. RuleOfLidarRoi3 = append(RuleOfLidarRoi3, f)
  340. goto JudgeDone
  341. }
  342. log(triggerLocalPath)
  343. continue
  344. } else if TopicOfLine1 == topic2 { //18
  345. if f, ok1 := rule.(func(data *nav_msgs.Path) string); ok1 {
  346. RuleOfLine11 = append(RuleOfLine11, f)
  347. goto JudgeDone
  348. }
  349. if f, ok2 := rule.(func(data *nav_msgs.Path, param *entity.PjisuvParam) string); ok2 {
  350. RuleOfLine12 = append(RuleOfLine12, f)
  351. goto JudgeDone
  352. }
  353. if f, ok3 := rule.(func(shareVars *sync.Map, data *nav_msgs.Path) string); ok3 {
  354. RuleOfLine13 = append(RuleOfLine13, f)
  355. goto JudgeDone
  356. }
  357. log(triggerLocalPath)
  358. continue
  359. } else if TopicOfLine2 == topic2 { //19
  360. if f, ok1 := rule.(func(data *nav_msgs.Path) string); ok1 {
  361. RuleOfLine21 = append(RuleOfLine21, f)
  362. goto JudgeDone
  363. }
  364. if f, ok2 := rule.(func(data *nav_msgs.Path, param *entity.PjisuvParam) string); ok2 {
  365. RuleOfLine22 = append(RuleOfLine22, f)
  366. goto JudgeDone
  367. }
  368. if f, ok3 := rule.(func(shareVars *sync.Map, data *nav_msgs.Path) string); ok3 {
  369. RuleOfLine23 = append(RuleOfLine23, f)
  370. goto JudgeDone
  371. }
  372. log(triggerLocalPath)
  373. continue
  374. } else if TopicOfMapPolygon == topic2 { //20
  375. if f, ok1 := rule.(func(data *pjisuv_msgs.PolygonStamped) string); ok1 {
  376. RuleOfMapPolygon1 = append(RuleOfMapPolygon1, f)
  377. goto JudgeDone
  378. }
  379. if f, ok2 := rule.(func(data *pjisuv_msgs.PolygonStamped, param *entity.PjisuvParam) string); ok2 {
  380. RuleOfMapPolygon2 = append(RuleOfMapPolygon2, f)
  381. goto JudgeDone
  382. }
  383. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.PolygonStamped) string); ok3 {
  384. RuleOfMapPolygon3 = append(RuleOfMapPolygon3, f)
  385. goto JudgeDone
  386. }
  387. log(triggerLocalPath)
  388. continue
  389. } else if TopicOfObstacleDisplay == topic2 { //21
  390. if f, ok1 := rule.(func(data *visualization_msgs.MarkerArray) string); ok1 {
  391. RuleOfObstacleDisplay1 = append(RuleOfObstacleDisplay1, f)
  392. goto JudgeDone
  393. }
  394. if f, ok2 := rule.(func(data *visualization_msgs.MarkerArray, param *entity.PjisuvParam) string); ok2 {
  395. RuleOfObstacleDisplay2 = append(RuleOfObstacleDisplay2, f)
  396. goto JudgeDone
  397. }
  398. if f, ok3 := rule.(func(shareVars *sync.Map, data *visualization_msgs.MarkerArray) string); ok3 {
  399. RuleOfObstacleDisplay3 = append(RuleOfObstacleDisplay3, f)
  400. goto JudgeDone
  401. }
  402. log(triggerLocalPath)
  403. continue
  404. } else if TopicOfPjControlPub == topic2 { //22
  405. if f, ok1 := rule.(func(data *pjisuv_msgs.CommonVehicleCmd) string); ok1 {
  406. RuleOfPjControlPub1 = append(RuleOfPjControlPub1, f)
  407. goto JudgeDone
  408. }
  409. if f, ok2 := rule.(func(data *pjisuv_msgs.CommonVehicleCmd, param *entity.PjisuvParam) string); ok2 {
  410. RuleOfPjControlPub2 = append(RuleOfPjControlPub2, f)
  411. goto JudgeDone
  412. }
  413. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.CommonVehicleCmd) string); ok3 {
  414. RuleOfPjControlPub3 = append(RuleOfPjControlPub3, f)
  415. goto JudgeDone
  416. }
  417. log(triggerLocalPath)
  418. continue
  419. } else if TopicOfPointsCluster == topic2 { //23
  420. if f, ok1 := rule.(func(data *sensor_msgs.PointCloud2) string); ok1 {
  421. RuleOfPointsCluster1 = append(RuleOfPointsCluster1, f)
  422. goto JudgeDone
  423. }
  424. if f, ok2 := rule.(func(data *sensor_msgs.PointCloud2, param *entity.PjisuvParam) string); ok2 {
  425. RuleOfPointsCluster2 = append(RuleOfPointsCluster2, f)
  426. goto JudgeDone
  427. }
  428. if f, ok3 := rule.(func(shareVars *sync.Map, data *sensor_msgs.PointCloud2) string); ok3 {
  429. RuleOfPointsCluster3 = append(RuleOfPointsCluster3, f)
  430. goto JudgeDone
  431. }
  432. log(triggerLocalPath)
  433. continue
  434. } else if TopicOfPointsConcat == topic2 { //24
  435. if f, ok1 := rule.(func(data *sensor_msgs.PointCloud2) string); ok1 {
  436. RuleOfPointsConcat1 = append(RuleOfPointsConcat1, f)
  437. goto JudgeDone
  438. }
  439. if f, ok2 := rule.(func(data *sensor_msgs.PointCloud2, param *entity.PjisuvParam) string); ok2 {
  440. RuleOfPointsConcat2 = append(RuleOfPointsConcat2, f)
  441. goto JudgeDone
  442. }
  443. if f, ok3 := rule.(func(shareVars *sync.Map, data *sensor_msgs.PointCloud2) string); ok3 {
  444. RuleOfPointsConcat3 = append(RuleOfPointsConcat3, f)
  445. goto JudgeDone
  446. }
  447. log(triggerLocalPath)
  448. continue
  449. } else if TopicOfReferenceDisplay == topic2 { //25
  450. if f, ok1 := rule.(func(data *nav_msgs.Path) string); ok1 {
  451. RuleOfReferenceDisplay1 = append(RuleOfReferenceDisplay1, f)
  452. goto JudgeDone
  453. }
  454. if f, ok2 := rule.(func(data *nav_msgs.Path, param *entity.PjisuvParam) string); ok2 {
  455. RuleOfReferenceDisplay2 = append(RuleOfReferenceDisplay2, f)
  456. goto JudgeDone
  457. }
  458. if f, ok3 := rule.(func(shareVars *sync.Map, data *nav_msgs.Path) string); ok3 {
  459. RuleOfReferenceDisplay3 = append(RuleOfReferenceDisplay3, f)
  460. goto JudgeDone
  461. }
  462. log(triggerLocalPath)
  463. continue
  464. } else if TopicOfReferenceTrajectory == topic2 { //26
  465. if f, ok1 := rule.(func(data *pjisuv_msgs.Trajectory) string); ok1 {
  466. RuleOfReferenceTrajectory1 = append(RuleOfReferenceTrajectory1, f)
  467. goto JudgeDone
  468. }
  469. if f, ok2 := rule.(func(data *pjisuv_msgs.Trajectory, param *entity.PjisuvParam) string); ok2 {
  470. RuleOfReferenceTrajectory2 = append(RuleOfReferenceTrajectory2, f)
  471. goto JudgeDone
  472. }
  473. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.Trajectory) string); ok3 {
  474. RuleOfReferenceTrajectory3 = append(RuleOfReferenceTrajectory3, f)
  475. goto JudgeDone
  476. }
  477. log(triggerLocalPath)
  478. continue
  479. } else if TopicOfRoiPoints == topic2 { //27
  480. if f, ok1 := rule.(func(data *sensor_msgs.PointCloud2) string); ok1 {
  481. RuleOfRoiPoints1 = append(RuleOfRoiPoints1, f)
  482. goto JudgeDone
  483. }
  484. if f, ok2 := rule.(func(data *sensor_msgs.PointCloud2, param *entity.PjisuvParam) string); ok2 {
  485. RuleOfRoiPoints2 = append(RuleOfRoiPoints2, f)
  486. goto JudgeDone
  487. }
  488. if f, ok3 := rule.(func(shareVars *sync.Map, data *sensor_msgs.PointCloud2) string); ok3 {
  489. RuleOfRoiPoints3 = append(RuleOfRoiPoints3, f)
  490. goto JudgeDone
  491. }
  492. log(triggerLocalPath)
  493. continue
  494. } else if TopicOfRoiPolygon == topic2 { //28
  495. if f, ok1 := rule.(func(data *nav_msgs.Path) string); ok1 {
  496. RuleOfRoiPolygon1 = append(RuleOfRoiPolygon1, f)
  497. goto JudgeDone
  498. }
  499. if f, ok2 := rule.(func(data *nav_msgs.Path, param *entity.PjisuvParam) string); ok2 {
  500. RuleOfRoiPolygon2 = append(RuleOfRoiPolygon2, f)
  501. goto JudgeDone
  502. }
  503. if f, ok3 := rule.(func(shareVars *sync.Map, data *nav_msgs.Path) string); ok3 {
  504. RuleOfRoiPolygon3 = append(RuleOfRoiPolygon3, f)
  505. goto JudgeDone
  506. }
  507. log(triggerLocalPath)
  508. continue
  509. } else if TopicOfTf == topic2 { //29
  510. if f, ok1 := rule.(func(data *tf2_msgs.TFMessage) string); ok1 {
  511. RuleOfTf1 = append(RuleOfTf1, f)
  512. goto JudgeDone
  513. }
  514. if f, ok2 := rule.(func(data *tf2_msgs.TFMessage, param *entity.PjisuvParam) string); ok2 {
  515. RuleOfTf2 = append(RuleOfTf2, f)
  516. goto JudgeDone
  517. }
  518. if f, ok3 := rule.(func(shareVars *sync.Map, data *tf2_msgs.TFMessage) string); ok3 {
  519. RuleOfTf3 = append(RuleOfTf3, f)
  520. goto JudgeDone
  521. }
  522. log(triggerLocalPath)
  523. continue
  524. } else if TopicOfTpperception == topic2 { //30
  525. if f, ok1 := rule.(func(data *pjisuv_msgs.PerceptionObjects) string); ok1 {
  526. RuleOfTpperception1 = append(RuleOfTpperception1, f)
  527. goto JudgeDone
  528. }
  529. if f, ok2 := rule.(func(data *pjisuv_msgs.PerceptionObjects, param *entity.PjisuvParam) string); ok2 {
  530. RuleOfTpperception2 = append(RuleOfTpperception2, f)
  531. goto JudgeDone
  532. }
  533. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.PerceptionObjects) string); ok3 {
  534. RuleOfTpperception3 = append(RuleOfTpperception3, f)
  535. goto JudgeDone
  536. }
  537. log(triggerLocalPath)
  538. continue
  539. } else if TopicOfTpperceptionVis == topic2 { //31
  540. if f, ok1 := rule.(func(data *visualization_msgs.MarkerArray) string); ok1 {
  541. RuleOfTpperceptionVis1 = append(RuleOfTpperceptionVis1, f)
  542. goto JudgeDone
  543. }
  544. if f, ok2 := rule.(func(data *visualization_msgs.MarkerArray, param *entity.PjisuvParam) string); ok2 {
  545. RuleOfTpperceptionVis2 = append(RuleOfTpperceptionVis2, f)
  546. goto JudgeDone
  547. }
  548. if f, ok3 := rule.(func(shareVars *sync.Map, data *visualization_msgs.MarkerArray) string); ok3 {
  549. RuleOfTpperceptionVis3 = append(RuleOfTpperceptionVis3, f)
  550. goto JudgeDone
  551. }
  552. log(triggerLocalPath)
  553. continue
  554. } else if TopicOfTprouteplan == topic2 { //32
  555. if f, ok1 := rule.(func(data *pjisuv_msgs.RoutePlan) string); ok1 {
  556. RuleOfTprouteplan1 = append(RuleOfTprouteplan1, f)
  557. goto JudgeDone
  558. }
  559. if f, ok2 := rule.(func(data *pjisuv_msgs.RoutePlan, param *entity.PjisuvParam) string); ok2 {
  560. RuleOfTprouteplan2 = append(RuleOfTprouteplan2, f)
  561. goto JudgeDone
  562. }
  563. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.RoutePlan) string); ok3 {
  564. RuleOfTprouteplan3 = append(RuleOfTprouteplan3, f)
  565. goto JudgeDone
  566. }
  567. log(triggerLocalPath)
  568. continue
  569. } else if TopicOfTrajectoryDisplay == topic2 { //33
  570. if f, ok1 := rule.(func(data *nav_msgs.Path) string); ok1 {
  571. RuleOfTrajectoryDisplay1 = append(RuleOfTrajectoryDisplay1, f)
  572. goto JudgeDone
  573. }
  574. if f, ok2 := rule.(func(data *nav_msgs.Path, param *entity.PjisuvParam) string); ok2 {
  575. RuleOfTrajectoryDisplay2 = append(RuleOfTrajectoryDisplay2, f)
  576. goto JudgeDone
  577. }
  578. if f, ok3 := rule.(func(shareVars *sync.Map, data *nav_msgs.Path) string); ok3 {
  579. RuleOfTrajectoryDisplay3 = append(RuleOfTrajectoryDisplay3, f)
  580. goto JudgeDone
  581. }
  582. log(triggerLocalPath)
  583. continue
  584. } else if TopicOfUngroundCloudpoints == topic2 { //34
  585. if f, ok1 := rule.(func(data *sensor_msgs.PointCloud2) string); ok1 {
  586. RuleOfUngroundCloudpoints1 = append(RuleOfUngroundCloudpoints1, f)
  587. goto JudgeDone
  588. }
  589. if f, ok2 := rule.(func(data *sensor_msgs.PointCloud2, param *entity.PjisuvParam) string); ok2 {
  590. RuleOfUngroundCloudpoints2 = append(RuleOfUngroundCloudpoints2, f)
  591. goto JudgeDone
  592. }
  593. if f, ok3 := rule.(func(shareVars *sync.Map, data *sensor_msgs.PointCloud2) string); ok3 {
  594. RuleOfUngroundCloudpoints3 = append(RuleOfUngroundCloudpoints3, f)
  595. goto JudgeDone
  596. }
  597. log(triggerLocalPath)
  598. continue
  599. } else if TopicOfCameraImage == topic2 { //35
  600. if f, ok1 := rule.(func(data *sensor_msgs.Image) string); ok1 {
  601. RuleOfCameraImage1 = append(RuleOfCameraImage1, f)
  602. goto JudgeDone
  603. }
  604. if f, ok2 := rule.(func(data *sensor_msgs.Image, param *entity.PjisuvParam) string); ok2 {
  605. RuleOfCameraImage2 = append(RuleOfCameraImage2, f)
  606. goto JudgeDone
  607. }
  608. if f, ok3 := rule.(func(shareVars *sync.Map, data *sensor_msgs.Image) string); ok3 {
  609. RuleOfCameraImage3 = append(RuleOfCameraImage3, f)
  610. goto JudgeDone
  611. }
  612. log(triggerLocalPath)
  613. continue
  614. } else if TopicOfDataRead == topic2 { //36
  615. if f, ok1 := rule.(func(data *pjisuv_msgs.Retrieval) string); ok1 {
  616. RuleOfDataRead1 = append(RuleOfDataRead1, f)
  617. goto JudgeDone
  618. }
  619. if f, ok2 := rule.(func(data *pjisuv_msgs.Retrieval, param *entity.PjisuvParam) string); ok2 {
  620. RuleOfDataRead2 = append(RuleOfDataRead2, f)
  621. goto JudgeDone
  622. }
  623. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.Retrieval) string); ok3 {
  624. RuleOfDataRead3 = append(RuleOfDataRead3, f)
  625. goto JudgeDone
  626. }
  627. log(triggerLocalPath)
  628. continue
  629. } else if TopicOfPjiGps == topic2 { //37
  630. if f, ok1 := rule.(func(data *pjisuv_msgs.PerceptionLocalization) string); ok1 {
  631. RuleOfPjiGps1 = append(RuleOfPjiGps1, f)
  632. goto JudgeDone
  633. }
  634. if f, ok2 := rule.(func(data *pjisuv_msgs.PerceptionLocalization, param *entity.PjisuvParam) string); ok2 {
  635. RuleOfPjiGps2 = append(RuleOfPjiGps2, f)
  636. goto JudgeDone
  637. }
  638. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string); ok3 {
  639. RuleOfPjiGps3 = append(RuleOfPjiGps3, f)
  640. goto JudgeDone
  641. }
  642. log(triggerLocalPath)
  643. continue
  644. } else if TopicOfFaultInfo == topic2 { //38
  645. if f, ok1 := rule.(func(data *pjisuv_msgs.FaultVec) string); ok1 {
  646. RuleOfFaultInfo1 = append(RuleOfFaultInfo1, f)
  647. goto JudgeDone
  648. }
  649. if f, ok2 := rule.(func(data *pjisuv_msgs.FaultVec, param *entity.PjisuvParam) string); ok2 {
  650. RuleOfFaultInfo2 = append(RuleOfFaultInfo2, f)
  651. goto JudgeDone
  652. }
  653. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.FaultVec) string); ok3 {
  654. RuleOfFaultInfo3 = append(RuleOfFaultInfo3, f)
  655. goto JudgeDone
  656. }
  657. log(triggerLocalPath)
  658. continue
  659. } else if TopicOfPjVehicleFdbPub == topic2 { //39
  660. if f, ok1 := rule.(func(data *pjisuv_msgs.VehicleFdb) string); ok1 {
  661. RuleOfPjVehicleFdbPub1 = append(RuleOfPjVehicleFdbPub1, f)
  662. goto JudgeDone
  663. }
  664. if f, ok2 := rule.(func(data *pjisuv_msgs.VehicleFdb, param *entity.PjisuvParam) string); ok2 {
  665. RuleOfPjVehicleFdbPub2 = append(RuleOfPjVehicleFdbPub2, f)
  666. goto JudgeDone
  667. }
  668. if f, ok3 := rule.(func(shareVars *sync.Map, data *pjisuv_msgs.VehicleFdb) string); ok3 {
  669. RuleOfPjVehicleFdbPub3 = append(RuleOfPjVehicleFdbPub3, f)
  670. goto JudgeDone
  671. }
  672. log(triggerLocalPath)
  673. continue
  674. } else {
  675. c_log.GlobalLogger.Error("未知的topic:", topic2)
  676. continue
  677. }
  678. JudgeDone:
  679. label, err := open.Lookup("Label")
  680. if err != nil {
  681. c_log.GlobalLogger.Error("加载本地插件 ", triggerLocalPath, " 中的 Label 方法失败。", err)
  682. continue
  683. }
  684. labelFunc := label.(func() string)
  685. labelString := labelFunc()
  686. LabelMapTriggerId.Store(labelString, triggerId)
  687. success++
  688. }
  689. c_log.GlobalLogger.Info("一共应加载", len(config.PlatformConfig.TaskTriggers), "个触发器,实际加载 ", success, " 个触发器。")
  690. }
  691. func log(triggerLocalPath string) {
  692. c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的 Rule 方法参数格式不正确。")
  693. }