add DescendingSteepHill & ClimbingSteepHill trigger

aarch64/pjisuv/master/service/produce_window.go

@@ -35,8 +35,10 @@ var (
 	cicvLocationTime = time.Now()
 	// -----------------------------共享变量
 	//cicv_location
+	//NumOfCicvLocation = 0
 	AccelXSlice = []float64{}
-	Yowslice    = make([]float64, 0)
+	//Yowslice          = make([]float64, 0)
+	//AngleSlice        = make([][]float64, 0)
 	// /tpperception
 	ObjDicOfTpperception      = make(map[uint32][][]float32)
 	objTypeDicOfTpperception  = make(map[uint32]uint8)
@@ -646,15 +648,27 @@ func ProduceWindow() {
 						shareVars.Store("PositionYOfCicvLocation", data.PositionY)
 						shareVars.Store("Latitude", data.Latitude)
 						shareVars.Store("Longitude", data.Longitude)
-						/*
-							Yowslice = append(Yowslice, data.Yaw)
-							if len(Yowslice) >= 600 {
-								Yowslice = Yowslice[1:]
-							}
-							shareVars.Store("Yowslice", Yowslice)
+						/*用于 陡坡-DescendingSteepHill/ClimbingSteepHill触发器
+						if NumOfCicvLocation%10==0{
+							AngleSlice[0] = append(AngleSlice[0], data.Qx)
+							AngleSlice[1] = append(AngleSlice[1], data.Qy)
+							AngleSlice[2] = append(AngleSlice[2], data.Qz)
+							AngleSlice[3] = append(AngleSlice[3], data.Qw)
+							shareVars.Store("AngleSlice", AngleSlice)
+							NumOfCicvLocation=1
+						}
+						NumOfCicvLocation++
 						*/
-						// 用于判断是否在车间内
 
+						/*用于 掉头TurnAround触发器
+						Yowslice = append(Yowslice, data.Yaw)
+						if len(Yowslice) >= 600 {
+							Yowslice = Yowslice[1:]
+						}
+						shareVars.Store("Yowslice", Yowslice)
+						*/
+
+						// 用于判断是否在车间内
 						if time.Since(cicvLocationTime).Seconds() > 1 {
 							p := Point{x: data.PositionX, y: data.PositionY}
 							OutsideWorkshopFlag := isPointInPolygon(p, vertices) //在车间返回０，不在车间返回１

trigger/pjisuv/cicv_location/CarFollowingTooCloseAtNight/main/CarFollowingTooCloseAtNight.go

@@ -0,0 +1,85 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+var (
+	StartTime int64
+	count1    int64
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "CarFollowingTooCloseAtNight"
+}
+
+func IfObstaclesNearby(shareVars *sync.Map) bool {
+	ObjDicOfTpperception, ok1 := shareVars.Load("objDicOfTpperception")
+	ObjDic := ObjDicOfTpperception.(map[uint32][][]float32)
+	if ok1 {
+		for _, obj := range ObjDic {
+			if obj[0][len(obj[0])-1] <= 15 && obj[0][len(obj[0])-1] >= 3 && (math.Abs(float64(obj[1][len(obj[1])-1]))) <= 2.0 {
+				return true
+
+			}
+		}
+	}
+	return false
+}
+func Ifatnight() bool {
+	// 获取当前时间
+	now := time.Now()
+	later := now.Add(0 * time.Hour)
+	// 获取当前小时
+	hour := later.Hour()
+	// 判断当前时间是白天还是夜晚
+	if hour >= 0 && hour < 5 || hour >= 20 && hour <= 23 {
+		return true
+	} else {
+		return false
+	}
+}
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	if count1%20 == 0 && Ifatnight() {
+		OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+		OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+		if OutsideWorkshopFlag == true {
+			AbsSpeed, _ := shareVars.Load("AbsSpeed")
+			flag := IfObstaclesNearby(shareVars)
+			if AbsSpeed.(float64) > 1 && flag {
+				// 如果之前没有记录开始时间，记录当前时间
+				if StartTime == 0 {
+					StartTime = time.Now().Unix()
+				}
+				// 判断是否持续超过 50s
+				if time.Now().Unix()-StartTime > 3 {
+					count1 = 1
+					return Label()
+
+				}
+			} else {
+				// 如果速度大于 0.1，重置开始时间和停止标志
+				StartTime = 0
+
+			}
+		} else {
+			StartTime = 0
+		}
+	}
+	count1++
+	return ""
+}

trigger/pjisuv/cicv_ticker/ClimbingSteepHill/main/ClimbingSteepHill.go

@@ -0,0 +1,89 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_ticker"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+//记得在produce_window.go中将AngleSlice的注释解除！！！
+
+var (
+	threshold float64 = 0.06 //道路坡度大于6%就可以被认为是陡坡
+)
+
+// 定时任务触发器固定的
+func Topic() string {
+	return pjisuv_ticker.TickerTopic
+}
+
+// ******* 禁止存在下划线_
+// 触发器标记
+
+func Label() string {
+	return "ClimbingSteepHill"
+}
+
+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
+	// 计算欧拉角
+	pitch := math.Asin(2 * (w*y - z*x))
+	return pitch
+}
+func countChanges(AngleSlice [][]float64) int {
+	num := 0
+	for i := 0; i < len(AngleSlice[1]); i++ {
+		pitch := QuaternionToEuler(AngleSlice[0][i], AngleSlice[1][i], AngleSlice[2][i], AngleSlice[3][i])
+		if pitch >= 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 := "ClimbingSteepHill"
+			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/cicv_ticker/DescendingSteepHill/main/DescendingSteepHill.go

@@ -0,0 +1,89 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_ticker"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+//记得在produce_window.go中将AngleSlice的注释解除！！！
+
+var (
+	threshold float64 = -0.06 //道路坡度大于6%就可以被认为是陡坡
+)
+
+// 定时任务触发器固定的
+func Topic() string {
+	return pjisuv_ticker.TickerTopic
+}
+
+// ******* 禁止存在下划线_
+// 触发器标记
+
+func Label() string {
+	return "DescendingSteepHill"
+}
+
+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
+	// 计算欧拉角
+	pitch := math.Asin(2 * (w*y - z*x))
+	return pitch
+}
+func countChanges(AngleSlice [][]float64) int {
+	num := 0
+	for i := 0; i < len(AngleSlice[1]); i++ {
+		pitch := QuaternionToEuler(AngleSlice[0][i], AngleSlice[1][i], AngleSlice[2][i], AngleSlice[3][i])
+		if pitch <= 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 := "DescendingSteepHill"
+			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/cicv_ticker/FrontVehicleCutInFar/main/FrontVehicleCutInFar.go

@@ -77,7 +77,6 @@ func FinalCallback(shareVars *sync.Map) {
 			}
 			event_lable := "FrontVehicleCutInFar"
 			fmt.Println(event_lable)
-			ObjDicOfTpperception = make(map[uint32][][]float32)
 			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
 		}
 

trigger/pjisuv/cicv_ticker/FrontVehicleCutInNear/main/FrontVehicleCutInNear.go

@@ -78,7 +78,7 @@ func FinalCallback(shareVars *sync.Map) {
 			}
 			event_lable := "FrontVehicleCutInNear"
 			fmt.Println(event_lable)
-			ObjDicOfTpperception = make(map[uint32][][]float32)
+
 			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
 		}
 

trigger/pjisuv/cicv_ticker/FrontVehicleCutOutFar/main/FrontVehicleCutOutFar.go

@@ -49,11 +49,11 @@ func isCuttingIn(ObjectList [][]float32, AngularVelocityZ float64) bool {
 
 	for i, objY := range ObjectList[1] {
 
-		if math.Abs(float64(objY)) >= 1.3 && math.Abs(AngularVelocityZ) <= 0.6 && ObjectList[0][i] >= 2 {
+		if math.Abs(float64(objY)) <= 0.9 && 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])) >= 1.7 && math.Abs(AngularVelocityZ) <= 0.6 && objX >= 20 && objX <= 50 {
+				if math.Abs(float64(ObjectList[1][1+i+j])) >= 2.1 && math.Abs(AngularVelocityZ) <= 0.6 && objX >= 20 && objX <= 50 {
 					//fmt.Println(objX)
 					return true
 				}
@@ -77,7 +77,7 @@ func FinalCallback(shareVars *sync.Map) {
 			}
 			event_lable := "FrontVehicleCutOutFar"
 			fmt.Println(event_lable)
-			ObjDicOfTpperception = make(map[uint32][][]float32)
+
 			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
 		}
 

trigger/pjisuv/cicv_ticker/FrontVehicleCutOutNear/main/FrontVehicleCutOutNear.go

@@ -49,11 +49,11 @@ func isCuttingIn(ObjectList [][]float32, AngularVelocityZ float64) bool {
 
 	for i, objY := range ObjectList[1] {
 
-		if math.Abs(float64(objY)) >= 1.3 && math.Abs(AngularVelocityZ) <= 0.6 && ObjectList[0][i] >= 2 {
+		if math.Abs(float64(objY)) <= 0.9 && 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])) >= 1.7 && math.Abs(AngularVelocityZ) <= 0.6 && objX >= 4 && objX <= 20 {
+				if math.Abs(float64(ObjectList[1][1+i+j])) >= 2.1 && math.Abs(AngularVelocityZ) <= 0.6 && objX >= 4 && objX <= 20 {
 					//fmt.Println(objX)
 					return true
 				}
@@ -77,7 +77,7 @@ func FinalCallback(shareVars *sync.Map) {
 			}
 			event_lable := "FrontVehicleCutOutNear"
 			fmt.Println(event_lable)
-			ObjDicOfTpperception = make(map[uint32][][]float32)
+
 			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
 		}
 

trigger/pjisuv/cicv_ticker/VehiclesCutInTogether/main/VehiclesCutInTogether.go

@@ -0,0 +1,92 @@
+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 "VehiclesCutInTogether"
+}
+
+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(3) * time.Second)
+		defer ticker.Stop()
+		// 3 运行一个无限循环
+		for {
+			select {
+			// 定时器触发时执行的代码
+			case <-ticker.C:
+				FinalCallback(shareVars)
+
+			}
+		}
+	}(shareVars)
+}
+func isCuttingIn(ObjectList [][]float32, AngularVelocityZ float64) int {
+
+	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 >= 2 {
+					//fmt.Println(objX)
+					return 1
+				}
+			}
+		}
+	}
+	return 0
+}
+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 {
+		numobjcutin := 0
+		for _, objValue := range ObjDic {
+			Maxlenobj = max(Maxlenobj, int32(len(objValue[0])))
+			numcoutin := isCuttingIn(objValue, AngularVelocityZ)
+			numobjcutin = numobjcutin + numcoutin
+		}
+		if numobjcutin >= 2 {
+			event_lable := "VehiclesCutInTogether"
+			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/VehiclesCutOutTogether/main/VehiclesCutOutTogether.go

@@ -0,0 +1,92 @@
+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 "VehiclesCutOutTogether"
+}
+
+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(3) * time.Second)
+		defer ticker.Stop()
+		// 3 运行一个无限循环
+		for {
+			select {
+			// 定时器触发时执行的代码
+			case <-ticker.C:
+				FinalCallback(shareVars)
+
+			}
+		}
+	}(shareVars)
+}
+func isCuttingIn(ObjectList [][]float32, AngularVelocityZ float64) int {
+
+	for i, objY := range ObjectList[1] {
+
+		if math.Abs(float64(objY)) <= 0.9 && 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])) >= 1.7 && math.Abs(AngularVelocityZ) <= 0.6 && objX >= 2 {
+					//fmt.Println(objX)
+					return 1
+				}
+			}
+		}
+	}
+	return 0
+}
+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 {
+		numobjcutin := 0
+		for _, objValue := range ObjDic {
+			Maxlenobj = max(Maxlenobj, int32(len(objValue[0])))
+			numcoutin := isCuttingIn(objValue, AngularVelocityZ)
+			numobjcutin = numobjcutin + numcoutin
+		}
+		if numobjcutin >= 2 {
+			event_lable := "VehiclesCutOutTogether"
+			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
+		}
+	}
+
+}