add FrontVehicleFrequentBrake trigger

aarch64/pjisuv/master/service/produce_window.go

@@ -41,6 +41,7 @@ var (
 	// -----------------------------共享变量
 	//cicv_location
 	//NumOfCicvLocation = 0
+	SpeedSlice  = []float64{}
 	AccelXSlice = []float64{}
 	//Yowslice          = make([]float64, 0)
 	//AngleSlice        = make([][]float64, 0)
@@ -655,6 +656,12 @@ func ProduceWindow() {
 						shareVars.Store("PositionYOfCicvLocation", data.PositionY)
 						shareVars.Store("Latitude", data.Latitude)
 						shareVars.Store("Longitude", data.Longitude)
+						//SpeedSlice 用于自车频繁起停触发器-FrequentStartsAndStops
+						SpeedSlice = append(SpeedSlice, AbsSpeed)
+						if len(SpeedSlice) > 6000 {
+							SpeedSlice = SpeedSlice[1:]
+						}
+						shareVars.Store("SpeedSlice", SpeedSlice)
 						/*用于 陡坡-DescendingSteepHill/ClimbingSteepHill 侧翻预警-RolloverWarning 触发器
 						if NumOfCicvLocation%10==0{
 							AngleSlice[0] = append(AngleSlice[0], data.Qx)

trigger/pjisuv/cicv_location/LongDownhill/main/LongDownhill.go

@@ -0,0 +1,107 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+var (
+	threshold        = -0.05
+	count1           = 0
+	UphillPointSlice = [][]float64{{}, {}}
+	CountNoneUphill  = 0
+)
+
+type Point struct {
+	X float64
+	Y float64
+}
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "LongDownhill"
+}
+
+// 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))
+	if math.IsNaN(pitch) {
+		return 0.0
+	}
+	return pitch
+}
+
+// 计算两点之间的距离（米）
+func distance(point1, point2 Point) float64 {
+
+	d := math.Sqrt((point2.X-point1.X)*(point2.X-point1.X) + (point2.Y-point1.Y)*(point2.Y-point1.Y))
+
+	return d
+}
+
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+	OutsideWorkshopFlag1 := OutsideWorkshopFlag.(bool)
+	if OutsideWorkshopFlag1 == true {
+		if count1%10 == 0 {
+			pitch := QuaternionToEuler(data.Qx, data.Qy, data.Qz, data.Qw)
+			//fmt.Println(pitch)
+			if pitch < threshold {
+				UphillPointSlice[0] = append(UphillPointSlice[0], data.PositionX)
+				UphillPointSlice[1] = append(UphillPointSlice[1], data.PositionY)
+				if len(UphillPointSlice[0]) > 250 {
+
+					P1 := Point{UphillPointSlice[0][0], UphillPointSlice[1][0]}
+					P2 := Point{UphillPointSlice[0][len(UphillPointSlice[0])-1], UphillPointSlice[1][len(UphillPointSlice[1])-1]}
+					distance := distance(P1, P2)
+					if distance >= 100 {
+						event_lable := "LongUphill"
+						fmt.Println(event_lable)
+						CountNoneUphill = 0
+						UphillPointSlice = [][]float64{{}, {}}
+						return Label()
+					}
+
+				}
+			} else {
+				CountNoneUphill++
+				if CountNoneUphill == 3 {
+					if len(UphillPointSlice[0]) > 1 {
+						P1 := Point{UphillPointSlice[0][0], UphillPointSlice[1][0]}
+						P2 := Point{UphillPointSlice[0][len(UphillPointSlice[0])-1], UphillPointSlice[1][len(UphillPointSlice[1])-1]}
+						distance := distance(P1, P2)
+						if distance >= 100 {
+							event_lable := "LongUphill"
+							fmt.Println(event_lable)
+							return Label()
+						}
+						UphillPointSlice = [][]float64{{}, {}}
+					}
+					CountNoneUphill = 0
+				}
+			}
+			count1 = 1
+		}
+		count1++
+	}
+
+	return ""
+}

trigger/pjisuv/cicv_location/LongUphill/main/LongUphill.go

@@ -0,0 +1,107 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+var (
+	threshold        = 0.05
+	count1           = 0
+	UphillPointSlice = [][]float64{{}, {}}
+	CountNoneUphill  = 0
+)
+
+type Point struct {
+	X float64
+	Y float64
+}
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "LongUphill"
+}
+
+// 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))
+	if math.IsNaN(pitch) {
+		return 0.0
+	}
+	return pitch
+}
+
+// 计算两点之间的距离（米）
+func distance(point1, point2 Point) float64 {
+
+	d := math.Sqrt((point2.X-point1.X)*(point2.X-point1.X) + (point2.Y-point1.Y)*(point2.Y-point1.Y))
+
+	return d
+}
+
+func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+	OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+	if OutsideWorkshopFlag == true {
+		if count1%10 == 0 {
+			pitch := QuaternionToEuler(data.Qx, data.Qy, data.Qz, data.Qw)
+			//fmt.Println(pitch)
+			if pitch >= threshold {
+				UphillPointSlice[0] = append(UphillPointSlice[0], data.PositionX)
+				UphillPointSlice[1] = append(UphillPointSlice[1], data.PositionY)
+				if len(UphillPointSlice[0]) > 250 {
+
+					P1 := Point{UphillPointSlice[0][0], UphillPointSlice[1][0]}
+					P2 := Point{UphillPointSlice[0][len(UphillPointSlice[0])-1], UphillPointSlice[1][len(UphillPointSlice[1])-1]}
+					distance := distance(P1, P2)
+					if distance >= 100 {
+						event_lable := "LongUphill"
+						fmt.Println(event_lable)
+						CountNoneUphill = 0
+						UphillPointSlice = [][]float64{{}, {}}
+						return Label()
+					}
+				}
+			} else {
+				CountNoneUphill++
+				if CountNoneUphill == 3 {
+					if len(UphillPointSlice[0]) > 1 {
+						P1 := Point{UphillPointSlice[0][0], UphillPointSlice[1][0]}
+						P2 := Point{UphillPointSlice[0][len(UphillPointSlice[0])-1], UphillPointSlice[1][len(UphillPointSlice[1])-1]}
+						distance := distance(P1, P2)
+						if distance >= 100 {
+							event_lable := "LongUphill"
+							fmt.Println(event_lable)
+							return Label()
+						}
+
+						UphillPointSlice = [][]float64{{}, {}}
+					}
+					CountNoneUphill = 0
+				}
+			}
+			count1 = 1
+		}
+		count1++
+	}
+
+	return ""
+}

trigger/pjisuv/cicv_ticker/FrequentStartsAndStops/main/FrequentStartsAndStops.go

@@ -0,0 +1,83 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_ticker"
+	"fmt"
+	"sync"
+	"time"
+)
+
+// 定时任务触发器固定的
+func Topic() string {
+	return pjisuv_ticker.TickerTopic
+}
+
+// ******* 禁止存在下划线_
+// 触发器标记
+
+func Label() string {
+	return "FrequentStartsAndStops"
+}
+
+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(6) * time.Second)
+		defer ticker.Stop()
+		// 3 运行一个无限循环
+		for {
+			select {
+			// 定时器触发时执行的代码
+			case <-ticker.C:
+				FinalCallback(shareVars)
+
+			}
+		}
+	}(shareVars)
+}
+
+func countChanges(slice []float64) int {
+	count := 0
+lable1:
+	for i := 0; i < len(slice); {
+		if slice[i] >= 2 {
+			for j := 0; j < len(slice)-i-1; j++ {
+				if slice[1+i+j] <= 0.5 {
+					count++
+					//fmt.Println("stop!!")
+					i = i + j + 1
+					continue lable1
+				}
+			}
+			break lable1
+		} else {
+			i++
+		}
+
+	}
+	return count
+}
+
+func FinalCallback(shareVars *sync.Map) {
+	OutsideWorkshopFlag, ok := shareVars.Load("OutsideWorkshopFlag")
+	SpeedSlice, ok1 := shareVars.Load("SpeedSlice")
+
+	if ok && ok1 && OutsideWorkshopFlag.(bool) == true {
+		count := countChanges(SpeedSlice.([]float64))
+		if count >= 3 {
+			event_lable := "FrequentStartsAndStops"
+			fmt.Println(event_lable)
+			SpeedSlice = []float64{}
+			shareVars.Store("SpeedSlice", SpeedSlice)
+			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
+		}
+
+	}
+
+}

trigger/pjisuv/cicv_ticker/FrontVehicleFrequentBrake/main/FrontVehicleFrequentBrake.go

@@ -0,0 +1,100 @@
+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 "FrontVehicleFrequentBrake"
+}
+
+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 countChanges(slice [][]float32) int {
+	count := 0
+lable1:
+	for i := 0; i < len(slice[3]); {
+		xi := slice[0][i]
+		yi := slice[1][i]
+		speedi := slice[3][i]
+		if math.Abs(float64(yi)) <= 1.5 && speedi >= 3 && xi >= 1.5 {
+			for j := 0; j < len(slice[1])-i-1; j++ {
+				xij := slice[0][1+i+j]
+				yij := slice[1][1+i+j]
+				speedij := slice[3][1+i+j]
+				if math.Abs(float64(yij)) <= 1.5 && speedij <= 1 && xij >= 1.5 {
+					count++
+					//fmt.Println("here!!")
+					i = i + j + 1
+					continue lable1
+				}
+			}
+			break lable1
+		} else {
+			i++
+		}
+
+	}
+	return count
+}
+
+func FinalCallback(shareVars *sync.Map) {
+	OutsideWorkshopFlag, ok := shareVars.Load("OutsideWorkshopFlag")
+	ObjDicOfTpperception, ok1 := shareVars.Load("objDicOfTpperception")
+	ObjDic := ObjDicOfTpperception.(map[uint32][][]float32)
+
+	if ok && ok1 && OutsideWorkshopFlag.(bool) == true {
+		for _, objValue := range ObjDic {
+			Maxlenobj = max(Maxlenobj, int32(len(objValue[0])))
+			if len(ObjDic[0]) <= 10 || countChanges(objValue) < 2 {
+				continue
+			}
+			event_lable := "FrontVehicleFrequentBrake"
+			fmt.Println(event_lable)
+			ObjDicOfTpperception = make(map[uint32][][]float32)
+			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/FrontVehiclesFrequentChangeLane/main/FrontVehiclesFrequentChangeLane.go

@@ -0,0 +1,98 @@
+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 "FrontVehiclesFrequentChangeLane"
+}
+
+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(4) * time.Second)
+		defer ticker.Stop()
+		// 3 运行一个无限循环
+		for {
+			select {
+			// 定时器触发时执行的代码
+			case <-ticker.C:
+				FinalCallback(shareVars)
+
+			}
+		}
+	}(shareVars)
+}
+func countChanges(slice [][]float32, AngularVelocityZOfCicvLocation float64) int {
+	count := 0
+lable1:
+	for i := 0; i < len(slice[1]); {
+		xi := slice[0][i]
+		yi := slice[1][i]
+		if (yi >= 1.8 || yi < 0.7) && xi >= 2 && math.Abs(AngularVelocityZOfCicvLocation) <= 1.6 {
+			for j := 0; j < len(slice[1])-i-1; j++ {
+				xij := slice[0][1+i+j]
+				yij := slice[1][1+i+j]
+				if ((yi >= 1.8 && yij <= 0.7) || (yi < 0.7 && yij >= 1.8)) && xij >= 2 && math.Abs(AngularVelocityZOfCicvLocation) <= 1.6 {
+					count++
+					//fmt.Println("here!!")
+					i = i + j + 1
+					continue lable1
+				}
+			}
+			break lable1
+		} else {
+			i++
+		}
+
+	}
+	return count
+}
+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 _, objValue := range ObjDic {
+			Maxlenobj = max(Maxlenobj, int32(len(objValue[0])))
+			if len(ObjDic[0]) <= 10 || countChanges(objValue, AngularVelocityZ) < 2 {
+				continue
+			}
+			event_lable := "FrontVehiclesFrequentChangeLane"
+			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
+		}
+	}
+
+}