cicv_data_closed_loop/aarch64/pjisuv/master/config/master_trigger_config.go

package config

import (
	"cicv-data-closedloop/aarch64/pjisuv/common/config"
	"cicv-data-closedloop/common/config/c_log"
	"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 (
	//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, velocityX float64, velocityY float64, yaw float64) 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

	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 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, velocityX float64, velocityY float64, yaw float64) string)
			if ok != true {
				c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *pjisuv_msgs.PerceptionObjects, velocityX float64, velocityY float64, yaw float64) 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), "个触发器。")
}