commit

README.md

@@ -2,6 +2,11 @@
 
 1.21.6
 
+# 编译
+cd /root/mlx/
+docker cp 23e9164d44d7:/root/cicv-data-closedloop ./
+
+
 # dynamic build
 
 ## arm64

pji/common/svc/rosbag_record.go

@@ -13,7 +13,7 @@ import (
 
 // BagRecord 打包rosbag
 func BagRecord(nodeName string) {
-	log.GlobalLogger.Info("------- 打包线程，启动！-------")
+	log.GlobalLogger.Info("rosbag record goroutine 启动")
 	for {
 		// 判断是否能运行
 		log.GlobalLogger.Info("正在等待rosnode启动完成。")
@@ -22,7 +22,7 @@ func BagRecord(nodeName string) {
 			time.Sleep(time.Duration(2) * time.Second)
 			canRecord = isCanRecord(cfg.RosNode)
 		}
-		log.GlobalLogger.Info("rosnode启动完成，正在启动record命令。")
+		log.GlobalLogger.Info("rosnode 启动完成，正在启动 rosbag record 命令。")
 
 		var command []string
 		command = append(command, "record")

trigger/kinglong/cicv_location/brake/main/brake.go

@@ -0,0 +1,21 @@
+package main
+
+import (
+	"cicv-data-closedloop/kinglong_msgs"
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+func Label() string {
+	return "brake"
+}
+
+func Rule(data *kinglong_msgs.PerceptionLocalization) string {
+	if data.AccelX*9.8 < -2.5 {
+		return "brake"
+	} else {
+		return ""
+	}
+}

trigger/kinglong/cicv_location/overspeed/main/overspeed.go

@@ -0,0 +1,22 @@
+package main
+
+import (
+	"cicv-data-closedloop/kinglong_msgs"
+	"math"
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+func Label() string {
+	return "overspeed"
+}
+
+func Rule(data *kinglong_msgs.PerceptionLocalization) string {
+	if math.Pow(math.Pow(data.VelocityX, 2)+math.Pow(data.VelocityY, 2), 0.5)*3.6 >= 65 {
+		return "overspeed"
+	} else {
+		return ""
+	}
+}

trigger/kinglong/cicv_location/rapidaccel/main/rapidaccel.go

@@ -0,0 +1,21 @@
+package main
+
+import (
+	"cicv-data-closedloop/kinglong_msgs"
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+func Label() string {
+	return "rapidaccel"
+}
+
+func Rule(data *kinglong_msgs.PerceptionLocalization) string {
+	if data.AccelX*9.8 > 2.0 {
+		return "rapidaccel"
+	} else {
+		return ""
+	}
+}

trigger/kinglong/data_read/AutoDLimit/main/AutoDLimit.go

@@ -0,0 +1,21 @@
+package main
+
+import (
+	"cicv-data-closedloop/kinglong_msgs"
+)
+
+func Topic() string {
+	return "/data_read"
+}
+
+func Label() string {
+	return "AutoDLimit"
+}
+
+func Rule(data *kinglong_msgs.Retrieval) string {
+	// 限制进入自动驾驶
+	if data.AutoDLimitInReason != 0 {
+		return "AutoDLimit"
+	}
+	return ""
+}

trigger/kinglong/data_read/EmergencyStop/main/EmergencyStop.go

@@ -0,0 +1,22 @@
+package main
+
+import (
+	"cicv-data-closedloop/kinglong_msgs"
+)
+
+func Topic() string {
+	return "/data_read"
+}
+
+func Label() string {
+	return "EmergencyStop"
+}
+
+func Rule(data *kinglong_msgs.Retrieval) string {
+
+	// 紧急停车激活
+	if data.EmergencyStopReason != 0 {
+		return "EmergencyStop"
+	}
+	return ""
+}

trigger/kinglong/data_read/brake/main/brake.go

@@ -0,0 +1,21 @@
+package main
+
+import (
+	"cicv-data-closedloop/kinglong_msgs"
+)
+
+func Topic() string {
+	return "/data_read"
+}
+
+func Label() string {
+	return "brake"
+}
+
+func Rule(data *kinglong_msgs.Retrieval) string {
+	// 惯导采集的加速度
+	if data.AsVehAccelerationValue <= -2.5 {
+		return "brake"
+	}
+	return ""
+}

trigger/kinglong/data_read/brakefault/main/brakefault.go

@@ -0,0 +1,23 @@
+package main
+
+import (
+	"cicv-data-closedloop/kinglong_msgs"
+)
+
+func Topic() string {
+	return "/data_read"
+}
+
+func Label() string {
+	return "brakefault"
+}
+
+func Rule(data *kinglong_msgs.Retrieval) string {
+
+	// 制动系统故障
+	if data.BrakeSysFaultSt != 0 {
+		return "brakefault"
+	}
+
+	return ""
+}

trigger/kinglong/data_read/lanechange/main/lanechange.go

@@ -0,0 +1,22 @@
+package main
+
+import (
+	"cicv-data-closedloop/kinglong_msgs"
+	"math"
+)
+
+func Topic() string {
+	return "/data_read"
+}
+
+func Label() string {
+	return "lanechange"
+}
+
+func Rule(data *kinglong_msgs.Retrieval) string {
+	// 车辆紧急换道
+	if math.Abs(data.StrgAngleRealValue) > 10 && data.StrgAngleSpdValue >= 25 && data.StrgAngleSpdValue <= 45 {
+		return "lanechange"
+	}
+	return ""
+}

trigger/kinglong/data_read/takeover/main/takeover.go

@@ -0,0 +1,21 @@
+package main
+
+import (
+	"cicv-data-closedloop/kinglong_msgs"
+)
+
+func Topic() string {
+	return "/data_read"
+}
+
+func Label() string {
+	return "takeover"
+}
+
+func Rule(data *kinglong_msgs.Retrieval) string {
+	// 驾驶员接管
+	if data.AsDriverTakeoverReq == 1 || data.VcuDriverTakeoverReq == 1 {
+		return "takeover"
+	}
+	return ""
+}

trigger/kinglong/fault_info/controlfault/main/controlfault.go

@@ -0,0 +1,22 @@
+package main
+
+import (
+	"cicv-data-closedloop/kinglong_msgs"
+)
+
+func Topic() string {
+	return "/fault_info"
+}
+
+func Label() string {
+	return "controlfault"
+}
+
+func Rule(data *kinglong_msgs.FaultVec) string {
+	for _, fault := range data.InfoVec {
+		if fault.ErrorCode == 12560 {
+			return "controlfault"
+		}
+	}
+	return ""
+}

trigger/kinglong/nodefault_info/controlfault/main/controlfault.go

@@ -0,0 +1,25 @@
+package main
+
+import (
+	"cicv-data-closedloop/kinglong_msgs"
+	"fmt"
+)
+
+func Topic() string {
+	return "/nodefault_info"
+}
+
+func Label() string {
+	return "controlfault"
+}
+
+func Rule(data *kinglong_msgs.FaultInfo) string {
+	if string(Hex(int(data.ErrorCode))[2]) == "3" {
+		return "controlfault"
+	}
+	return ""
+}
+
+func Hex(num int) string {
+	return fmt.Sprintf("0x%x", num)
+}

trigger/kinglong/nodefault_info/driverfault/main/driverfault.go

@@ -0,0 +1,24 @@
+package main
+
+import (
+	"cicv-data-closedloop/kinglong_msgs"
+	"fmt"
+)
+
+func Topic() string {
+	return "/nodefault_info"
+}
+
+func Label() string {
+	return "driverfault"
+}
+func Rule(data *kinglong_msgs.FaultInfo) string {
+	if string(Hex(int(data.ErrorCode))[2]) == "4" {
+		return "driverfault"
+	}
+	return ""
+}
+
+func Hex(num int) string {
+	return fmt.Sprintf("0x%x", num)
+}

trigger/kinglong/nodefault_info/enginefault/main/enginefault.go

@@ -0,0 +1,23 @@
+package main
+
+import (
+	"cicv-data-closedloop/kinglong_msgs"
+	"fmt"
+)
+
+func Topic() string {
+	return "/nodefault_info"
+}
+func Label() string {
+	return "enginefault"
+}
+func Rule(data *kinglong_msgs.FaultInfo) string {
+	if string(Hex(int(data.ErrorCode))[2]) == "5" {
+		return "enginefault"
+	}
+	return ""
+}
+
+func Hex(num int) string {
+	return fmt.Sprintf("0x%x", num)
+}

trigger/kinglong/nodefault_info/perceptionfault/main/perceptionfault.go

@@ -0,0 +1,23 @@
+package main
+
+import (
+	"cicv-data-closedloop/kinglong_msgs"
+	"fmt"
+)
+
+func Topic() string {
+	return "/nodefault_info"
+}
+func Label() string {
+	return "perceptionfault"
+}
+func Rule(data *kinglong_msgs.FaultInfo) string {
+	if string(Hex(int(data.ErrorCode))[2]) == "1" {
+		return "perceptionfault"
+	}
+	return ""
+}
+
+func Hex(num int) string {
+	return fmt.Sprintf("0x%x", num)
+}

trigger/kinglong/nodefault_info/plannerfault/main/plannerfault.go

@@ -0,0 +1,24 @@
+package main
+
+import (
+	"cicv-data-closedloop/kinglong_msgs"
+	"fmt"
+)
+
+func Topic() string {
+	return "/nodefault_info"
+}
+func Label() string {
+	return "plannerfault"
+}
+func Rule(data *kinglong_msgs.FaultInfo) string {
+	// 驾驶员接管
+	if string(Hex(int(data.ErrorCode))[2]) == "2" {
+		return "plannerfault"
+	}
+	return ""
+}
+
+func Hex(num int) string {
+	return fmt.Sprintf("0x%x", num)
+}

trigger/kinglong/tpperception/TTC/main/TTC.go

@@ -0,0 +1,33 @@
+package main
+
+import (
+	"cicv-data-closedloop/kinglong_msgs"
+	"math"
+)
+
+func Topic() string {
+	return "/tpperception"
+}
+
+func Label() string {
+	return "TTC"
+}
+
+func Rule(data *kinglong_msgs.PerceptionObjects, velocityX float64, velocityY float64, yaw float64) string {
+	for _, obj := range data.Objs {
+		if math.Abs(float64(obj.Y)) <= 2 && obj.X >= 8 {
+			theta := DegreesToRadians(yaw)
+			vxrel := (float64(obj.Vxabs)-velocityX)*math.Cos(theta) + (float64(obj.Vyabs)-velocityY)*math.Sin(theta)
+			ttc := -((float64(obj.X) - 5.2) / (vxrel + 0.001))
+			if ttc >= 0 && ttc <= 3 {
+				return "TTC"
+			}
+		}
+	}
+	return ""
+}
+
+// DegreesToRadians 计算角度转弧度
+func DegreesToRadians(degrees float64) float64 {
+	return degrees * math.Pi / 180.0
+}