Bladeren bron

add turnaround trigger

zwh 9 maanden geleden
bovenliggende
commit
b2e0af1305

+ 9 - 1
aarch64/pjisuv/master/service/produce_window.go

@@ -36,6 +36,7 @@ var (
 	// -----------------------------共享变量
 	//cicv_location
 	AccelXSlice = []float64{}
+	Yowslice    = make([]float64, 0)
 	// /tpperception
 	ObjDicOfTpperception      = make(map[uint32][][]float32)
 	objTypeDicOfTpperception  = make(map[uint32]uint8)
@@ -645,8 +646,15 @@ 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)
+						*/
 						// 用于判断是否在车间内
+
 						if time.Since(cicvLocationTime).Seconds() > 1 {
 							p := Point{x: data.PositionX, y: data.PositionY}
 							OutsideWorkshopFlag := isPointInPolygon(p, vertices) //在车间返回0,不在车间返回1

+ 72 - 0
trigger/pjisuv/cicv_location/CarFollowingTooCloseHigh/main/CarFollowingTooCloseHigh.go

@@ -0,0 +1,72 @@
+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 "CarFollowingTooCloseHigh"
+}
+
+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 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 {
+		OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+		OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+		if OutsideWorkshopFlag == true {
+			AbsSpeed, _ := shareVars.Load("AbsSpeed")
+			flag := IfObstaclesNearby(shareVars)
+			if AbsSpeed.(float64) > 4 && 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 ""
+}

+ 71 - 0
trigger/pjisuv/cicv_location/CarFollowingTooCloseLow/main/CarFollowingTooCloseLow.go

@@ -0,0 +1,71 @@
+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 "CarFollowingTooCloseLow"
+}
+
+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] <= 12 && obj[0][len(obj[0])-1] >= 3 && (math.Abs(float64(obj[1][len(obj[1])-1]))) <= 2.0 {
+				return true
+
+			}
+		}
+	}
+	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 {
+		OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+		OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+		if OutsideWorkshopFlag == true {
+			AbsSpeed, _ := shareVars.Load("AbsSpeed")
+			flag := IfObstaclesNearby(shareVars)
+			if AbsSpeed.(float64) >= 1 && AbsSpeed.(float64) <= 3 && 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 ""
+}

+ 77 - 0
trigger/pjisuv/cicv_location/FindLongCurve/main/FindLongCurve.go

@@ -0,0 +1,77 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// todo 禁止存在下划线_
+func Label() string {
+	return "FindLongCurve"
+}
+
+type Point struct {
+	Latitude  float64
+	Longitude float64
+}
+
+var (
+	count1 int = 0
+
+	//定义弯道坐标
+	point = Point{39.73004426154644, 116.49248639463602}
+)
+
+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%10 == 0 {
+
+		enterflag := IfEnter(point, 25.0, data.Latitude, data.Longitude)
+		AbsSpeed, _ := shareVars.Load("AbsSpeed")
+		if enterflag && AbsSpeed.(float64) > 1 {
+			//eventLabel := "FindTrafficLight"
+			//fmt.Println(eventLabel)
+			return "FindLongCurve"
+		}
+	}
+	count1++
+	return ""
+}
+
+func IfEnter(point Point, radius float64, lat, lon float64) bool {
+	// 判断是否进入点列表中的区域
+	point1 := Point{Latitude: lat, Longitude: lon}
+	d := distance(point1, point)
+	if d <= radius {
+		return true
+	}
+
+	return false
+}
+
+// 计算两点之间的距离(米)
+func distance(point1, point2 Point) float64 {
+	// 经纬度转弧度
+	lat1 := point1.Latitude * math.Pi / 180
+	lon1 := point1.Longitude * math.Pi / 180
+	lat2 := point2.Latitude * math.Pi / 180
+	lon2 := point2.Longitude * math.Pi / 180
+
+	// 计算距离
+	dlon := lon2 - lon1
+	dlat := lat2 - lat1
+	a := math.Sin(dlat/2)*math.Sin(dlat/2) + math.Sin(dlon/2)*math.Sin(dlon/2)*math.Cos(lat1)*math.Cos(lat2)
+	c := 2 * math.Atan2(math.Sqrt(a), math.Sqrt(1-a))
+	d := 6371000 * c
+	return d
+}

+ 60 - 0
trigger/pjisuv/cicv_location/OverspeedAtNight/main/OverspeedAtNight.go

@@ -0,0 +1,60 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"sync"
+	"time"
+)
+
+var (
+	threshold float64 = 25 / 3.6
+	count1    int     = 0
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "OverspeedAtNight"
+}
+
+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%100 == 0 {
+		Automode, _ := shareVars.Load("AutomodeOfPjVehicleFdbPub")
+		OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+		Automode = Automode.(int16)
+		if OutsideWorkshopFlag.(bool) && Automode == 1 && Ifatnight() {
+			absspeed, ok := shareVars.Load("AbsSpeed")
+			if ok && absspeed.(float64) >= threshold {
+				eventLabel := "OverspeedAtNight"
+				fmt.Println(eventLabel)
+				count1 = 1
+				return Label()
+			}
+		}
+		count1 = 1
+	}
+	count1++
+	return ""
+}

+ 71 - 0
trigger/pjisuv/cicv_location/RearVehicleFollowingTooCloseHigh/main/RearVehicleFollowingTooCloseHigh.go

@@ -0,0 +1,71 @@
+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 "RearVehicleFollowingTooCloseHigh"
+}
+
+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] >= -12 && obj[0][len(obj[0])-1] <= -1 && (math.Abs(float64(obj[1][len(obj[1])-1]))) <= 2.0 {
+				return true
+
+			}
+		}
+	}
+	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 {
+		OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+		OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+		if OutsideWorkshopFlag == true {
+			AbsSpeed, _ := shareVars.Load("AbsSpeed")
+			flag := IfObstaclesNearby(shareVars)
+			if AbsSpeed.(float64) >= 4 && 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 ""
+}

+ 71 - 0
trigger/pjisuv/cicv_location/RearVehicleFollowingTooCloseLow/main/RearVehicleFollowingTooCloseLow.go

@@ -0,0 +1,71 @@
+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 "RearVehicleFollowingTooCloseLow"
+}
+
+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] >= -12 && obj[0][len(obj[0])-1] <= -1 && (math.Abs(float64(obj[1][len(obj[1])-1]))) <= 2.0 {
+				return true
+
+			}
+		}
+	}
+	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 {
+		OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+		OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+		if OutsideWorkshopFlag == true {
+			AbsSpeed, _ := shareVars.Load("AbsSpeed")
+			flag := IfObstaclesNearby(shareVars)
+			if AbsSpeed.(float64) >= 1 && AbsSpeed.(float64) <= 3 && 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 ""
+}

+ 55 - 0
trigger/pjisuv/cicv_location/TurnAround/main/TurnAround.go

@@ -0,0 +1,55 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+// 记得在produce_window.go中将Yowslice的注释解除!!!
+var (
+	count1 int = 0
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "TurnAround"
+}
+func IfTurnAround(Yowslice []float64) bool {
+	diffangle := math.Abs(Yowslice[0] - Yowslice[len(Yowslice)-1])
+	//fmt.Println(diffangle)
+	if diffangle >= 110 {
+		return true
+	}
+	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%200 == 0 {
+		OutsideWorkshopFlag, ok3 := shareVars.Load("OutsideWorkshopFlag")
+		Yowslice, ok := shareVars.Load("Yowslice")
+		if ok3 && ok && OutsideWorkshopFlag.(bool) {
+			if IfTurnAround(Yowslice.([]float64)) {
+				eventLabel := "TurnAround"
+				fmt.Println(eventLabel)
+				count1 = 1
+				Yowslice = make([]float64, 0)
+				shareVars.Store("Yowslice", Yowslice)
+
+				return Label()
+			}
+		}
+		count1 = 1
+	}
+	count1++
+	return ""
+}

+ 93 - 0
trigger/pjisuv/cicv_ticker/HighSpeedOvertaking/main/HighSpeedOvertaking.go

@@ -0,0 +1,93 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_ticker"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+var (
+	threshold float32 = 15 / 3.6
+	Maxlenobj int32   = 0
+)
+
+// 定时任务触发器固定的
+func Topic() string {
+	return pjisuv_ticker.TickerTopic
+}
+
+// ******* 禁止存在下划线_
+// 触发器标记
+func Label() string {
+	return "HighSpeedOvertaking"
+}
+
+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 IsOvertaken(yaw float64, ObjectList [][]float32) bool {
+	for i, objX := range ObjectList[0] {
+		diffangle := math.Abs(yaw - float64(ObjectList[4][i]))
+
+		if math.Abs(float64(ObjectList[1][i])) <= 4 && objX >= 2.5 && diffangle <= 90 {
+			for j := 0; j < len(ObjectList[0])-i-1; j++ {
+				if math.Abs(float64(ObjectList[1][1+i+j])) <= 4 && ObjectList[0][1+i+j] <= -1.5 {
+					return true
+				}
+			}
+		}
+	}
+
+	return false
+}
+
+func FinalCallback(shareVars *sync.Map) {
+	OutsideWorkshopFlag, ok := shareVars.Load("OutsideWorkshopFlag")
+	AbsSpeed, ok3 := shareVars.Load("AbsSpeed")
+
+	if ok && ok3 && AbsSpeed.(float32) > threshold {
+		ObjDicOfTpperception, okn := shareVars.Load("objDicOfTpperception")
+		ObjDic := ObjDicOfTpperception.(map[uint32][][]float32)
+		Yaw, ok4 := shareVars.Load("YawOfCicvLocation")
+
+		if okn && ok4 && OutsideWorkshopFlag.(bool) == true {
+			for _, objValue := range ObjDic {
+				Maxlenobj = max(Maxlenobj, int32(len(objValue[0])))
+				if len(ObjDic[0]) <= 10 || !IsOvertaken(Yaw.(float64), objValue) {
+					continue
+				}
+				event_lable := "HighSpeedOvertaking"
+				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
+			}
+		}
+	}
+
+}

+ 132 - 0
trigger/pjisuv/cicv_ticker/OvertakingInCurve/main/OvertakingInCurve.go

@@ -0,0 +1,132 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_ticker"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+var (
+	Maxlenobj  int32 = 0
+	pointcurve       = Point{39.73004426154644, 116.49248639463602}
+	pointlist1       = []Point{pointcurve}
+)
+
+type Point struct {
+	Latitude  float64
+	Longitude float64
+}
+
+// 定时任务触发器固定的
+func Topic() string {
+	return pjisuv_ticker.TickerTopic
+}
+
+// ******* 禁止存在下划线_
+// 触发器标记
+func Label() string {
+	return "OvertakingInCurve"
+}
+
+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 IsOvertaken(AbsSpeed float32, ObjectList [][]float32) bool {
+
+	if AbsSpeed > 1.0 {
+		//fmt.Println("yes")
+		//fmt.Println(ObjectList)
+		for i, objX := range ObjectList[0] {
+
+			if math.Abs(float64(ObjectList[1][i])) <= 8 && objX >= 2.5 {
+				for j := 0; j < len(ObjectList[0])-i-1; j++ {
+					if math.Abs(float64(ObjectList[1][1+i+j])) <= 8 && ObjectList[0][1+i+j] <= -1.5 {
+						return true
+					}
+				}
+			}
+		}
+	}
+	return false
+}
+func IfEnter(pointlist []Point, radius float64, lat, lon float64) bool {
+	// 判断是否进入点列表中的区域
+	point1 := Point{Latitude: lat, Longitude: lon}
+	for _, point := range pointlist {
+		d := distance(point1, point)
+		if d <= radius {
+			return true
+		}
+	}
+	return false
+}
+
+// 计算两点之间的距离(米)
+func distance(point1, point2 Point) float64 {
+	// 经纬度转弧度
+	lat1 := point1.Latitude * math.Pi / 180
+	lon1 := point1.Longitude * math.Pi / 180
+	lat2 := point2.Latitude * math.Pi / 180
+	lon2 := point2.Longitude * math.Pi / 180
+
+	// 计算距离
+	dlon := lon2 - lon1
+	dlat := lat2 - lat1
+	a := math.Sin(dlat/2)*math.Sin(dlat/2) + math.Sin(dlon/2)*math.Sin(dlon/2)*math.Cos(lat1)*math.Cos(lat2)
+	c := 2 * math.Atan2(math.Sqrt(a), math.Sqrt(1-a))
+	d := 6371000 * c
+
+	return d
+}
+func FinalCallback(shareVars *sync.Map) {
+	OutsideWorkshopFlag, ok := shareVars.Load("OutsideWorkshopFlag")
+
+	Latitude, ok1 := shareVars.Load("Latitude")
+	Longitude, ok2 := shareVars.Load("Longitude")
+	AbsSpeed, ok3 := shareVars.Load("AbsSpeed")
+
+	if ok && ok1 && ok2 && ok3 && IfEnter(pointlist1, 50, Latitude.(float64), Longitude.(float64)) {
+		ObjDicOfTpperception, okn := shareVars.Load("objDicOfTpperception")
+		ObjDic := ObjDicOfTpperception.(map[uint32][][]float32)
+
+		if okn && OutsideWorkshopFlag.(bool) == true {
+			for _, objValue := range ObjDic {
+				Maxlenobj = max(Maxlenobj, int32(len(objValue[0])))
+				if len(ObjDic[0]) <= 10 || !IsOvertaken(AbsSpeed.(float32), objValue) {
+					continue
+				}
+				event_lable := "OvertakingInCurve"
+				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
+			}
+		}
+	}
+
+}

+ 93 - 0
trigger/pjisuv/cicv_ticker/OvertakingRight/main/OvertakingRight.go

@@ -0,0 +1,93 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_ticker"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+var (
+	threshold float32 = 3.6 / 3.6
+	Maxlenobj int32   = 0
+)
+
+// 定时任务触发器固定的
+func Topic() string {
+	return pjisuv_ticker.TickerTopic
+}
+
+// ******* 禁止存在下划线_
+// 触发器标记
+func Label() string {
+	return "OvertakingRight"
+}
+
+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 IsOvertaken(yaw float64, ObjectList [][]float32) bool {
+	for i, objX := range ObjectList[0] {
+		diffangle := math.Abs(yaw - float64(ObjectList[4][i]))
+
+		if ObjectList[1][i] >= -0.8 && ObjectList[1][i] <= 4 && objX >= 2.5 && diffangle <= 90 {
+			for j := 0; j < len(ObjectList[0])-i-1; j++ {
+				if ObjectList[1][i] >= -0.8 && ObjectList[1][i] <= 4 && ObjectList[0][1+i+j] <= -1.5 {
+					return true
+				}
+			}
+		}
+	}
+
+	return false
+}
+
+func FinalCallback(shareVars *sync.Map) {
+	OutsideWorkshopFlag, ok := shareVars.Load("OutsideWorkshopFlag")
+	AbsSpeed, ok3 := shareVars.Load("AbsSpeed")
+
+	if ok && ok3 && AbsSpeed.(float32) > threshold {
+		ObjDicOfTpperception, okn := shareVars.Load("objDicOfTpperception")
+		ObjDic := ObjDicOfTpperception.(map[uint32][][]float32)
+		Yaw, ok4 := shareVars.Load("YawOfCicvLocation")
+
+		if okn && ok4 && OutsideWorkshopFlag.(bool) == true {
+			for _, objValue := range ObjDic {
+				Maxlenobj = max(Maxlenobj, int32(len(objValue[0])))
+				if len(ObjDic[0]) <= 10 || !IsOvertaken(Yaw.(float64), objValue) {
+					continue
+				}
+				event_lable := "OvertakingRight"
+				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
+			}
+		}
+	}
+
+}