package config import ( "cicv-data-closedloop/aarch64/pjisuv/common/config" "cicv-data-closedloop/common/config/c_log" "cicv-data-closedloop/common/entity" "cicv-data-closedloop/common/util" "cicv-data-closedloop/pjisuv_msgs" "github.com/bluenviron/goroslib/v2/pkg/msgs/geometry_msgs" "github.com/bluenviron/goroslib/v2/pkg/msgs/nav_msgs" "github.com/bluenviron/goroslib/v2/pkg/msgs/sensor_msgs" "github.com/bluenviron/goroslib/v2/pkg/msgs/tf2_msgs" "github.com/bluenviron/goroslib/v2/pkg/msgs/visualization_msgs" "plugin" "strconv" "sync" ) var ( // 扩展 TopicOfCicvExtend = "/cicv_extend" RuleOfCicvExtend []func(param entity.PjisuvParam) string //1 TopicOfAmrPose = "/amr_pose" RuleOfAmrPose []func(data *visualization_msgs.MarkerArray) string //2 TopicOfBoundingBoxesFast = "/bounding_boxes_fast" RuleOfBoundingBoxesFast []func(data *pjisuv_msgs.BoundingBoxArray) string //3 TopicOfCameraFault = "/camera_fault" RuleOfCameraFault []func(data *pjisuv_msgs.FaultVec) string //4 TopicOfCanData = "/can_data" RuleOfCanData []func(data *pjisuv_msgs.Frame) string //5 TopicOfCh128x1LslidarPointCloud = "/ch128x1/lslidar_point_cloud" RuleOfCh128x1LslidarPointCloud []func(data *sensor_msgs.PointCloud2) string //6 TopicOfCh64wLLslidarPointCloud = "/ch64w_l/lslidar_point_cloud" RuleOfCh64wLLslidarPointCloud []func(data *sensor_msgs.PointCloud2) string //7 TopicOfCh64wLScan = "/ch64w_l/scan" RuleOfCh64wLScan []func(data *sensor_msgs.LaserScan) string //8 TopicOfCh64wRLslidarPointCloud = "/ch64w_r/lslidar_point_cloud" RuleOfCh64wRLslidarPointCloud []func(data *sensor_msgs.PointCloud2) string //9 TopicOfCh64wRScan = "/ch64w_r/scan" RuleOfCh64wRScan []func(data *sensor_msgs.LaserScan) string //10 TopicOfCicvLidarclusterMovingObjects = "/cicv/lidarcluster_moving_objects" RuleOfCicvLidarclusterMovingObjects []func(data *pjisuv_msgs.PerceptionCicvMovingObjects) string //11 TopicOfCicvAmrTrajectory = "/cicv_amr_trajectory" RuleOfCicvAmrTrajectory []func(data *pjisuv_msgs.Trajectory) string //12 TopicOfCicvLocation = "/cicv_location" RuleOfCicvLocation []func(data *pjisuv_msgs.PerceptionLocalization) string //13 TopicOfCloudClusters = "/cloud_clusters" RuleOfCloudClusters []func(data *pjisuv_msgs.AutowareCloudClusterArray) string //14 TopicOfHeartbeatInfo = "/heartbeat_info" RuleOfHeartbeatInfo []func(data *pjisuv_msgs.HeartBeatInfo) string //15 TopicOfLidarPretreatmentCost = "/lidarPretreatment_Cost" RuleOfLidarPretreatmentCost []func(data *geometry_msgs.Vector3Stamped) string //16 TopicOfLidarPretreatmentOdometry = "/lidar_pretreatment/odometry" RuleOfLidarPretreatmentOdometry []func(data *nav_msgs.Odometry) string //17 TopicOfLidarRoi = "/lidar_roi" RuleOfLidarRoi []func(data *geometry_msgs.PolygonStamped) string //18 TopicOfLine1 = "/line_1" RuleOfLine1 []func(data *nav_msgs.Path) string //19 TopicOfLine2 = "/line_2" RuleOfLine2 []func(data *nav_msgs.Path) string //20 TopicOfMapPolygon = "/map_polygon" RuleOfMapPolygon []func(data *pjisuv_msgs.PolygonStamped) string //21 TopicOfObstacleDisplay = "/obstacle_display" RuleOfObstacleDisplay []func(data *visualization_msgs.MarkerArray) string //22 TopicOfPjControlPub = "/pj_control_pub" RuleOfPjControlPub []func(data *pjisuv_msgs.CommonVehicleCmd) string //23 TopicOfPointsCluster = "/points_cluster" RuleOfPointsCluster []func(data *sensor_msgs.PointCloud2) string //24 TopicOfPointsConcat = "/points_concat" RuleOfPointsConcat []func(data *sensor_msgs.PointCloud2) string //25 TopicOfReferenceDisplay = "/reference_display" RuleOfReferenceDisplay []func(data *nav_msgs.Path) string //26 TopicOfReferenceTrajectory = "/reference_trajectory" RuleOfReferenceTrajectory []func(data *pjisuv_msgs.Trajectory) string //27 TopicOfRoiPoints = "/roi/points" RuleOfRoiPoints []func(data *sensor_msgs.PointCloud2) string //28 TopicOfRoiPolygon = "/roi/polygon" RuleOfRoiPolygon []func(data *nav_msgs.Path) string //29 TopicOfTf = "/tf" RuleOfTf []func(data *tf2_msgs.TFMessage) string //30 TopicOfTpperception = "/tpperception" RuleOfTpperception []func(data *pjisuv_msgs.PerceptionObjects, extendParam entity.PjisuvParam) string //31 TopicOfTpperceptionVis = "/tpperception/vis" RuleOfTpperceptionVis []func(data *visualization_msgs.MarkerArray) string //32 TopicOfTprouteplan = "/tprouteplan" RuleOfTprouteplan []func(data *pjisuv_msgs.RoutePlan) string //33 TopicOfTrajectoryDisplay = "/trajectory_display" RuleOfTrajectoryDisplay []func(data *nav_msgs.Path) string //34 TopicOfUngroundCloudpoints = "/unground_cloudpoints" RuleOfUngroundCloudpoints []func(data *sensor_msgs.PointCloud2) string //35 TopicOfCameraImage = "/camera_image" RuleOfCameraImage []func(data *sensor_msgs.Image) string //36 TopicOfDataRead = "/data_read" RuleOfDataRead []func(data *pjisuv_msgs.Retrieval) string //37 TopicOfPjiGps = "/pji_gps" RuleOfPjiGps []func(data *pjisuv_msgs.PerceptionLocalization) string //38 TopicOfFaultInfo = "/fault_info" //RuleOfFaultInfo []func(data *pjisuv_msgs.FaultVec) string LabelMapTriggerId sync.Map ) func InitTriggerConfig() { c_log.GlobalLogger.Info("主节点加载触发器插件 - 开始。") // 下载所有触发器的文件 for _, trigger := range config.PlatformConfig.TaskTriggers { triggerLocalPath := config.CloudConfig.TriggersDir + trigger.TriggerScriptPath _ = util.CreateParentDir(triggerLocalPath) c_log.GlobalLogger.Info("下载触发器插件从 ", trigger.TriggerScriptPath, " 到 ", triggerLocalPath) config.OssMutex.Lock() err := config.OssBucket.GetObjectToFile(trigger.TriggerScriptPath, triggerLocalPath) config.OssMutex.Unlock() if err != nil { c_log.GlobalLogger.Error("下载 oss 上的触发器插件失败:", err) continue } // 载入插件到数组 open, err := plugin.Open(triggerLocalPath) if err != nil { c_log.GlobalLogger.Error("加载本地插件", triggerLocalPath, "失败。", err) continue } topic0, err := open.Lookup("Topic") if err != nil { c_log.GlobalLogger.Error("加载本地插件", triggerLocalPath, "中的Topic方法失败。", err) continue } topic1, ok := topic0.(func() string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func() string):", err) continue } topic2 := topic1() rule, err := open.Lookup("Rule") if err != nil { c_log.GlobalLogger.Error("加载本地插件", triggerLocalPath, "中的Rule方法失败。", err) continue } if TopicOfCicvExtend == topic2 { // 自定义扩展 f, ok := rule.(func(param entity.PjisuvParam) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(param entity.ExtendParam):", err) continue } RuleOfCicvExtend = append(RuleOfCicvExtend, f) } else if TopicOfAmrPose == topic2 { //1 f, ok := rule.(func(data *visualization_msgs.MarkerArray) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *visualization_msgs.MarkerArray) string):", err) continue } RuleOfAmrPose = append(RuleOfAmrPose, f) } else if TopicOfBoundingBoxesFast == topic2 { //2 f, ok := rule.(func(data *pjisuv_msgs.BoundingBoxArray) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *pjisuv_msgs.BoundingBoxArray) string):", err) continue } RuleOfBoundingBoxesFast = append(RuleOfBoundingBoxesFast, f) } else if TopicOfCameraFault == topic2 { //3 f, ok := rule.(func(data *pjisuv_msgs.FaultVec) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *pjisuv_msgs.FaultVec) string):", err) continue } RuleOfCameraFault = append(RuleOfCameraFault, f) } else if TopicOfCanData == topic2 { //4 f, ok := rule.(func(data *pjisuv_msgs.Frame) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *pjisuv_msgs.Frame) string):", err) continue } RuleOfCanData = append(RuleOfCanData, f) } else if TopicOfCh128x1LslidarPointCloud == topic2 { //5 f, ok := rule.(func(data *sensor_msgs.PointCloud2) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *sensor_msgs.PointCloud2) string):", err) continue } RuleOfCh128x1LslidarPointCloud = append(RuleOfCh128x1LslidarPointCloud, f) } else if TopicOfCh64wLLslidarPointCloud == topic2 { //6 f, ok := rule.(func(data *sensor_msgs.PointCloud2) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *sensor_msgs.PointCloud2) string):", err) continue } RuleOfCh64wLLslidarPointCloud = append(RuleOfCh64wLLslidarPointCloud, f) } else if TopicOfCh64wLScan == topic2 { //7 f, ok := rule.(func(data *sensor_msgs.LaserScan) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *sensor_msgs.LaserScan) string):", err) continue } RuleOfCh64wLScan = append(RuleOfCh64wLScan, f) } else if TopicOfCh64wRLslidarPointCloud == topic2 { //8 f, ok := rule.(func(data *sensor_msgs.PointCloud2) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *sensor_msgs.PointCloud2) string):", err) continue } RuleOfCh64wRLslidarPointCloud = append(RuleOfCh64wRLslidarPointCloud, f) } else if TopicOfCh64wRScan == topic2 { //9 f, ok := rule.(func(data *sensor_msgs.LaserScan) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *sensor_msgs.LaserScan) string):", err) continue } RuleOfCh64wRScan = append(RuleOfCh64wRScan, f) } else if TopicOfCicvLidarclusterMovingObjects == topic2 { //10 f, ok := rule.(func(data *pjisuv_msgs.PerceptionCicvMovingObjects) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *pjisuv_msgs.PerceptionCicvMovingObjects) string):", err) continue } RuleOfCicvLidarclusterMovingObjects = append(RuleOfCicvLidarclusterMovingObjects, f) } else if TopicOfCicvAmrTrajectory == topic2 { //11 f, ok := rule.(func(data *pjisuv_msgs.Trajectory) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *pjisuv_msgs.Trajectory) string):", err) continue } RuleOfCicvAmrTrajectory = append(RuleOfCicvAmrTrajectory, f) } else if TopicOfCicvLocation == topic2 { //12 f, ok := rule.(func(data *pjisuv_msgs.PerceptionLocalization) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *pjisuv_msgs.PerceptionLocalization) string):", err) continue } RuleOfCicvLocation = append(RuleOfCicvLocation, f) } else if TopicOfCloudClusters == topic2 { //13 f, ok := rule.(func(data *pjisuv_msgs.AutowareCloudClusterArray) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *pjisuv_msgs.AutowareCloudClusterArray) string):", err) continue } RuleOfCloudClusters = append(RuleOfCloudClusters, f) } else if TopicOfHeartbeatInfo == topic2 { //14 f, ok := rule.(func(data *pjisuv_msgs.HeartBeatInfo) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *pjisuv_msgs.HeartBeatInfo) string):", err) continue } RuleOfHeartbeatInfo = append(RuleOfHeartbeatInfo, f) } else if TopicOfLidarPretreatmentCost == topic2 { //15 f, ok := rule.(func(data *geometry_msgs.Vector3Stamped) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *geometry_msgs.Vector3Stamped) string):", err) continue } RuleOfLidarPretreatmentCost = append(RuleOfLidarPretreatmentCost, f) } else if TopicOfLidarPretreatmentOdometry == topic2 { //16 f, ok := rule.(func(data *nav_msgs.Odometry) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *nav_msgs.Odometry) string):", err) continue } RuleOfLidarPretreatmentOdometry = append(RuleOfLidarPretreatmentOdometry, f) } else if TopicOfLidarRoi == topic2 { //17 f, ok := rule.(func(data *geometry_msgs.PolygonStamped) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *geometry_msgs.PolygonStamped) string):", err) continue } RuleOfLidarRoi = append(RuleOfLidarRoi, f) } else if TopicOfLine1 == topic2 { //18 f, ok := rule.(func(data *nav_msgs.Path) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *nav_msgs.Path) string):", err) continue } RuleOfLine1 = append(RuleOfLine1, f) } else if TopicOfLine2 == topic2 { //19 f, ok := rule.(func(data *nav_msgs.Path) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *nav_msgs.Path) string):", err) continue } RuleOfLine2 = append(RuleOfLine2, f) } else if TopicOfMapPolygon == topic2 { //20 f, ok := rule.(func(data *pjisuv_msgs.PolygonStamped) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *pjisuv_msgs.PolygonStamped) string):", err) continue } RuleOfMapPolygon = append(RuleOfMapPolygon, f) } else if TopicOfObstacleDisplay == topic2 { //21 f, ok := rule.(func(data *visualization_msgs.MarkerArray) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *visualization_msgs.MarkerArray) string):", err) continue } RuleOfObstacleDisplay = append(RuleOfObstacleDisplay, f) } else if TopicOfPjControlPub == topic2 { //22 f, ok := rule.(func(data *pjisuv_msgs.CommonVehicleCmd) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *pjisuv_msgs.CommonVehicleCmd) string):", err) continue } RuleOfPjControlPub = append(RuleOfPjControlPub, f) } else if TopicOfPointsCluster == topic2 { //23 f, ok := rule.(func(data *sensor_msgs.PointCloud2) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *sensor_msgs.PointCloud2) string):", err) continue } RuleOfPointsCluster = append(RuleOfPointsCluster, f) } else if TopicOfPointsConcat == topic2 { //24 f, ok := rule.(func(data *sensor_msgs.PointCloud2) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *sensor_msgs.PointCloud2) string):", err) continue } RuleOfPointsConcat = append(RuleOfPointsConcat, f) } else if TopicOfReferenceDisplay == topic2 { //25 f, ok := rule.(func(data *nav_msgs.Path) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *nav_msgs.Path) string):", err) continue } RuleOfReferenceDisplay = append(RuleOfReferenceDisplay, f) } else if TopicOfReferenceTrajectory == topic2 { //26 f, ok := rule.(func(data *pjisuv_msgs.Trajectory) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *pjisuv_msgs.Trajectory) string):", err) continue } RuleOfReferenceTrajectory = append(RuleOfReferenceTrajectory, f) } else if TopicOfRoiPoints == topic2 { //27 f, ok := rule.(func(data *sensor_msgs.PointCloud2) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *sensor_msgs.PointCloud2) string):", err) continue } RuleOfRoiPoints = append(RuleOfRoiPoints, f) } else if TopicOfRoiPolygon == topic2 { //28 f, ok := rule.(func(data *nav_msgs.Path) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *nav_msgs.Path) string):", err) continue } RuleOfRoiPolygon = append(RuleOfRoiPolygon, f) } else if TopicOfTf == topic2 { //29 f, ok := rule.(func(data *tf2_msgs.TFMessage) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *tf2_msgs.TFMessage) string):", err) continue } RuleOfTf = append(RuleOfTf, f) } else if TopicOfTpperception == topic2 { //30 f, ok := rule.(func(data *pjisuv_msgs.PerceptionObjects, param entity.PjisuvParam) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *pjisuv_msgs.PerceptionObjects, param entity.ExtendParam) string):", err) continue } RuleOfTpperception = append(RuleOfTpperception, f) } else if TopicOfTpperceptionVis == topic2 { //31 f, ok := rule.(func(data *visualization_msgs.MarkerArray) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *visualization_msgs.MarkerArray) string):", err) continue } RuleOfTpperceptionVis = append(RuleOfTpperceptionVis, f) } else if TopicOfTprouteplan == topic2 { //32 f, ok := rule.(func(data *pjisuv_msgs.RoutePlan) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *pjisuv_msgs.RoutePlan) string):", err) continue } RuleOfTprouteplan = append(RuleOfTprouteplan, f) } else if TopicOfTrajectoryDisplay == topic2 { //33 f, ok := rule.(func(data *nav_msgs.Path) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *nav_msgs.Path) string):", err) continue } RuleOfTrajectoryDisplay = append(RuleOfTrajectoryDisplay, f) } else if TopicOfUngroundCloudpoints == topic2 { //34 f, ok := rule.(func(data *sensor_msgs.PointCloud2) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *sensor_msgs.PointCloud2) string):", err) continue } RuleOfUngroundCloudpoints = append(RuleOfUngroundCloudpoints, f) } else if TopicOfCameraImage == topic2 { //35 f, ok := rule.(func(data *sensor_msgs.Image) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *sensor_msgs.Image) string):", err) continue } RuleOfCameraImage = append(RuleOfCameraImage, f) } else if TopicOfDataRead == topic2 { //36 f, ok := rule.(func(data *pjisuv_msgs.Retrieval) string) if ok != true { c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *pjisuv_msgs.Retrieval) string):", err) continue } RuleOfDataRead = append(RuleOfDataRead, f) } else { c_log.GlobalLogger.Error("未知的topic:", topic2) continue } label, err := open.Lookup("Label") if err != nil { c_log.GlobalLogger.Error("加载本地插件 ", triggerLocalPath, " 中的 Label 方法失败。", err) continue } labelFunc := label.(func() string) labelString := labelFunc() LabelMapTriggerId.Store(labelString, strconv.Itoa(trigger.TriggerId)) c_log.GlobalLogger.Info("主节点加载触发器插件:【ros topic】=", topic2, ",【触发器label】=", labelString, "【触发器ID】=", trigger.TriggerId) } c_log.GlobalLogger.Info("一共有", len(config.PlatformConfig.TaskTriggers), "个触发器。") }