modify produce_window.go & RaerVehicleApproch trigger

aarch64/pjisuv/master/service/produce_window.go

@@ -35,7 +35,7 @@ var (
 	cicvLocationTime = time.Now()
 	// -----------------------------共享变量
 	// /tpperception
-	objDicOfTpperception      = make(map[uint32][]float32)
+	ObjDicOfTpperception      = make(map[uint32][][]float32)
 	objTypeDicOfTpperception  = make(map[uint32]uint8)
 	objSpeedDicOfTpperception = make(map[uint32]float64)
 	// /pji_control_pub
@@ -1405,14 +1405,17 @@ func ProduceWindow() {
 							if obj.X <= 5 || math.Abs(float64(obj.Y)) >= 10 {
 								continue
 							}
-							if _, ok := objDicOfTpperception[obj.Id]; !ok {
-								objDicOfTpperception[obj.Id] = []float32{}
+							if _, ok := ObjDicOfTpperception[obj.Id]; !ok {
+								ObjDicOfTpperception[obj.Id] = [][]float32{{}, {}, {}, {}}
 							}
-							objDicOfTpperception[obj.Id] = append(objDicOfTpperception[obj.Id], obj.Y)
+							ObjDicOfTpperception[obj.Id][0] = append(ObjDicOfTpperception[obj.Id][0], obj.X)
+							ObjDicOfTpperception[obj.Id][1] = append(ObjDicOfTpperception[obj.Id][1], obj.Y)
+							ObjDicOfTpperception[obj.Id][2] = append(ObjDicOfTpperception[obj.Id][2], obj.Vxrel)
+							ObjDicOfTpperception[obj.Id][3] = append(ObjDicOfTpperception[obj.Id][3], obj.Vxabs)
 							objTypeDicOfTpperception[obj.Id] = obj.Type
 							objSpeedDicOfTpperception[obj.Id] = math.Pow(math.Pow(float64(obj.Vxabs), 2)+math.Pow(float64(obj.Vyabs), 2), 0.5)
 						}
-						shareVars.Store("ObjDicOfTpperception", objDicOfTpperception)
+						shareVars.Store("ObjDicOfTpperception", ObjDicOfTpperception)
 						shareVars.Store("ObjTypeDicOfTpperception", objTypeDicOfTpperception)
 						shareVars.Store("ObjSpeedDicOfTpperception", objSpeedDicOfTpperception)
 					},

trigger/pjisuv/cicv_ticker/FrontVehicleBrake/main/FrontVehicleBrake.go

@@ -0,0 +1,76 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_ticker"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+// 定时任务触发器固定的
+func Topic() string {
+	return pjisuv_ticker.TickerTopic
+}
+
+// ******* 禁止存在下划线_
+// 触发器标记
+func Label() string {
+	return "FrontVehicleBrake"
+}
+
+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 isBrake(ObjectList [][]float32) bool {
+	for i, speed := range ObjectList[3] {
+
+		if math.Abs(float64(ObjectList[1][i])) <= 1.3 && speed >= 3 && ObjectList[0][i] >= 1.3 {
+			for j := 0; j < len(ObjectList[0])-i-1; j++ {
+				if math.Abs(float64(ObjectList[1][1+i+j])) <= 1.3 && ObjectList[3][1+i+j] <= 1 {
+					return true
+				}
+			}
+		}
+	}
+	return false
+}
+
+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 {
+			if len(ObjDic[0]) <= 10 || !isBrake(objValue) {
+				continue
+			}
+			event_lable := "FrontVehicleBrake"
+			fmt.Println(event_lable)
+			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
+		}
+
+		ObjDicOfTpperception = make(map[uint32][][]float32)
+	}
+
+}

trigger/pjisuv/cicv_ticker/FrontVehicleCutInFar/main/FrontVehicleCutInFar.go

@@ -0,0 +1,81 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_ticker"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+// 定时任务触发器固定的
+func Topic() string {
+	return pjisuv_ticker.TickerTopic
+}
+
+// ******* 禁止存在下划线_
+// 触发器标记
+func Label() string {
+	return "FrontVehicleCutInFar"
+}
+
+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 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 {
+			//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 >= 16 && objX <= 50 {
+					//fmt.Println(objX)
+					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 _, objValue := range ObjDic {
+			if len(ObjDic[0]) <= 10 || !isCuttingIn(objValue, AngularVelocityZ) {
+				continue
+			}
+			event_lable := "FrontVehicleCutInFar"
+			fmt.Println(event_lable)
+			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
+		}
+
+		ObjDicOfTpperception = make(map[uint32][][]float32)
+	}
+
+}

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

@@ -0,0 +1,81 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_ticker"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+// 定时任务触发器固定的
+func Topic() string {
+	return pjisuv_ticker.TickerTopic
+}
+
+// ******* 禁止存在下划线_
+// 触发器标记
+func Label() string {
+	return "FrontVehicleCutInNear"
+}
+
+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 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 {
+			//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 && objX <= 16 {
+					//fmt.Println(objX)
+					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 _, objValue := range ObjDic {
+			if len(ObjDic[0]) <= 4 || !isCuttingIn(objValue, AngularVelocityZ) {
+				continue
+			}
+			event_lable := "FrontVehicleCutInNear"
+			fmt.Println(event_lable)
+			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
+		}
+
+		ObjDicOfTpperception = make(map[uint32][][]float32)
+	}
+
+}

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

@@ -0,0 +1,82 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_ticker"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+// 定时任务触发器固定的
+func Topic() string {
+	return pjisuv_ticker.TickerTopic
+}
+
+// ******* 禁止存在下划线_
+// 触发器标记
+func Label() string {
+	return "FrontVehicleCutOutFar"
+}
+
+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 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 {
+			//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 {
+					//fmt.Println(objX)
+					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 _, objValue := range ObjDic {
+			if len(ObjDic[0]) <= 10 || !isCuttingIn(objValue, AngularVelocityZ) {
+				continue
+			}
+			event_lable := "FrontVehicleCutOutFar"
+			fmt.Println(event_lable)
+			//ObjDicOfTpperception = make(map[uint32][][]float32)
+			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
+		}
+
+		ObjDicOfTpperception = make(map[uint32][][]float32)
+	}
+
+}

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

@@ -0,0 +1,81 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_ticker"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+// 定时任务触发器固定的
+func Topic() string {
+	return pjisuv_ticker.TickerTopic
+}
+
+// ******* 禁止存在下划线_
+// 触发器标记
+func Label() string {
+	return "FrontVehicleCutOutNear"
+}
+
+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 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 {
+			//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 {
+					//fmt.Println(objX)
+					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 _, objValue := range ObjDic {
+			if len(ObjDic[0]) <= 10 || !isCuttingIn(objValue, AngularVelocityZ) {
+				continue
+			}
+			event_lable := "FrontVehicleCutOutNear"
+			fmt.Println(event_lable)
+			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
+		}
+
+		ObjDicOfTpperception = make(map[uint32][][]float32)
+	}
+
+}

trigger/pjisuv/cicv_ticker/RearVehicleApproach/main/RearVehicleApproach.go

@@ -0,0 +1,75 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_ticker"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+// 定时任务触发器固定的
+func Topic() string {
+	return pjisuv_ticker.TickerTopic
+}
+
+// ******* 禁止存在下划线_
+// 触发器标记
+func Label() string {
+	return "RearVehicleApproach"
+}
+
+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 isApproach(ObjectList [][]float32) bool {
+	for i, objX := range ObjectList[0] {
+
+		if math.Abs(float64(ObjectList[1][i])) <= 1.3 && objX <= -10 {
+			for j := 0; j < len(ObjectList[0])-i-1; j++ {
+				if math.Abs(float64(ObjectList[1][1+i+j])) <= 1.3 && ObjectList[0][1+i+j] >= -8 && ObjectList[2][1+i+j] >= 1 {
+					return true
+				}
+			}
+		}
+	}
+	return false
+}
+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 {
+			if len(ObjDic[0]) <= 10 || !isApproach(objValue) {
+				continue
+			}
+			event_lable := "RearVehicleApproach"
+			fmt.Println(event_lable)
+			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
+		}
+
+		ObjDicOfTpperception = make(map[uint32][][]float32)
+	}
+
+}