commit

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

@@ -5,6 +5,11 @@ import (
 	"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"
 	"strings"
@@ -12,17 +17,147 @@ import (
 )
 
 var (
-	TopicOfNodeFaultInfo = "/nodefault_info"
-	TopicOfCicvLocation  = "/cicv_location"
-	TopicOfTpperception  = "/tpperception"
-	TopicOfFaultInfo     = "/fault_info"
-	TopicOfDataRead      = "/data_read"
-	RuleOfNodefaultInfo  []func(data *pjisuv_msgs.FaultInfo) string
-	RuleOfCicvLocation   []func(data *pjisuv_msgs.PerceptionLocalization) string
-	RuleOfTpperception   []func(data *pjisuv_msgs.PerceptionObjects, velocityX float64, velocityY float64, yaw float64) string
-	RuleOfFaultInfo      []func(data *pjisuv_msgs.FaultVec) string
-	RuleOfDataRead       []func(data *pjisuv_msgs.Retrieval) string
-	LabelMapTriggerId    sync.Map
+	//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
+	TopicOfDataRead = "/data_read"
+	RuleOfDataRead  []func(data *pjisuv_msgs.Retrieval) string
+
+	LabelMapTriggerId sync.Map
 )
 
 func InitTriggerConfig() {
@@ -65,35 +200,245 @@ func InitTriggerConfig() {
 			c_log.GlobalLogger.Error("加载本地插件", triggerLocalPath, "中的Rule方法失败。", err)
 			continue
 		}
-		if TopicOfNodeFaultInfo == topic2 {
-			f, ok := rule.(func(data *pjisuv_msgs.FaultInfo) string)
+		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 *pjisuv_msgs.FaultInfo) string):", err)
+				c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *sensor_msgs.PointCloud2) string):", err)
 				continue
 			}
-			RuleOfNodefaultInfo = append(RuleOfNodefaultInfo, f)
-		} else if TopicOfCicvLocation == topic2 {
+			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 TopicOfTpperception == topic2 {
+		} 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 TopicOfFaultInfo == topic2 {
-			f, ok := rule.(func(data *pjisuv_msgs.FaultVec) string)
+		} 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 *pjisuv_msgs.FaultVec) string):", err)
+				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
 			}
-			RuleOfFaultInfo = append(RuleOfFaultInfo, f)
-		} else if TopicOfDataRead == topic2 {
+			RuleOfUngroundCloudpoints = append(RuleOfUngroundCloudpoints, f)
+		} else if TopicOfDataRead == topic2 { //34
 			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)