commit

kinglong/master/pkg/cfg/master_trigger_cfg.go

@@ -24,118 +24,6 @@ var (
 	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("主节点加载触发器插件 - 开始。")
 	// 下载所有触发器的文件
@@ -146,10 +34,10 @@ func InitTriggerConfig() {
 		// 下载
 		triggerLocalPath := cfg.CloudConfig.TriggersDir + fileName
 		cutil.CreateParentDir(triggerLocalPath)
-		log.GlobalLogger.Info("下载触发器插件从", trigger.TriggerScriptPath, "到", triggerLocalPath)
+		log.GlobalLogger.Info("下载触发器插件从 ", trigger.TriggerScriptPath, " 到 ", triggerLocalPath)
 		err := cfg.OssBucket.GetObjectToFile(trigger.TriggerScriptPath, triggerLocalPath)
 		if err != nil {
-			log.GlobalLogger.Error("下载oss上的触发器插件失败：", err)
+			log.GlobalLogger.Error("下载 oss 上的触发器插件失败：", err)
 			continue
 		}
 		// 载入插件到数组
@@ -175,19 +63,39 @@ func InitTriggerConfig() {
 			continue
 		}
 		if TopicOfNodeFaultInfo == topic2 {
-			f := rule.(func(data *kinglong_msgs.FaultInfo) string)
+			f, ok := rule.(func(data *kinglong_msgs.FaultInfo) string)
+			if ok != true {
+				log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *kinglong_msgs.FaultInfo) string):", err)
+				continue
+			}
 			RuleOfNodefaultInfo = append(RuleOfNodefaultInfo, f)
 		} else if TopicOfCicvLocation == topic2 {
-			f := rule.(func(data *kinglong_msgs.PerceptionLocalization) string)
+			f, ok := rule.(func(data *kinglong_msgs.PerceptionLocalization) string)
+			if ok != true {
+				log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *kinglong_msgs.PerceptionLocalization) string):", err)
+				continue
+			}
 			RuleOfCicvLocation = append(RuleOfCicvLocation, f)
 		} else if TopicOfTpperception == topic2 {
-			f := rule.(func(data *kinglong_msgs.PerceptionObjects, velocityX float64, velocityY float64, yaw float64) string)
+			f, ok := rule.(func(data *kinglong_msgs.PerceptionObjects, velocityX float64, velocityY float64, yaw float64) string)
+			if ok != true {
+				log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *kinglong_msgs.PerceptionObjects, velocityX float64, velocityY float64, yaw float64) string):", err)
+				continue
+			}
 			RuleOfTpperception = append(RuleOfTpperception, f)
 		} else if TopicOfFaultInfo == topic2 {
-			f := rule.(func(data *kinglong_msgs.FaultVec) string)
+			f, ok := rule.(func(data *kinglong_msgs.FaultVec) string)
+			if ok != true {
+				log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *kinglong_msgs.FaultVec) string):", err)
+				continue
+			}
 			RuleOfFaultInfo = append(RuleOfFaultInfo, f)
 		} else if TopicOfDataRead == topic2 {
-			f := rule.(func(data *kinglong_msgs.Retrieval) string)
+			f, ok := rule.(func(data *kinglong_msgs.Retrieval) string)
+			if ok != true {
+				log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *kinglong_msgs.Retrieval) string):", err)
+				continue
+			}
 			RuleOfDataRead = append(RuleOfDataRead, f)
 		} else {
 			log.GlobalLogger.Error("未知的topic：", topic2)
@@ -196,7 +104,7 @@ func InitTriggerConfig() {
 
 		label, err := open.Lookup("Label")
 		if err != nil {
-			log.GlobalLogger.Error("加载本地插件", triggerLocalPath, "中的Label方法失败。", err)
+			log.GlobalLogger.Error("加载本地插件 ", triggerLocalPath, " 中的 Label 方法失败。", err)
 			continue
 		}
 		labelFunc := label.(func() string)

pji/master/pkg/cfg/master_trigger_cfg.go

@@ -46,61 +46,86 @@ func InitTriggerConfig() {
 			continue
 		}
 		// 载入插件到数组
-		{
-			open, err := plugin.Open(triggerLocalPath)
-			if err != nil {
-				log.GlobalLogger.Error("加载本地插件", triggerLocalPath, "失败。", err)
+		open, err := plugin.Open(triggerLocalPath)
+		if err != nil {
+			log.GlobalLogger.Error("加载本地插件", triggerLocalPath, "失败。", err)
+		}
+		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 TopicOfOdom == topic2 {
+			f, ok := rule.(func(data *nav_msgs.Odometry) string)
+			if ok != true {
+				log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *nav_msgs.Odometry) string):", err)
+				continue
 			}
-			topic0, err := open.Lookup("Topic")
-			if err != nil {
-				log.GlobalLogger.Error("加载本地插件", triggerLocalPath, "中的Topic方法失败。", err)
+			RuleOfOdom = append(RuleOfOdom, f)
+		} else if TopicOfObstacleDetection == topic2 {
+			f, ok := rule.(func(data *std_msgs.UInt8) string)
+			if ok != true {
+				log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *std_msgs.UInt8) string):", err)
 				continue
 			}
-			topic1, ok := topic0.(func() string)
+			RuleOfObstacleDetection = append(RuleOfObstacleDetection, f)
+		} else if TopicOfSysInfo == topic2 {
+			f, ok := rule.(func(data *pji_msgs.SysInfo) string)
 			if ok != true {
-				log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func() string):", err)
+				log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *pji_msgs.SysInfo) string):", err)
 				continue
 			}
-			topic2 := topic1()
-			rule, err := open.Lookup("Rule")
-			if err != nil {
-				log.GlobalLogger.Error("加载本地插件", triggerLocalPath, "中的Rule方法失败。", err)
+			RuleOfSysInfo = append(RuleOfSysInfo, f)
+		} else if TopicOfLocateInfo == topic2 {
+			f, ok := rule.(func(data *pji_msgs.LocateInfo) string)
+			if ok != true {
+				log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *pji_msgs.LocateInfo) string):", err)
 				continue
 			}
-			//TODO 类型断言需要处理异常，防止程序崩溃
-			if TopicOfOdom == topic2 {
-				f := rule.(func(data *nav_msgs.Odometry) string)
-				RuleOfOdom = append(RuleOfOdom, f)
-			} else if TopicOfObstacleDetection == topic2 {
-				f := rule.(func(data *std_msgs.UInt8) string)
-				RuleOfObstacleDetection = append(RuleOfObstacleDetection, f)
-			} else if TopicOfSysInfo == topic2 {
-				f := rule.(func(data *pji_msgs.SysInfo) string)
-				RuleOfSysInfo = append(RuleOfSysInfo, f)
-			} else if TopicOfLocateInfo == topic2 {
-				f := rule.(func(data *pji_msgs.LocateInfo) string)
-				RuleOfLocateInfo = append(RuleOfLocateInfo, f)
-			} else if TopicOfImu == topic2 {
-				f := rule.(func(data *sensor_msgs.Imu) string)
-				RuleOfImu = append(RuleOfImu, f)
-			} else if TopicOfDiagnostics == topic2 {
-				f := rule.(func(data *diagnostic_msgs.DiagnosticArray) string)
-				RuleOfDiagnostics = append(RuleOfDiagnostics, f)
-			} else {
-				log.GlobalLogger.Error("未知的topic：", topic2)
+			RuleOfLocateInfo = append(RuleOfLocateInfo, f)
+		} else if TopicOfImu == topic2 {
+			f, ok := rule.(func(data *sensor_msgs.Imu) string)
+			if ok != true {
+				log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *sensor_msgs.Imu) string):", err)
 				continue
 			}
-
-			label, err := open.Lookup("Label")
-			if err != nil {
-				log.GlobalLogger.Error("加载本地插件", triggerLocalPath, "中的TriggerName方法失败。", err)
+			RuleOfImu = append(RuleOfImu, f)
+		} else if TopicOfDiagnostics == topic2 {
+			f, ok := rule.(func(data *diagnostic_msgs.DiagnosticArray) string)
+			if ok != true {
+				log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *diagnostic_msgs.DiagnosticArray) string):", 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)
+			RuleOfDiagnostics = append(RuleOfDiagnostics, f)
+		} else {
+			log.GlobalLogger.Error("未知的topic：", topic2)
+			continue
 		}
 
+		label, err := open.Lookup("Label")
+		if err != nil {
+			log.GlobalLogger.Error("加载本地插件", triggerLocalPath, "中的TriggerName方法失败。", err)
+			continue
+		}
+		labelFunc, ok := label.(func() string)
+		if ok != true {
+			log.GlobalLogger.Error("插件", triggerLocalPath, "中的Label方法必须是(func() string):", err)
+			continue
+		}
+		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("主节点加载触发器插件 - 成功。")
 }