add ExcessiveSpeedWhenDownhill trigger

aarch64/pjisuv/master/service/produce_window.go

@@ -40,9 +40,11 @@ var (
 	//Yowslice          = make([]float64, 0)
 	//AngleSlice        = make([][]float64, 0)
 	// /tpperception
-	ObjDicOfTpperception      = make(map[uint32][][]float32)
-	objTypeDicOfTpperception  = make(map[uint32]uint8)
-	objSpeedDicOfTpperception = make(map[uint32]float64)
+	Frame                     float32 = 0.0
+	ObjDicOfTpperception              = make(map[uint32][][]float32)
+	objTypeDicOfTpperception          = make(map[uint32]uint8)
+	objSpeedDicOfTpperception         = make(map[uint32]float64)
+	PreCloseTargetSlice               = []uint32{}
 	// /pji_control_pub
 	numCountPjiControlCommandOfPjControlPub int
 	egoSteeringCmdOfPjControlPub            []float64
@@ -648,7 +650,7 @@ func ProduceWindow() {
 						shareVars.Store("PositionYOfCicvLocation", data.PositionY)
 						shareVars.Store("Latitude", data.Latitude)
 						shareVars.Store("Longitude", data.Longitude)
-						/*用于 陡坡-DescendingSteepHill/ClimbingSteepHill触发器
+						/*用于 陡坡-DescendingSteepHill/ClimbingSteepHill 侧翻预警-RolloverWarning 触发器
 						if NumOfCicvLocation%10==0{
 							AngleSlice[0] = append(AngleSlice[0], data.Qx)
 							AngleSlice[1] = append(AngleSlice[1], data.Qy)
@@ -1522,7 +1524,7 @@ func ProduceWindow() {
 								continue
 							}
 							if _, ok := ObjDicOfTpperception[obj.Id]; !ok {
-								ObjDicOfTpperception[obj.Id] = [][]float32{{}, {}, {}, {}, {}}
+								ObjDicOfTpperception[obj.Id] = [][]float32{{}, {}, {}, {}, {}, {}}
 							}
 							ObjDicOfTpperception[obj.Id][0] = append(ObjDicOfTpperception[obj.Id][0], obj.X)
 							ObjDicOfTpperception[obj.Id][1] = append(ObjDicOfTpperception[obj.Id][1], obj.Y)
@@ -1530,6 +1532,7 @@ func ProduceWindow() {
 							absspeed := math.Sqrt(math.Pow(float64(obj.Vxabs), 2) + math.Pow(float64(obj.Vyabs), 2))
 							ObjDicOfTpperception[obj.Id][3] = append(ObjDicOfTpperception[obj.Id][3], float32(absspeed))
 							ObjDicOfTpperception[obj.Id][4] = append(ObjDicOfTpperception[obj.Id][4], obj.Heading)
+							ObjDicOfTpperception[obj.Id][5] = append(ObjDicOfTpperception[obj.Id][5], Frame)
 
 							objTypeDicOfTpperception[obj.Id] = obj.Type
 							objSpeedDicOfTpperception[obj.Id] = math.Pow(math.Pow(float64(obj.Vxabs), 2)+math.Pow(float64(obj.Vyabs), 2), 0.5)
@@ -1537,6 +1540,7 @@ func ProduceWindow() {
 						shareVars.Store("ObjDicOfTpperception", ObjDicOfTpperception)
 						shareVars.Store("ObjTypeDicOfTpperception", objTypeDicOfTpperception)
 						shareVars.Store("ObjSpeedDicOfTpperception", objSpeedDicOfTpperception)
+						Frame++
 					},
 				})
 				if err == nil {

trigger/pjisuv/cicv_location/ExcessiveSpeedWhenDownhill/main/ExcessiveSpeedWhenDownhill.go

@@ -0,0 +1,72 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+/*
+def callback_cicv_location(data):
+    global angular_velocity_z
+    global Ego_position_x
+    global Ego_position_y
+    global Ego_yaw
+
+
+    Ego_position_x=data.position_x
+    Ego_position_y=data.position_y
+    Ego_yaw=data.yaw
+    #print(Ego_yaw)
+    angular_velocity_z=data.angular_velocity_z
+    #print(angular_velocity_z)
+    if abs(angular_velocity_z)>=27:
+        event_label='overswing' #横摆角速度过大
+        print(event_label)
+
+*/
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+func Label() string {
+	return "ExcessiveSpeedWhenDownhill"
+}
+
+var (
+	count     int = 0
+	threshold     = -0.05
+)
+
+func QuaternionToEuler(x, y, z, w float64) float64 {
+	// 归一化四元数
+	length := math.Sqrt(x*x + y*y + z*z + w*w)
+	x /= length
+	y /= length
+	z /= length
+	w /= length
+	// 计算欧拉角
+	pitch := math.Asin(2 * (w*y - z*x))
+	return pitch
+}
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	if count%10 == 0 {
+
+		OutsideWorkshopFlag, ok1 := shareVars.Load("OutsideWorkshopFlag")
+		if ok1 && OutsideWorkshopFlag.(bool) {
+			if math.Abs(data.AngularVelocityZ) >= 17.0 && QuaternionToEuler(data.Qx, data.Qy, data.Qz, data.Qw) <= threshold {
+				count = 1
+				return Label()
+			}
+		}
+	}
+	count++
+	return ""
+}

trigger/pjisuv/cicv_location/ExcessiveSpeedWhenUphill/main/ExcessiveSpeedWhenUphill.go

@@ -0,0 +1,72 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+/*
+def callback_cicv_location(data):
+    global angular_velocity_z
+    global Ego_position_x
+    global Ego_position_y
+    global Ego_yaw
+
+
+    Ego_position_x=data.position_x
+    Ego_position_y=data.position_y
+    Ego_yaw=data.yaw
+    #print(Ego_yaw)
+    angular_velocity_z=data.angular_velocity_z
+    #print(angular_velocity_z)
+    if abs(angular_velocity_z)>=27:
+        event_label='overswing' #横摆角速度过大
+        print(event_label)
+
+*/
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+func Label() string {
+	return "ExcessiveSpeedWhenUphill"
+}
+
+var (
+	count     int = 0
+	threshold     = 0.05
+)
+
+func QuaternionToEuler(x, y, z, w float64) float64 {
+	// 归一化四元数
+	length := math.Sqrt(x*x + y*y + z*z + w*w)
+	x /= length
+	y /= length
+	z /= length
+	w /= length
+	// 计算欧拉角
+	pitch := math.Asin(2 * (w*y - z*x))
+	return pitch
+}
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	if count%10 == 0 {
+
+		OutsideWorkshopFlag, ok1 := shareVars.Load("OutsideWorkshopFlag")
+		if ok1 && OutsideWorkshopFlag.(bool) {
+			if math.Abs(data.AngularVelocityZ) >= 17.0 && QuaternionToEuler(data.Qx, data.Qy, data.Qz, data.Qw) >= threshold {
+				count = 1
+				return Label()
+			}
+		}
+	}
+	count++
+	return ""
+}

trigger/pjisuv/cicv_ticker/CutinWithSightBblock/main/CutinWithSightBblock.go

@@ -0,0 +1,117 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_ticker"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+var (
+	Maxlenobj int32 = 0
+)
+
+// 定时任务触发器固定的
+func Topic() string {
+	return pjisuv_ticker.TickerTopic
+}
+
+// ******* 禁止存在下划线_
+// 触发器标记
+func Label() string {
+	return "CutinWithSightBblock"
+}
+
+func Rule(shareVars *sync.Map) {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	// 1 使用goroutine
+	go func(shareVars *sync.Map) {
+		// 2 定义触发器的间隔时间
+		ticker := time.NewTicker(time.Duration(2) * time.Second)
+		defer ticker.Stop()
+		// 3 运行一个无限循环
+		for {
+			select {
+			// 定时器触发时执行的代码
+			case <-ticker.C:
+				FinalCallback(shareVars)
+
+			}
+		}
+	}(shareVars)
+}
+func isCuttingIn(ObjectList [][]float32, AngularVelocityZ float64) (bool, float32, float32, float64) {
+
+	for i, objY := range ObjectList[1] {
+
+		if math.Abs(float64(objY)) >= 1.8 && math.Abs(AngularVelocityZ) <= 0.6 && ObjectList[0][i] >= 2 {
+			//fmt.Println(objY)
+			for j := 0; j < len(ObjectList[1])-i-1; j++ {
+				objX := ObjectList[0][1+i+j]
+				if math.Abs(float64(ObjectList[1][1+i+j])) <= 0.7 && math.Abs(AngularVelocityZ) <= 0.6 && objX >= 1 {
+					//fmt.Println(objX)
+					return true, ObjectList[5][1+i+j], ObjectList[0][1+i+j], math.Abs(float64(ObjectList[1][1+i+j]))
+				}
+			}
+		}
+	}
+	return false, 0.0, 0.0, 0.0
+}
+func SightBlock(cutobjx float32, cutobjy float64, objId uint32, cutinframe float32, ObjectSlice map[uint32][][]float32) bool {
+
+	for Id, objValue := range ObjectSlice {
+		if Id != objId {
+			for i := 0; i < len(objValue[1]); i++ {
+				if objValue[2][i] == cutinframe {
+					//fmt.Println("yes")
+					objx := objValue[0][i]
+					objy := math.Abs(float64(objValue[1][i]))
+					//fmt.Println(objx)
+					//fmt.Println(objy)
+					diffx := cutobjx - objx
+					diffy := math.Abs(cutobjy - objy)
+					if diffx >= 0 && diffx <= 6 && diffy <= 4.0 {
+						return true
+					}
+				}
+
+			}
+
+		}
+	}
+	return false
+}
+
+func FinalCallback(shareVars *sync.Map) {
+	OutsideWorkshopFlag, ok := shareVars.Load("OutsideWorkshopFlag")
+	ObjDicOfTpperception, ok1 := shareVars.Load("objDicOfTpperception")
+	ObjDic := ObjDicOfTpperception.(map[uint32][][]float32)
+	AngularVelocityZOfCicvLocation, _ := shareVars.Load("AngularVelocityZOfCicvLocation")
+	AngularVelocityZ := AngularVelocityZOfCicvLocation.(float64)
+
+	if ok && ok1 && OutsideWorkshopFlag.(bool) == true {
+		for objId, objValue := range ObjDic {
+			Maxlenobj = max(Maxlenobj, int32(len(objValue[0])))
+			cutinflag, cutinframe, cutobjx, cutobjy := isCuttingIn(objValue, AngularVelocityZ)
+			if cutinflag {
+				if SightBlock(cutobjx, cutobjy, objId, cutinframe, ObjDic) {
+					event_lable := "CutinWithSightBblock"
+					fmt.Println(event_lable)
+					pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
+				}
+			}
+
+			if Maxlenobj >= 100 {
+				ObjDicOfTpperception = make(map[uint32][][]float32)
+				shareVars.Store("ObjDicOfTpperception", ObjDicOfTpperception)
+				Maxlenobj = 0
+			}
+		}
+
+	}
+}

trigger/pjisuv/cicv_ticker/LongTimeParallel/main/LongTimeParallel.go

@@ -0,0 +1,115 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_ticker"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+var (
+	Maxlenobj int32 = 0
+)
+
+// 定时任务触发器固定的
+func Topic() string {
+	return pjisuv_ticker.TickerTopic
+}
+
+// ******* 禁止存在下划线_
+// 触发器标记
+func Label() string {
+	return "LongTimeParallel"
+}
+
+func Rule(shareVars *sync.Map) {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	// 1 使用goroutine
+	go func(shareVars *sync.Map) {
+		// 2 定义触发器的间隔时间
+		ticker := time.NewTicker(time.Duration(2) * time.Second)
+		defer ticker.Stop()
+		// 3 运行一个无限循环
+		for {
+			select {
+			// 定时器触发时执行的代码
+			case <-ticker.C:
+				FinalCallback(shareVars)
+
+			}
+		}
+	}(shareVars)
+}
+func getMaxValue(slice []int) int {
+	max := slice[0]
+	for _, value := range slice {
+		if value > max {
+			max = value
+		}
+	}
+	return max
+}
+func isParallel(ObjectList [][]float32) bool {
+	numParallelslice := []int{0}
+	numParallel := 0
+lable1:
+	for i := 0; i < len(ObjectList[1]); i++ {
+		objx := ObjectList[0][i]
+		objy := ObjectList[1][i]
+		if math.Abs(float64(objx)) <= 2 && math.Abs(float64(objy)) <= 4.5 {
+			for j := 0; j < len(ObjectList[1])-i-1; j++ {
+				objxj := ObjectList[0][1+i+j]
+				objyj := ObjectList[1][1+i+j]
+				if math.Abs(float64(objxj)) <= 2 && math.Abs(float64(objyj)) <= 4.5 {
+					numParallel++
+				} else {
+					i = i + j + 1
+					numParallelslice = append(numParallelslice, numParallel)
+					numParallel = 0
+					continue lable1
+				}
+			}
+			numParallelslice = append(numParallelslice, numParallel)
+			break lable1
+		} else {
+			i++
+		}
+	}
+	maxValue := getMaxValue(numParallelslice)
+	fmt.Println(maxValue)
+	if maxValue >= 100 {
+		return true
+	} else {
+		return false
+	}
+}
+func FinalCallback(shareVars *sync.Map) {
+	OutsideWorkshopFlag, ok := shareVars.Load("OutsideWorkshopFlag")
+	ObjDicOfTpperception, ok1 := shareVars.Load("objDicOfTpperception")
+	ObjDic := ObjDicOfTpperception.(map[uint32][][]float32)
+	AbsSpeed, _ := shareVars.Load("AbsSpeed")
+
+	if ok && ok1 && OutsideWorkshopFlag.(bool) == true {
+		for _, objValue := range ObjDic {
+			Maxlenobj = max(Maxlenobj, int32(len(objValue[0])))
+			if len(ObjDic[0]) <= 10 || !isParallel(objValue) && AbsSpeed.(float64) >= 1 {
+				continue
+			}
+			event_lable := "LongTimeParallel"
+			fmt.Println(event_lable)
+			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
+		}
+
+		if Maxlenobj >= 100 {
+			ObjDicOfTpperception = make(map[uint32][][]float32)
+			shareVars.Store("ObjDicOfTpperception", ObjDicOfTpperception)
+			Maxlenobj = 0
+		}
+	}
+
+}

trigger/pjisuv/cicv_ticker/RolloverWarning/main/RolloverWarning.go

@@ -0,0 +1,91 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_ticker"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+//记得在produce_window.go中将AngleSlice的注释解除！！！
+
+var (
+	threshold float64 = 0.5236 //车辆的横滚角大于 30 度到 45 度时，就存在较高的侧翻风险。
+)
+
+// 定时任务触发器固定的
+func Topic() string {
+	return pjisuv_ticker.TickerTopic
+}
+
+// ******* 禁止存在下划线_
+// 触发器标记
+
+func Label() string {
+	return "RolloverWarning"
+}
+
+func Rule(shareVars *sync.Map) {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	// 1 使用goroutine
+	go func(shareVars *sync.Map) {
+		// 2 定义触发器的间隔时间
+		ticker := time.NewTicker(time.Duration(2) * time.Second)
+		defer ticker.Stop()
+		// 3 运行一个无限循环
+		for {
+			select {
+			// 定时器触发时执行的代码
+			case <-ticker.C:
+				FinalCallback(shareVars)
+			}
+		}
+	}(shareVars)
+}
+
+// QuaternionToEuler 将四元数转换为欧拉角
+func QuaternionToEuler(x, y, z, w float64) float64 {
+	// 归一化四元数
+	length := math.Sqrt(x*x + y*y + z*z + w*w)
+	x /= length
+	y /= length
+	z /= length
+	w /= length
+	// 计算欧拉角
+	roll := math.Atan2(2*(w*x+y*z), 1-2*(x*x+y*y))
+	//pitch := math.Asin(2 * (w*y - z*x))
+	//yaw := math.Atan2(2*(w*z+x*y), 1-2*(y*y+z*z))
+	return roll
+}
+func countChanges(AngleSlice [][]float64) int {
+	num := 0
+	for i := 0; i < len(AngleSlice[1]); i++ {
+		roll := QuaternionToEuler(AngleSlice[0][i], AngleSlice[1][i], AngleSlice[2][i], AngleSlice[3][i])
+		if math.Abs(roll) >= threshold {
+			num++
+		}
+	}
+	return num
+}
+func FinalCallback(shareVars *sync.Map) {
+	OutsideWorkshopFlag, ok := shareVars.Load("OutsideWorkshopFlag")
+	AngleSlice, ok1 := shareVars.Load("AngleSlice")
+
+	if ok && ok1 && OutsideWorkshopFlag.(bool) == true {
+		count := countChanges(AngleSlice.([][]float64))
+		if count >= 5 {
+			event_lable := "RolloverWarning"
+			fmt.Println(event_lable)
+			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
+			AngleSlice = make([][]float64, 0)
+			shareVars.Store("AngleSlice", AngleSlice)
+		}
+
+	}
+
+}

trigger/pjisuv/tpperception/CloseTargetDisappears/main/CloseTargetDisappears.go

@@ -0,0 +1,64 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+func Topic() string {
+	return "/tpperception"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "CloseTargetDisappears"
+}
+func IsDisappears(PreCloseTargetSlice []uint32, perceptionslice []uint32) bool {
+	flag := false
+	if len(PreCloseTargetSlice) > 0 {
+	lable1:
+		for i := 0; i <= len(PreCloseTargetSlice)-1; i++ {
+			PreID := PreCloseTargetSlice[i]
+			for _, perceptionID := range perceptionslice {
+				if PreID == perceptionID {
+					continue lable1
+				}
+			}
+			//fmt.Println("CloseTargetDisappears")
+			return true
+		}
+	}
+	return flag
+}
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionObjects) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	PreCloseTargetSlice, ok := shareVars.Load("PreCloseTargetSlice")
+	OutsideWorkshopFlag, ok3 := shareVars.Load("OutsideWorkshopFlag")
+	if ok && ok3 && OutsideWorkshopFlag.(bool) {
+		NowCloseTargetSlice := []uint32{}
+		perceptionslice := []uint32{}
+		for _, obj := range data.Objs {
+			perceptionslice = append(perceptionslice, obj.Id)
+			if math.Abs(float64(obj.X)) <= 20 && math.Abs(float64(obj.Y)) <= 10 {
+				NowCloseTargetSlice = append(NowCloseTargetSlice, obj.Id)
+			}
+		}
+		if IsDisappears(PreCloseTargetSlice.([]uint32), perceptionslice) {
+			eventLabel := "CloseTargetDisappears"
+			fmt.Println(eventLabel)
+			PreCloseTargetSlice = NowCloseTargetSlice
+			shareVars.Store("PreCloseTargetSlice", PreCloseTargetSlice)
+			return Label()
+		}
+		PreCloseTargetSlice = NowCloseTargetSlice
+		shareVars.Store("PreCloseTargetSlice", PreCloseTargetSlice)
+	}
+
+	return ""
+}