Merge remote-tracking branch 'gogs/master'

aarch64/pjisuv/master/service/produce_window.go

@@ -48,6 +48,7 @@ var (
 	numCountDataReadOfDataRead int
 	egoSteeringRealOfDataRead  []float64
 	egoThrottleRealOfDataRead  []float64
+	GearPosSlice               = []int16{}
 	// --------------------------------------------------
 	shareVars           = new(sync.Map)
 	saveTimeWindowMutex sync.Mutex // 保存时间窗口需要锁，防止数据竟态
@@ -1794,10 +1795,12 @@ func ProduceWindow() {
 							egoThrottleRealOfDataRead = append(egoThrottleRealOfDataRead, data.AccPed2)
 							numCountDataReadOfDataRead = 0
 						}
+						GearPosSlice = append(GearPosSlice, data.GearPos)
 						shareVars.Store("NumCountDataReadOfDataRead", numCountDataReadOfDataRead)
 						shareVars.Store("EgoSteeringRealOfDataRead", egoSteeringRealOfDataRead)
 						shareVars.Store("EgoThrottleRealOfDataRead", egoThrottleRealOfDataRead)
 						shareVars.Store("ActStrWhAngOfDataRead", data.ActStrWhAng)
+						shareVars.Store("GearPosSlice", GearPosSlice)
 					},
 				})
 				if err == nil {

trigger/pjisuv/cicv_location/AbnormalParking/main/AbnormalParking.go

@@ -76,6 +76,7 @@ func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string
 	if count1%10 == 0 {
 		OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
 		OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+
 		Automode, _ := shareVars.Load("AutomodeOfPjVehicleFdbPub")
 		Longitude, _ := shareVars.Load("EndPointX")
 		Latitude, _ := shareVars.Load("EndPointY")
@@ -86,7 +87,8 @@ func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string
 		IsTrafficLight = IfEnter(pointlist1, 30.0, data.Latitude, data.Longitude)
 		IsEndPoint = IfEnter(pointlist2, 5.0, data.Latitude, data.Longitude)
 		if Automode == 1 && !IsTrafficLight && !IsEndPoint && OutsideWorkshopFlag == true {
-			if data.VelocityX < 0.5 {
+			AbsSpeed, _ := shareVars.Load("AbsSpeed")
+			if AbsSpeed.(float64) < 0.5 {
 				// 如果之前没有记录开始时间，记录当前时间
 				if StartTime == 0 {
 					StartTime = time.Now().Unix()

trigger/pjisuv/cicv_location/AbnormalStopOnCurve/main/AbnormalStopOnCurve.go

@@ -87,8 +87,8 @@ func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string
 		IsCurve = IfEnter(pointlist1, 30.0, data.Latitude, data.Longitude)
 		IsEndPoint = IfEnter(pointlist2, 5.0, data.Latitude, data.Longitude)
 		if Automode == 1 && IsCurve && !IsEndPoint {
-			AbsSpeed := math.Sqrt(math.Pow(data.VelocityX, 2) + math.Pow(data.VelocityY, 2))
-			if AbsSpeed < 0.5 {
+			AbsSpeed, _ := shareVars.Load("AbsSpeed")
+			if AbsSpeed.(float64) < 0.5 {
 				// 如果之前没有记录开始时间，记录当前时间
 				if StartTime == 0 {
 					StartTime = time.Now().Unix()

trigger/pjisuv/cicv_location/AbnormalStopOnJunction/main/AbnormalStopOnJunction.go

@@ -93,8 +93,8 @@ func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string
 		IsJunction = IfEnter(PointJunctionList, 25.0, data.Latitude, data.Longitude)
 		IsEndPoint = IfEnter(pointlist2, 5.0, data.Latitude, data.Longitude)
 		if Automode == 1 && IsJunction && !IsEndPoint {
-			AbsSpeed := math.Sqrt(math.Pow(data.VelocityX, 2) + math.Pow(data.VelocityY, 2))
-			if AbsSpeed < 0.5 {
+			AbsSpeed, _ := shareVars.Load("AbsSpeed")
+			if AbsSpeed.(float64) < 0.5 {
 				// 如果之前没有记录开始时间，记录当前时间
 				if StartTime == 0 {
 					StartTime = time.Now().Unix()

trigger/pjisuv/cicv_location/CannotBypassObstacles/main/CannotBypassObstacles.go

@@ -0,0 +1,128 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+type Point struct {
+	Latitude  float64
+	Longitude float64
+}
+
+var (
+	StartTime      int64
+	IsStopped      bool
+	IsEndPoint     bool
+	IsTrafficLight bool
+	count1         int64
+	//定义园区４个信号灯的坐标
+	point2     = Point{39.72975930689718, 116.48861102824081}
+	point3     = Point{39.7288805296616, 116.48812315228867}
+	point4     = Point{39.73061430369551, 116.49225103553502}
+	point5     = Point{39.73077491578002, 116.49060085035634}
+	EndPoint   = Point{0, 0}
+	pointlist1 = []Point{point2, point3, point4, point5}
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "CannotBypassObstacles"
+}
+
+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 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] <= 13 && obj[0][len(obj[0])-1] >= 3 && (math.Abs(float64(obj[1][len(obj[1])-1]))) <= 6 {
+				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%10 == 0 {
+		OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+		OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+		Automode, _ := shareVars.Load("AutomodeOfPjVehicleFdbPub")
+		Longitude, _ := shareVars.Load("EndPointX")
+		Latitude, _ := shareVars.Load("EndPointY")
+
+		Automode = Automode.(int16)
+		EndPoint.Longitude = Longitude.(float64)
+		EndPoint.Latitude = Latitude.(float64)
+		pointlist2 := []Point{EndPoint}
+		IsTrafficLight = IfEnter(pointlist1, 30.0, data.Latitude, data.Longitude)
+		IsEndPoint = IfEnter(pointlist2, 5.0, data.Latitude, data.Longitude)
+		if Automode == 1 && !IsTrafficLight && !IsEndPoint && OutsideWorkshopFlag == true {
+			AbsSpeed, _ := shareVars.Load("AbsSpeed")
+			flag := IfObstaclesNearby(shareVars)
+			if AbsSpeed.(float64) < 0.5 && flag {
+				// 如果之前没有记录开始时间，记录当前时间
+				if StartTime == 0 {
+					StartTime = time.Now().Unix()
+				}
+				// 判断是否持续超过 50s
+				if time.Now().Unix()-StartTime > 5 {
+					if !IsStopped {
+						IsStopped = true
+						return Label()
+					}
+				}
+			} else {
+				// 如果速度大于 0.1，重置开始时间和停止标志
+				StartTime = 0
+				IsStopped = false
+			}
+		} else {
+			StartTime = 0
+			IsStopped = false
+		}
+	}
+	count1++
+	return ""
+}

trigger/pjisuv/cicv_ticker/FrontVehicleBrakeInCurve/main/FrontVehicleBrakeInCurve.go

@@ -0,0 +1,126 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_ticker"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+type Point struct {
+	Latitude  float64
+	Longitude float64
+}
+
+var (
+	Maxlenobj  int32 = 0
+	pointcurve       = Point{39.73004426154644, 116.49248639463602}
+	pointlist1       = []Point{pointcurve}
+)
+
+// 定时任务触发器固定的
+func Topic() string {
+	return pjisuv_ticker.TickerTopic
+}
+
+// ******* 禁止存在下划线_
+// 触发器标记
+func Label() string {
+	return "FrontVehicleBrakeInCurve"
+}
+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 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 isBrake(ObjectList [][]float32) bool {
+	for i, speed := range ObjectList[3] {
+
+		if math.Abs(float64(ObjectList[1][i])) <= 5.3 && speed >= 6/3.6 && ObjectList[0][i] >= 1.3 {
+			for j := 0; j < len(ObjectList[0])-i-1; j++ {
+				if math.Abs(float64(ObjectList[1][1+i+j])) <= 5.3 && ObjectList[3][1+i+j] <= 1/3.6 {
+					return true
+				}
+			}
+		}
+	}
+	return false
+}
+
+func FinalCallback(shareVars *sync.Map) {
+	OutsideWorkshopFlag, ok := shareVars.Load("OutsideWorkshopFlag")
+	ObjDicOfTpperception, ok1 := shareVars.Load("objDicOfTpperception")
+	Latitude, ok2 := shareVars.Load("Latitude")
+	Longitude, ok3 := shareVars.Load("Longitude")
+	ObjDic := ObjDicOfTpperception.(map[uint32][][]float32)
+
+	if ok && ok1 && ok2 && ok3 && OutsideWorkshopFlag.(bool) == true {
+		enterflag := IfEnter(pointlist1, 30.0, Latitude.(float64), Longitude.(float64))
+		if enterflag {
+			for _, objValue := range ObjDic {
+				Maxlenobj = max(Maxlenobj, int32(len(objValue[0])))
+				if len(ObjDic[0]) <= 10 || !isBrake(objValue) {
+					continue
+				}
+				event_lable := "FrontVehicleBrakeInCurve"
+				fmt.Println(event_lable)
+				pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
+			}
+		}
+		if Maxlenobj >= 60 {
+			ObjDicOfTpperception = make(map[uint32][][]float32)
+			shareVars.Store("ObjDicOfTpperception", ObjDicOfTpperception)
+			Maxlenobj = 0
+		}
+	}
+
+}

trigger/pjisuv/cicv_ticker/FrontVehicleBrakeInJunction/main/FrontVehicleBrakeInJunction.go

@@ -0,0 +1,133 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_ticker"
+	"fmt"
+	"math"
+	"sync"
+	"time"
+)
+
+type Point struct {
+	Latitude  float64
+	Longitude float64
+}
+
+var (
+	Maxlenobj int32 = 0
+	//定义园区Ｔ字路口的经纬度坐标值
+	point3 = Point{39.73040966605621, 116.48995329696209}
+	point4 = Point{39.73083727413453, 116.49079780188244}
+	point5 = Point{39.72976753711939, 116.49043130389033}
+	point6 = Point{39.73012466515933, 116.49128381717591}
+	point7 = Point{39.729251498328246, 116.49077484625299}
+	point8 = Point{39.72964529630643, 116.49164592200161}
+
+	PointJunctionList = []Point{point3, point4, point5, point6, point7, point8}
+)
+
+// 定时任务触发器固定的
+func Topic() string {
+	return pjisuv_ticker.TickerTopic
+}
+
+// ******* 禁止存在下划线_
+// 触发器标记
+func Label() string {
+	return "FrontVehicleBrakeInJunction"
+}
+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 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 isBrake(ObjectList [][]float32) bool {
+	for i, speed := range ObjectList[3] {
+
+		if math.Abs(float64(ObjectList[1][i])) <= 5.3 && speed >= 6/3.6 && ObjectList[0][i] >= 1.3 {
+			for j := 0; j < len(ObjectList[0])-i-1; j++ {
+				if math.Abs(float64(ObjectList[1][1+i+j])) <= 5.3 && ObjectList[3][1+i+j] <= 1/3.6 {
+					return true
+				}
+			}
+		}
+	}
+	return false
+}
+
+func FinalCallback(shareVars *sync.Map) {
+	OutsideWorkshopFlag, ok := shareVars.Load("OutsideWorkshopFlag")
+	ObjDicOfTpperception, ok1 := shareVars.Load("objDicOfTpperception")
+	Latitude, ok2 := shareVars.Load("Latitude")
+	Longitude, ok3 := shareVars.Load("Longitude")
+	ObjDic := ObjDicOfTpperception.(map[uint32][][]float32)
+
+	if ok && ok1 && ok2 && ok3 && OutsideWorkshopFlag.(bool) == true {
+		enterflag := IfEnter(PointJunctionList, 20.0, Latitude.(float64), Longitude.(float64))
+		if enterflag {
+			for _, objValue := range ObjDic {
+				Maxlenobj = max(Maxlenobj, int32(len(objValue[0])))
+				if len(ObjDic[0]) <= 10 || !isBrake(objValue) {
+					continue
+				}
+				event_lable := "FrontVehicleBrakeInJunction"
+				fmt.Println(event_lable)
+				pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
+			}
+		}
+		if Maxlenobj >= 60 {
+			ObjDicOfTpperception = make(map[uint32][][]float32)
+			shareVars.Store("ObjDicOfTpperception", ObjDicOfTpperception)
+			Maxlenobj = 0
+		}
+	}
+
+}

trigger/pjisuv/cicv_ticker/GearJump/main/GearJump.go

@@ -0,0 +1,71 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_ticker"
+	"fmt"
+	"sync"
+	"time"
+)
+
+// 定时任务触发器固定的
+func Topic() string {
+	return pjisuv_ticker.TickerTopic
+}
+
+// ******* 禁止存在下划线_
+// 触发器标记
+
+func Label() string {
+	return "GearJump"
+}
+
+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 []int16) int {
+	count := 0
+	for i := 0; i < len(slice)-1; i++ {
+		if slice[i] != slice[i+1] {
+			count++
+		}
+	}
+	return count
+}
+
+func FinalCallback(shareVars *sync.Map) {
+	OutsideWorkshopFlag, ok := shareVars.Load("OutsideWorkshopFlag")
+	GearPosSlice, ok1 := shareVars.Load("GearPosSlice")
+
+	if ok && ok1 && OutsideWorkshopFlag.(bool) == true {
+		count := countChanges(GearPosSlice.([]int16))
+		if count >= 3 {
+			event_lable := "GearJump"
+			fmt.Println(event_lable)
+			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
+		}
+
+		GearPosSlice = []int16{}
+		shareVars.Store("AccelXSlice", GearPosSlice)
+
+	}
+
+}