feat: 金龙车配置急减速触发器

aarch64/jili/master/config/trigger_init.go

@@ -59,7 +59,7 @@ func InitTriggerConfig() {
 	} else {
 		errorCodeSoPath := "/mnt/media/sda1/cicv-data-closedloop/ErrorCode.so"
 		c_log.GlobalLogger.Info("无法连接互联网，提前准备触发器插件文件：", errorCodeSoPath)
-		triggerLocalPathsMapTriggerId[errorCodeSoPath] = "ErrorCodeId"
+		triggerLocalPathsMapTriggerId[errorCodeSoPath] = "ErrorCodeId" // ErrorCodeId 无实际意义，离线状态下徐
 	}
 
 	success := 0

aarch64/pjisuv/master/config/trigger_init.go

@@ -57,9 +57,13 @@ func InitTriggerConfig() {
 			triggerLocalPathsMapTriggerId[triggerLocalPath] = id
 		}
 	} else {
-		errorCodeSoPath := "/mnt/media/sda1/cicv-data-closedloop/ErrorCode.so"
-		c_log.GlobalLogger.Info("无法连接互联网，提前准备触发器插件文件：", errorCodeSoPath)
-		triggerLocalPathsMapTriggerId[errorCodeSoPath] = "ErrorCodeId"
+		//errorCodeSoPath := "/mnt/media/sda1/cicv-data-closedloop/ErrorCode.so"
+		//c_log.GlobalLogger.Info("无法连接互联网，提前准备触发器插件文件：", errorCodeSoPath)
+		//triggerLocalPathsMapTriggerId[errorCodeSoPath] = "ErrorCodeId"
+
+		brakeSoPath := "/mnt/media/sda1/cicv-data-closedloop/trigger/Brake.so"
+		c_log.GlobalLogger.Info("无法连接互联网，提前准备触发器插件文件：", brakeSoPath)
+		triggerLocalPathsMapTriggerId[brakeSoPath] = "20250327171824000054e42a399b9c664f5a80f8"
 	}
 
 	success := 0

aarch64/pjisuv/金龙中巴3号-soc1-local-config.yaml

@@ -5,8 +5,8 @@ node:
   name: node1
   ip: 192.168.1.104
 # 数据闭环平台参数
-equipment-no: kinglong-003
-secret-key: ri6uhgae
+equipment-no: kinglong-0031
+secret-key: QgRq7qWyb8ZQSRJf2dMHOsLr7w2DiAf0RcFw7mom
 # 获取oss连接信息的接口url
 url-get-oss-config: http://36.110.106.156:18379/oss/config?token=nXonLUcMtGcrQqqKiyygIwyVbvizE0wD
 # 金龙车数据前缀

aarch64/pjisuv/金龙中巴3号-soc2-local-config.yaml

@@ -0,0 +1,22 @@
+# 是否联通互联网
+internet: false
+# 节点名称和IP
+node:
+  name: node2
+  ip: 192.168.1.105
+# 数据闭环平台参数
+equipment-no: kinglong-0031
+secret-key: QgRq7qWyb8ZQSRJf2dMHOsLr7w2DiAf0RcFw7mom
+# 获取oss连接信息的接口url
+url-get-oss-config: http://36.110.106.156:18379/oss/config?token=nXonLUcMtGcrQqqKiyygIwyVbvizE0wD
+# 金龙车数据前缀
+oss-base-prefix: kinglong/
+# oss上的配置文件的名称
+cloud-config-filename: cloud-config.yaml
+# 将oss上的配置文件下载到本地的路径
+cloud-config-local-path: /mnt/media/sda1/cicv-data-closedloop/config/cloud-config.yaml
+restart-cmd:
+  dir: "/mnt/media/sda1/cicv-data-closedloop/"
+  name: "sh"
+  args:
+    - "start-soc1.sh"

trigger/kinglong/cicv_extend/cutin_difference/main/cutin_difference.go

@@ -1,101 +0,0 @@
-package main
-
-import (
-	"cicv-data-closedloop/kinglong_param"
-	"math"
-	"sort"
-)
-
-func Topic() string {
-	return "/cicv_extend"
-}
-
-// Label todo 禁止存在下划线_
-func Label() string {
-	return "brake"
-}
-
-func Rule(param kinglong_param.KinglongParam) string {
-	for _, objValue := range param.ObjDicOfTpperception {
-		if len(objValue) <= 5 || !isCuttingIn(objValue, param) || !isDifference(param) {
-			continue
-		}
-		return "cutin_difference"
-	}
-	return ""
-}
-
-func isCuttingIn(objYList []float32, param kinglong_param.KinglongParam) bool {
-	for i, objY := range objYList {
-		if math.Abs(float64(objY)) >= 1.3 && math.Abs(param.AngularVelocityZOfCicvLocation) <= 0.6 {
-			for j := 0; j < len(objYList)-i-1; j++ {
-				if math.Abs(float64(objYList[1+i+j])) <= 0.7 && math.Abs(param.AngularVelocityZOfCicvLocation) <= 0.6 {
-					return true
-				}
-			}
-		}
-	}
-	return false
-}
-
-// 利用曼惠特尼U检验的方式判断车端数据和驾驶员控制数据是否存在显著差异
-func isDifference(param kinglong_param.KinglongParam) bool {
-	_, pValueSteering := mannWhitneyUTest(param.EgoSteeringRealOfDataRead, param.EgoSteeringCmdOfJinlongControlPub)
-	_, pValueThrottle := mannWhitneyUTest(param.EgoThrottleRealOfDataRead, param.EgoThrottleRealOfDataRead)
-	_, pValueBrake := mannWhitneyUTest(param.EgoBrakeRealOfDataRead, param.EgoBrakeCmdOfJinlongControlPub)
-	if pValueSteering < 0.05 || pValueThrottle < 0.05 || pValueBrake < 0.05 {
-		return true
-	}
-	return false
-}
-
-func mannWhitneyUTest(group1, group2 []float64) (float64, float64) {
-	// 统计样本大小
-	n1 := float64(len(group1))
-	n2 := float64(len(group2))
-
-	// 合并样本并排序
-	combined := append(group1, group2...)
-	sort.Float64s(combined)
-
-	// 计算秩和
-	var rankSum1, rankSum2 float64
-	for i := range combined {
-		if i < len(group1) {
-			rankSum1 += float64(i + 1)
-		} else {
-			rankSum2 += float64(i + 1)
-		}
-	}
-
-	// 计算 U 统计量
-	u1 := rankSum1 - (n1 * (n1 + 1) / 2)
-	u2 := rankSum2 - (n2 * (n2 + 1) / 2)
-	u := math.Min(u1, u2)
-
-	// 计算 p 值
-	meanU := (n1 * n2 / 2)
-	stdDevU := math.Sqrt((n1 * n2 * (n1 + n2 + 1)) / 12)
-	z := (u - meanU) / stdDevU
-	p := 2 * (1 - normalCDF(z))
-
-	return u, p
-}
-
-func normalCDF(x float64) float64 {
-	const (
-		b1 = 0.319381530
-		b2 = -0.356563782
-		b3 = 1.781477937
-		b4 = -1.821255978
-		b5 = 1.330274429
-		p  = 0.2316419
-		c  = 0.39894228
-	)
-	if x >= 0.0 {
-		t := 1.0 / (1.0 + p*x)
-		return 1.0 - c*t*(t*(t*(t*(t*b5+b4)+b3)+b2)+b1)
-	}
-	t := 1.0 / (1.0 - p*x)
-	return c * t * (t*(t*(t*(t*b5+b4)+b3)+b2) + b1)
-}

trigger/kinglong/cicv_extend/cutout_difference/main/cutout_difference.go

@@ -1,101 +0,0 @@
-package main
-
-import (
-	"cicv-data-closedloop/common/entity"
-	"math"
-	"sort"
-)
-
-func Topic() string {
-	return "/cicv_extend"
-}
-
-// Label todo 禁止存在下划线_
-func Label() string {
-	return "cutout_difference"
-}
-
-func Rule(param entity.KinglongParam) string {
-	for _, objValue := range param.ObjDicOfTpperception {
-		if len(objValue) <= 5 || !isCuttingOut(objValue, param) || !isDifference(param) {
-			continue
-		}
-		return "cutout_difference"
-	}
-	return ""
-}
-
-func isCuttingOut(objYList []float32, param entity.KinglongParam) bool {
-	for i, objY := range objYList {
-		if math.Abs(float64(objY)) <= 0.6 && math.Abs(param.AngularVelocityZOfCicvLocation) <= 0.6 {
-			for j := 0; j < len(objYList)-i-1; j++ {
-				if math.Abs(float64(objYList[1+i+j])) >= 1.2 && math.Abs(param.AngularVelocityZOfCicvLocation) <= 0.6 {
-					return true
-				}
-			}
-		}
-	}
-	return false
-}
-
-// 利用曼惠特尼U检验的方式判断车端数据和驾驶员控制数据是否存在显著差异
-func isDifference(param entity.KinglongParam) bool {
-	_, pValueSteering := mannWhitneyUTest(param.EgoSteeringRealOfDataRead, param.EgoSteeringCmdOfJinlongControlPub)
-	_, pValueThrottle := mannWhitneyUTest(param.EgoThrottleRealOfDataRead, param.EgoThrottleRealOfDataRead)
-	_, pValueBrake := mannWhitneyUTest(param.EgoBrakeRealOfDataRead, param.EgoBrakeCmdOfJinlongControlPub)
-	if pValueSteering < 0.05 || pValueThrottle < 0.05 || pValueBrake < 0.05 {
-		return true
-	}
-	return false
-}
-
-func mannWhitneyUTest(group1, group2 []float64) (float64, float64) {
-	// 统计样本大小
-	n1 := float64(len(group1))
-	n2 := float64(len(group2))
-
-	// 合并样本并排序
-	combined := append(group1, group2...)
-	sort.Float64s(combined)
-
-	// 计算秩和
-	var rankSum1, rankSum2 float64
-	for i := range combined {
-		if i < len(group1) {
-			rankSum1 += float64(i + 1)
-		} else {
-			rankSum2 += float64(i + 1)
-		}
-	}
-
-	// 计算 U 统计量
-	u1 := rankSum1 - (n1 * (n1 + 1) / 2)
-	u2 := rankSum2 - (n2 * (n2 + 1) / 2)
-	u := math.Min(u1, u2)
-
-	// 计算 p 值
-	meanU := n1 * n2 / 2
-	stdDevU := math.Sqrt((n1 * n2 * (n1 + n2 + 1)) / 12)
-	z := (u - meanU) / stdDevU
-	p := 2 * (1 - normalCDF(z))
-
-	return u, p
-}
-
-func normalCDF(x float64) float64 {
-	const (
-		b1 = 0.319381530
-		b2 = -0.356563782
-		b3 = 1.781477937
-		b4 = -1.821255978
-		b5 = 1.330274429
-		p  = 0.2316419
-		c  = 0.39894228
-	)
-	if x >= 0.0 {
-		t := 1.0 / (1.0 + p*x)
-		return 1.0 - c*t*(t*(t*(t*(t*b5+b4)+b3)+b2)+b1)
-	}
-	t := 1.0 / (1.0 - p*x)
-	return c * t * (t*(t*(t*(t*b5+b4)+b3)+b2) + b1)
-}

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

@@ -1,21 +0,0 @@
-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 < -5.0 {
-		return "brake"
-	} else {
-		return ""
-	}
-}

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

@@ -1,22 +0,0 @@
-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

@@ -1,21 +0,0 @@
-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

@@ -1,21 +0,0 @@
-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

@@ -1,22 +0,0 @@
-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

@@ -1,21 +0,0 @@
-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

@@ -1,23 +0,0 @@
-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

@@ -1,22 +0,0 @@
-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/manualTrig/main/manualTrig.go

@@ -1,21 +0,0 @@
-package main
-
-import (
-	"cicv-data-closedloop/kinglong_msgs"
-)
-
-func Topic() string {
-	return "/data_read"
-}
-
-func Label() string {
-	return "manualTrig"
-}
-
-func Rule(data *kinglong_msgs.Retrieval) string {
-	// 手动按喇叭触发
-	if data.BCHornSt > 1 {
-		return "manualTrig"
-	}
-	return ""
-}

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

@@ -1,21 +0,0 @@
-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/failure/camera/main/camera.go

@@ -1,25 +0,0 @@
-package main
-
-import (
-	"github.com/bluenviron/goroslib/v2/pkg/msgs/std_msgs"
-)
-
-var (
-	topic = "/failure/camera"
-	label = "failurecamera"
-)
-
-func Topic() string {
-	return "/failure/camera"
-}
-
-func Label() string {
-	return label
-}
-
-func Rule(data *std_msgs.Bool) string {
-	if data.Data {
-		return label
-	}
-	return ""
-}

trigger/kinglong/failure/lidar/main/lidar.go

@@ -1,25 +0,0 @@
-package main
-
-import (
-	"github.com/bluenviron/goroslib/v2/pkg/msgs/std_msgs"
-)
-
-var (
-	topic = "/failure/lidar"
-	label = "failurelidar"
-)
-
-func Topic() string {
-	return "/failure/lidar"
-}
-
-func Label() string {
-	return label
-}
-
-func Rule(data *std_msgs.Bool) string {
-	if data.Data {
-		return label
-	}
-	return ""
-}

trigger/kinglong/failure/radar/main/radar.go

@@ -1,25 +0,0 @@
-package main
-
-import (
-	"github.com/bluenviron/goroslib/v2/pkg/msgs/std_msgs"
-)
-
-var (
-	topic = "/failure/radar"
-	label = "failureradar"
-)
-
-func Topic() string {
-	return "/failure/radar"
-}
-
-func Label() string {
-	return label
-}
-
-func Rule(data *std_msgs.Bool) string {
-	if data.Data {
-		return label
-	}
-	return ""
-}

trigger/kinglong/fault_info/ErrorCode/main/ErrorCode.go

@@ -1,29 +0,0 @@
-package main
-
-import (
-	"cicv-data-closedloop/kinglong_msgs"
-	"fmt"
-)
-
-func Topic() string {
-	return "/fault_info"
-}
-
-// 禁止存在下划线_
-func Label() string {
-	return "ErrorCode"
-}
-
-func Rule(data *kinglong_msgs.FaultVec) string {
-	defer func() {
-		if r := recover(); r != nil {
-			fmt.Println("Recovered from panic:", r)
-		}
-	}()
-	for _, infoVec := range data.InfoVec {
-		if infoVec.ErrorCode != 0 {
-			return Label()
-		}
-	}
-	return ""
-}

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

@@ -1,25 +0,0 @@
-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

@@ -1,24 +0,0 @@
-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

@@ -1,23 +0,0 @@
-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

@@ -1,23 +0,0 @@
-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

@@ -1,24 +0,0 @@
-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

@@ -1,33 +0,0 @@
-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 && math.Abs(velocityX) > 0.5 {
-			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 <= 5 {
-				return "TTC"
-			}
-		}
-	}
-	return ""
-}
-
-// DegreesToRadians 计算角度转弧度
-func DegreesToRadians(degrees float64) float64 {
-	return degrees * math.Pi / 180.0
-}