cicv_data_closed_loop/kinglong/master/pkg/cfg/master_trigger_cfg.go

package cfg

import (
	"cicv-data-closedloop/kinglong/common/cfg"
	"cicv-data-closedloop/kinglong/common/cutil"
	"cicv-data-closedloop/kinglong/common/log"
	"cicv-data-closedloop/kinglong_msgs"
	"plugin"
	"strconv"
	"strings"
)

var (
	TopicOfNodeFaultInfo = "/nodefault_info"
	TopicOfCicvLocation  = "/cicv_location"
	TopicOfTpperception  = "/tpperception"
	TopicOfFaultInfo     = "/fault_info"
	TopicOfDataRead      = "/data_read"
	RuleOfNodefaultInfo  []func(data *kinglong_msgs.FaultInfo) string
	RuleOfCicvLocation   []func(data *kinglong_msgs.PerceptionLocalization) string
	RuleOfTpperception   []func(data *kinglong_msgs.PerceptionObjects, velocityX float64, velocityY float64, yaw float64) string
	RuleOfFaultInfo      []func(data *kinglong_msgs.FaultVec) string
	RuleOfDataRead       []func(data *kinglong_msgs.Retrieval) string
	LabelMapTriggerId    = make(map[string]string)
)

//func InitTriggerConfig() {
//	log.GlobalLogger.Info("主节点加载触发器插件 - 开始。")
//	LabelMapTriggerId["rapidaccel"] = strconv.Itoa(1744178491185696770)
//	LabelMapTriggerId["brake"] = strconv.Itoa(1744178775815360514)
//	LabelMapTriggerId["EmergencyStop"] = strconv.Itoa(1744178920674037761)
//	LabelMapTriggerId["AutoDLimit"] = strconv.Itoa(1744177451715530754)
//	LabelMapTriggerId["lanechange"] = strconv.Itoa(1744180284594589697)
//	LabelMapTriggerId["brakefault"] = strconv.Itoa(1744180401267544065)
//	LabelMapTriggerId["takeover"] = strconv.Itoa(1744180567135490049)
//	LabelMapTriggerId["TTC"] = strconv.Itoa(1744180835122155522)
//	log.GlobalLogger.Info("触发器label对应id为：", LabelMapTriggerId)
//	RuleOfCicvLocation = append(RuleOfCicvLocation,
//		func(data *masterEntity.PerceptionLocalization) string {
//			if data.AccelX*9.8 > 2.0 {
//				log.GlobalLogger.Info("rapidaccel 触发")
//				return "rapidaccel"
//			} else {
//				//log.GlobalLogger.Info("rapidaccel 未触发")
//				return ""
//			}
//		},
//		func(data *masterEntity.PerceptionLocalization) string {
//			if data.AccelX*9.8 < -2.5 {
//				log.GlobalLogger.Info("brake 触发")
//				return "brake"
//			} else {
//				//log.GlobalLogger.Info("brake 未触发")
//				return ""
//			}
//		},
//	)
//	//RuleOfDataRead = append(RuleOfDataRead,
//	//	func(data *masterEnt.Retrieval) string {
//	//		log.GlobalLogger.Info("/data 数据为：")
//	//		// 紧急停车激活
//	//		if data.EmergencyStopReason != 0 {
//	//			log.GlobalLogger.Info("EmergencyStop 触发")
//	//			return "EmergencyStop"
//	//		}
//	//		//log.GlobalLogger.Info("EmergencyStop 未触发")
//	//		return ""
//	//	},
//	//	func(data *masterEnt.Retrieval) string {
//	//		log.GlobalLogger.Info("/data 数据为：")
//	//		// 限制进入自动驾驶
//	//		if data.AutoDLimitInReason != 0 {
//	//			log.GlobalLogger.Info("AutoDLimit 触发")
//	//			return "AutoDLimit"
//	//		}
//	//		//log.GlobalLogger.Info("AutoDLimit 未触发")
//	//		return ""
//	//	},
//	//	func(data *masterEnt.Retrieval) string {
//	//		log.GlobalLogger.Info("/data 数据为：")
//	//		// 车辆紧急换道
//	//		if math.Abs(data.StrgAngleRealValue) > 10 && data.StrgAngleSpdValue >= 25 && data.StrgAngleSpdValue <= 45 {
//	//			log.GlobalLogger.Info("lanechange 触发")
//	//			return "lanechange"
//	//		}
//	//		//log.GlobalLogger.Info("lanechange 未触发")
//	//		return ""
//	//	},
//	//	func(data *masterEnt.Retrieval) string {
//	//		log.GlobalLogger.Info("/data 数据为：")
//	//		// 制动系统故障
//	//		if data.BrakeSysFaultSt != 0 {
//	//			log.GlobalLogger.Info("brakefault 触发")
//	//			return "brakefault"
//	//		}
//	//		//log.GlobalLogger.Info("brakefault 未触发")
//	//		return ""
//	//	},
//	//	func(data *masterEnt.Retrieval) string {
//	//		log.GlobalLogger.Info("/data 数据为：")
//	//		// 驾驶员接管
//	//		if data.AsDriverTakeoverReq == 1 || data.VcuDriverTakeoverReq == 1 {
//	//			log.GlobalLogger.Info("takeover 触发")
//	//			return "takeover"
//	//		}
//	//		//log.GlobalLogger.Info("takeover 未触发")
//	//		return ""
//	//	},
//	//)
//	RuleOfTpperception = append(RuleOfTpperception,
//		func(data *masterEntity.PerceptionObjects, velocityX float64, velocityY float64, yaw float64) string {
//			//log.GlobalLogger.Info("TTC 触发")
//			//return "TTC"
//			//log.GlobalLogger.Info("/tpperception 物体个数为：", len(data.Objs))
//			for _, obj := range data.Objs {
//				//log.GlobalLogger.Info("【obj.X】=", obj.X)
//				//log.GlobalLogger.Info("【obj.Y】=", obj.Y)
//				if obj.X >= 8 && math.Abs(float64(obj.Y)) <= 2 {
//					theta := util.DegreesToRadians(yaw)
//					//log.GlobalLogger.Info("【theta】=", theta)
//					vxrel := (float64(obj.Vxabs)-velocityX)*math.Cos(theta) + (float64(obj.Vyabs)-velocityY)*math.Sin(theta)
//					//log.GlobalLogger.Info("【vxrel】=", vxrel)
//					ttc := -((float64(obj.X) - 5.2) / (vxrel + 0.001))
//					//log.GlobalLogger.Info("【ttc】=", ttc)
//					//if ttc >= 0 && ttc <= 3 {
//					if ttc >= 0 && ttc <= 5 {
//						log.GlobalLogger.Info("TTC 触发，值为：", ttc)
//						return "TTC"
//					}
//				}
//			}
//			log.GlobalLogger.Info("TTC 未触发")
//			return ""
//		},
//	)
//	log.GlobalLogger.Info("主节点加载触发器插件 - 成功。长度分别为：", len(RuleOfNodefaultInfo), len(RuleOfCicvLocation), len(RuleOfTpperception), len(RuleOfFaultInfo), len(RuleOfDataRead))
//}

func InitTriggerConfig() {
	log.GlobalLogger.Info("主节点加载触发器插件 - 开始。")
	// 下载所有触发器的文件
	for _, trigger := range cfg.PlatformConfig.TaskTriggers {
		// 获取文件名
		pathSplit := strings.Split(trigger.TriggerScriptPath, "/")
		fileName := pathSplit[len(pathSplit)-1]
		// 下载
		triggerLocalPath := cfg.CloudConfig.TriggersDir + fileName
		cutil.CreateParentDir(triggerLocalPath)
		log.GlobalLogger.Info("下载触发器插件从", trigger.TriggerScriptPath, "到", triggerLocalPath)
		err := cfg.OssBucket.GetObjectToFile(trigger.TriggerScriptPath, triggerLocalPath)
		if err != nil {
			log.GlobalLogger.Error("下载oss上的触发器插件失败：", err)
			continue
		}
		// 载入插件到数组
		open, err := plugin.Open(triggerLocalPath)
		if err != nil {
			log.GlobalLogger.Error("加载本地插件", triggerLocalPath, "失败。", err)
			continue
		}
		topic0, err := open.Lookup("Topic")
		if err != nil {
			log.GlobalLogger.Error("加载本地插件", triggerLocalPath, "中的Topic方法失败。", err)
			continue
		}
		topic1, ok := topic0.(func() string)
		if ok != true {
			log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func() string):", err)
			continue
		}
		topic2 := topic1()
		rule, err := open.Lookup("Rule")
		if err != nil {
			log.GlobalLogger.Error("加载本地插件", triggerLocalPath, "中的Rule方法失败。", err)
			continue
		}
		if TopicOfNodeFaultInfo == topic2 {
			f := rule.(func(data *kinglong_msgs.FaultInfo) string)
			RuleOfNodefaultInfo = append(RuleOfNodefaultInfo, f)
		} else if TopicOfCicvLocation == topic2 {
			f := rule.(func(data *kinglong_msgs.PerceptionLocalization) string)
			RuleOfCicvLocation = append(RuleOfCicvLocation, f)
		} else if TopicOfTpperception == topic2 {
			f := rule.(func(data *kinglong_msgs.PerceptionObjects, velocityX float64, velocityY float64, yaw float64) string)
			RuleOfTpperception = append(RuleOfTpperception, f)
		} else if TopicOfFaultInfo == topic2 {
			f := rule.(func(data *kinglong_msgs.FaultVec) string)
			RuleOfFaultInfo = append(RuleOfFaultInfo, f)
		} else if TopicOfDataRead == topic2 {
			f := rule.(func(data *kinglong_msgs.Retrieval) string)
			RuleOfDataRead = append(RuleOfDataRead, f)
		} else {
			log.GlobalLogger.Error("未知的topic：", topic2)
			continue
		}

		label, err := open.Lookup("Label")
		if err != nil {
			log.GlobalLogger.Error("加载本地插件", triggerLocalPath, "中的Label方法失败。", err)
			continue
		}
		labelFunc := label.(func() string)
		labelString := labelFunc()
		LabelMapTriggerId[labelString] = strconv.Itoa(trigger.TriggerId)
		log.GlobalLogger.Info("主节点加载触发器插件：【ros topic】=", topic2, "，【触发器label】=", labelString, "【触发器ID】=", trigger.TriggerId, "【label和id映射关系】=", LabelMapTriggerId)
	}
	log.GlobalLogger.Info("主节点加载触发器插件 - 成功。")
}