Merge remote-tracking branch 'origin/master'

aarch64/pjisuv/master/service/produce_window.go

@@ -601,7 +601,8 @@ func ProduceWindow() {
 						}
 						subscribersTimeMutexes[i].Unlock()
 						// 更新共享变量
-						AccelXSlice = append(AccelXSlice, data.AccelX)
+						AbsAccel := math.Sqrt(math.Pow(data.AccelX, 2) + math.Pow(data.AccelY, 2))
+						AccelXSlice = append(AccelXSlice, AbsAccel)
 						shareVars.Store("AccelXSlice", AccelXSlice)
 						shareVars.Store("VelocityXOfCicvLocation", data.VelocityX)
 						shareVars.Store("VelocityYOfCicvLocation", data.VelocityY)
@@ -610,6 +611,9 @@ func ProduceWindow() {
 						shareVars.Store("AngularVelocityZOfCicvLocation", data.AngularVelocityZ)
 						shareVars.Store("PositionXOfCicvLocation", data.PositionX)
 						shareVars.Store("PositionYOfCicvLocation", data.PositionY)
+						shareVars.Store("Latitude", data.Latitude)
+						shareVars.Store("Longitude", data.Longitude)
+
 						// 用于判断是否在车间内
 						if time.Since(cicvLocationTime).Seconds() > 1 {
 							p := Point{x: data.PositionX, y: data.PositionY}
@@ -1415,7 +1419,8 @@ func ProduceWindow() {
 							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)
+							absspeed := math.Sqrt(math.Pow(float64(obj.Vxabs), 2) + math.Pow(float64(obj.Vyabs), 2))
+							ObjDicOfTpperception[obj.Id][3] = append(ObjDicOfTpperception[obj.Id][3], float32(absspeed))
 							objTypeDicOfTpperception[obj.Id] = obj.Type
 							objSpeedDicOfTpperception[obj.Id] = math.Pow(math.Pow(float64(obj.Vxabs), 2)+math.Pow(float64(obj.Vyabs), 2), 0.5)
 						}

test/ey_common_test.go

@@ -6,60 +6,137 @@ import (
 	"github.com/bluenviron/goroslib/v2"
 	"log"
 	"math"
+	"sync"
 	"testing"
 	"time"
 )
 
-type Record struct {
-	StableFrame      int
-	LastExistingTime float64
+var shareVars = new(sync.Map)
+
+func UpdateShareVariable(msg *pjisuv_msgs.PerceptionLocalization) {
+	shareVars.Store("OutsideWorkshopFlag", true)
+
+	shareVars.Store("VelocityXOfCicvLocation", msg.VelocityX)
+	shareVars.Store("VelocityYOfCicvLocation", msg.VelocityY)
 }
 
-var record *Record = nil
+func TestGoRosLib(t *testing.T) {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println(fmt.Sprintf("Recovered from panic: [%s], [%#v]", Label(), r))
+		}
+	}()
+
+	rosNode, err := goroslib.NewNode(goroslib.NodeConf{
+		Name:          "eyTest",
+		MasterAddress: "localhost:11311",
+	})
+	if err != nil {
+		log.Panicln(err, fmt.Sprintf("failed to create rosNode: [%#v]", err), false)
+	}
+
+	_, err = goroslib.NewSubscriber(goroslib.SubscriberConf{
+		Node:  rosNode,
+		Topic: "/tpperception",
+		Callback: func(msg *pjisuv_msgs.PerceptionObjects) {
+			Rule(shareVars, msg)
+		},
+	})
+	if err != nil {
+		log.Panicln(err, fmt.Sprintf("failed to create subscriber: [%#v]", err), false)
+	}
+
+	_, err = goroslib.NewSubscriber(goroslib.SubscriberConf{
+		Node:  rosNode,
+		Topic: "/cicv_location",
+		Callback: func(msg *pjisuv_msgs.PerceptionLocalization) {
+			UpdateShareVariable(msg)
+		},
+	})
+	if err != nil {
+		log.Panicln(err, fmt.Sprintf("failed to create subscriber: [%#v]", err), false)
+	}
+
+	select {}
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+func Topic() string {
+	return "/tpperception"
+}
 
 // Label todo 禁止存在下划线_
+// MultiPedestrianAhead
+// MultiCarAhead
+// MultiTruckAhead
+// MultiBicycleAhead
+// MultiTricycleAhead
+// MultiTrafficConeAhead
 func Label() string {
-	return "test"
+	return "MultiTrafficConeAhead"
+}
+
+type Record struct {
+	StableFrame      int
+	LastExistingTime float64
 }
 
-// typeCheck 目标物类型检测
+var record *Record = nil
+
+// objTypeCheck 目标物类型检测
 // × 金龙车：CAR_TYPE=0, TRUCK_TYPE=1, PEDESTRIAN_TYPE=2, CYCLIST_TYPE=3, UNKNOWN_TYPE=4, UNKNOWN_MOVABLE_TYPE=5, UNKNOWN_UNMOVABLE_TYPE=6
 // √ 多功能车：UNKNOWN TYPE=O, PEDESTRIAN TYPE=1, CAR TYPE=2, TRUCK TYPE=3, Bicycle TYPE=4, Tricycle TYPE=5, Traffic Cone TYPE=6
-func typeCheck(obj *pjisuv_msgs.PerceptionObject) bool {
+func objTypeCheck(obj *pjisuv_msgs.PerceptionObject) bool {
 	const targetType uint8 = 4
 
 	return targetType == obj.Type
 }
 
-// posCheck 判断目标物位置关系
-func posCheck(obj *pjisuv_msgs.PerceptionObject) bool {
+// objPosCheck 判断目标物位置关系
+func objPosCheck(obj *pjisuv_msgs.PerceptionObject) bool {
 	const laneWidth = 3.5 // m
 
 	return obj.X > 0 && math.Abs(float64(obj.Y)) < laneWidth*1.5
 }
 
+// isEgoStationary 判断主车是否“静止”（速度低于阈值）
+func isEgoStationary(shareVars *sync.Map) bool {
+	const minSpeed = 1 // m/s
+
+	VelocityXOfCicvLocation, _ := shareVars.Load("VelocityXOfCicvLocation") // float64
+	VelocityYOfCicvLocation, _ := shareVars.Load("VelocityYOfCicvLocation")
+	speed := math.Sqrt(math.Pow(VelocityXOfCicvLocation.(float64), 2) + math.Pow(VelocityYOfCicvLocation.(float64), 2))
+
+	return speed > minSpeed
+}
+
 // Rule 检测前方指定目标物数量
-func Rule(msg *pjisuv_msgs.PerceptionObjects) string {
+func Rule(shareVars *sync.Map, msg *pjisuv_msgs.PerceptionObjects) string {
 	defer func() {
 		if r := recover(); r != nil {
 			fmt.Println("Recovered from panic:", r)
 		}
 	}()
 
+	outsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+	if !outsideWorkshopFlag.(bool) || !isEgoStationary(shareVars) {
+		return ""
+	}
+
 	const Cgcs2000X = 456256.260152
 	const Cgcs2000Y = 4397809.886833
 	const MinStableFrameCount = 5   // frame
 	const MaxTimeBetweenFrame = 0.5 // second
-	const minTargetNum = 4
+	const minTargetNum = 3
 
-	fmt.Println("\n", time.Unix(int64(msg.Header.TimeStamp), int64(msg.Header.TimeStamp*1e9)%1e9).Format(time.StampNano))
 	if len(msg.Objs) >= minTargetNum {
 
 		currTargetNum := 0
+		fmt.Println("\n", time.Unix(int64(msg.Header.TimeStamp), int64(msg.Header.TimeStamp*1e9)%1e9).Format(time.StampNano))
 		for _, obj := range msg.Objs {
-
 			// 判断目标物是否满足筛选条件
-			if typeCheck(&obj) && posCheck(&obj) {
+			if objTypeCheck(&obj) && objPosCheck(&obj) {
 				fmt.Println(fmt.Sprintf("id: [%d], type: [%d], x/yrel: [%f, %f], x/yabs: [%f, %f], speed: [%f], size: [%f/%f/%f]",
 					obj.Id, obj.Type, obj.X, obj.Y, obj.Xabs-Cgcs2000X, obj.Yabs-Cgcs2000Y, obj.Speed, obj.Length, obj.Width, obj.Height))
 				currTargetNum++
@@ -83,7 +160,7 @@ func Rule(msg *pjisuv_msgs.PerceptionObjects) string {
 					newRecord.StableFrame = record.StableFrame + 1
 					if newRecord.StableFrame == MinStableFrameCount {
 						record = nil
-						fmt.Println("!!!!!!!!!! found !!!!!!!!!!") // todo
+						fmt.Println("!!!!!!!!!! found !!!!!!!!!!")
 						return Label()
 					} else {
 						fmt.Println("---------- update ----------")
@@ -99,32 +176,3 @@ func Rule(msg *pjisuv_msgs.PerceptionObjects) string {
 
 	return ""
 }
-
-func TestGoRosLib(t *testing.T) {
-	defer func() {
-		if err := recover(); err != nil {
-			log.Println(err, fmt.Sprintf("recover: [%#v]", err), false)
-		}
-	}()
-
-	rosNode, err := goroslib.NewNode(goroslib.NodeConf{
-		Name:          "eyTest",
-		MasterAddress: "localhost:11311",
-	})
-	if err != nil {
-		log.Panicln(err, fmt.Sprintf("failed to create rosNode: [%#v]", err), false)
-	}
-
-	_, err = goroslib.NewSubscriber(goroslib.SubscriberConf{
-		Node:  rosNode,
-		Topic: "/tpperception",
-		Callback: func(msg *pjisuv_msgs.PerceptionObjects) {
-			Rule(msg)
-		},
-	})
-	if err != nil {
-		log.Panicln(err, fmt.Sprintf("failed to create subscriber: [%#v]", err), false)
-	}
-
-	select {}
-}

trigger/pjisuv/cicv_location/CurveOverspeed/main/CurveOverspeed.go

@@ -13,7 +13,7 @@ type Point struct {
 }
 
 var (
-	threshold  float64 = 3
+	threshold  float64 = 10
 	IsCurve    bool
 	count1     int64
 	pointcurve = Point{39.73004426154644, 116.49248639463602}

trigger/pjisuv/cicv_location/JunctionOverspeed/main/JunctionOverspeed.go

@@ -0,0 +1,92 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+	"sync"
+)
+
+type Point struct {
+	Latitude  float64
+	Longitude float64
+}
+
+var (
+	threshold float64 = 10
+	IsCurve   bool
+	count1    int64
+	//定义园区Ｔ字路口的经纬度坐标值
+	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}
+
+	pointlist = []Point{point3, point4, point5, point6, point7, point8}
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// 禁止存在下划线_
+func Label() string {
+	return "JunctionOverspeed"
+}
+
+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, data *pjisuv_msgs.PerceptionLocalization) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	if count1%10 == 0 {
+		Automode, _ := shareVars.Load("AutomodeOfPjVehicleFdbPub")
+		Automode = Automode.(int16)
+		IsCurve = IfEnter(pointlist, 16.0, data.Latitude, data.Longitude)
+
+		if Automode == 1 && IsCurve {
+			absspeed := math.Sqrt(math.Pow(data.VelocityX, 2) + math.Pow(data.VelocityY, 2))
+			if absspeed >= threshold {
+				eventLabel := "JunctionOverspeed"
+				fmt.Println(eventLabel)
+				count1 = 1
+				return Label()
+			}
+		}
+	}
+	count1++
+	return ""
+}

trigger/pjisuv/cicv_ticker/BeOvertakenInCorner/main/BeOvertakenInCorner.go

@@ -0,0 +1,125 @@
+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 "BeOvertakenInCorner"
+}
+
+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 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 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")
+
+	if ok && ok1 && ok2 && 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 || !isBrake(objValue) {
+					continue
+				}
+				event_lable := "BeOvertakenInCorner"
+				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/Cadence/main/Cadence.go

@@ -32,7 +32,7 @@ func Rule(shareVars *sync.Map) {
 	// 1 使用goroutine
 	go func(shareVars *sync.Map) {
 		// 2 定义触发器的间隔时间
-		ticker := time.NewTicker(time.Duration(6) * time.Second)
+		ticker := time.NewTicker(time.Duration(2) * time.Second)
 		defer ticker.Stop()
 		// 3 运行一个无限循环
 		for {
@@ -77,9 +77,10 @@ func FinalCallback(shareVars *sync.Map) {
 			fmt.Println(event_lable)
 			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
 		}
-
-		AccelXSlice = []float64{}
-		shareVars.Store("AccelXSlice", AccelXSlice)
+		if len(AccelXSlice.([]float64)) >= 500 {
+			AccelXSlice = []float64{}
+			shareVars.Store("AccelXSlice", AccelXSlice)
+		}
 
 	}
 

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

@@ -8,6 +8,10 @@ import (
 	"time"
 )
 
+var (
+	Maxlenobj int32 = 0
+)
+
 // 定时任务触发器固定的
 func Topic() string {
 	return pjisuv_ticker.TickerTopic
@@ -28,7 +32,7 @@ func Rule(shareVars *sync.Map) {
 	// 1 使用goroutine
 	go func(shareVars *sync.Map) {
 		// 2 定义触发器的间隔时间
-		ticker := time.NewTicker(time.Duration(4) * time.Second)
+		ticker := time.NewTicker(time.Duration(2) * time.Second)
 		defer ticker.Stop()
 		// 3 运行一个无限循环
 		for {
@@ -62,6 +66,7 @@ func FinalCallback(shareVars *sync.Map) {
 
 	if ok && ok1 && OutsideWorkshopFlag.(bool) == true {
 		for _, objValue := range ObjDic {
+			Maxlenobj = max(Maxlenobj, int32(len(objValue[0])))
 			if len(ObjDic[0]) <= 10 || !isBrake(objValue) {
 				continue
 			}
@@ -69,9 +74,11 @@ func FinalCallback(shareVars *sync.Map) {
 			fmt.Println(event_lable)
 			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
 		}
-
-		ObjDicOfTpperception = make(map[uint32][][]float32)
-		shareVars.Store("ObjDicOfTpperception", ObjDicOfTpperception)
+		if Maxlenobj >= 100 {
+			ObjDicOfTpperception = make(map[uint32][][]float32)
+			shareVars.Store("ObjDicOfTpperception", ObjDicOfTpperception)
+			Maxlenobj = 0
+		}
 	}
 
 }

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

@@ -8,6 +8,10 @@ import (
 	"time"
 )
 
+var (
+	Maxlenobj int32 = 0
+)
+
 // 定时任务触发器固定的
 func Topic() string {
 	return pjisuv_ticker.TickerTopic
@@ -28,7 +32,7 @@ func Rule(shareVars *sync.Map) {
 	// 1 使用goroutine
 	go func(shareVars *sync.Map) {
 		// 2 定义触发器的间隔时间
-		ticker := time.NewTicker(time.Duration(4) * time.Second)
+		ticker := time.NewTicker(time.Duration(2) * time.Second)
 		defer ticker.Stop()
 		// 3 运行一个无限循环
 		for {
@@ -67,6 +71,7 @@ func FinalCallback(shareVars *sync.Map) {
 
 	if ok && ok1 && OutsideWorkshopFlag.(bool) == true {
 		for _, objValue := range ObjDic {
+			Maxlenobj = max(Maxlenobj, int32(len(objValue[0])))
 			if len(ObjDic[0]) <= 10 || !isCuttingIn(objValue, AngularVelocityZ) {
 				continue
 			}
@@ -75,8 +80,11 @@ func FinalCallback(shareVars *sync.Map) {
 			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
 		}
 
-		ObjDicOfTpperception = make(map[uint32][][]float32)
-		shareVars.Store("ObjDicOfTpperception", ObjDicOfTpperception)
+		if Maxlenobj >= 100 {
+			ObjDicOfTpperception = make(map[uint32][][]float32)
+			shareVars.Store("ObjDicOfTpperception", ObjDicOfTpperception)
+			Maxlenobj = 0
+		}
 	}
 
 }

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

@@ -8,6 +8,10 @@ import (
 	"time"
 )
 
+var (
+	Maxlenobj int32 = 0
+)
+
 // 定时任务触发器固定的
 func Topic() string {
 	return pjisuv_ticker.TickerTopic
@@ -28,7 +32,7 @@ func Rule(shareVars *sync.Map) {
 	// 1 使用goroutine
 	go func(shareVars *sync.Map) {
 		// 2 定义触发器的间隔时间
-		ticker := time.NewTicker(time.Duration(4) * time.Second)
+		ticker := time.NewTicker(time.Duration(2) * time.Second)
 		defer ticker.Stop()
 		// 3 运行一个无限循环
 		for {
@@ -66,7 +70,9 @@ func FinalCallback(shareVars *sync.Map) {
 	AngularVelocityZ := AngularVelocityZOfCicvLocation.(float64)
 
 	if ok && ok1 && OutsideWorkshopFlag.(bool) == true {
+
 		for _, objValue := range ObjDic {
+			Maxlenobj = max(Maxlenobj, int32(len(objValue[0])))
 			if len(ObjDic[0]) <= 4 || !isCuttingIn(objValue, AngularVelocityZ) {
 				continue
 			}
@@ -75,8 +81,11 @@ func FinalCallback(shareVars *sync.Map) {
 			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
 		}
 
-		ObjDicOfTpperception = make(map[uint32][][]float32)
-		shareVars.Store("ObjDicOfTpperception", ObjDicOfTpperception)
+		if Maxlenobj >= 100 {
+			ObjDicOfTpperception = make(map[uint32][][]float32)
+			shareVars.Store("ObjDicOfTpperception", ObjDicOfTpperception)
+			Maxlenobj = 0
+		}
 	}
 
 }

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

@@ -8,6 +8,10 @@ import (
 	"time"
 )
 
+var (
+	Maxlenobj int32 = 0
+)
+
 // 定时任务触发器固定的
 func Topic() string {
 	return pjisuv_ticker.TickerTopic
@@ -28,7 +32,7 @@ func Rule(shareVars *sync.Map) {
 	// 1 使用goroutine
 	go func(shareVars *sync.Map) {
 		// 2 定义触发器的间隔时间
-		ticker := time.NewTicker(time.Duration(4) * time.Second)
+		ticker := time.NewTicker(time.Duration(2) * time.Second)
 		defer ticker.Stop()
 		// 3 运行一个无限循环
 		for {
@@ -67,6 +71,7 @@ func FinalCallback(shareVars *sync.Map) {
 
 	if ok && ok1 && OutsideWorkshopFlag.(bool) == true {
 		for _, objValue := range ObjDic {
+			Maxlenobj = max(Maxlenobj, int32(len(objValue[0])))
 			if len(ObjDic[0]) <= 10 || !isCuttingIn(objValue, AngularVelocityZ) {
 				continue
 			}
@@ -76,8 +81,11 @@ func FinalCallback(shareVars *sync.Map) {
 			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
 		}
 
-		ObjDicOfTpperception = make(map[uint32][][]float32)
-		shareVars.Store("ObjDicOfTpperception", ObjDicOfTpperception)
+		if Maxlenobj >= 100 {
+			ObjDicOfTpperception = make(map[uint32][][]float32)
+			shareVars.Store("ObjDicOfTpperception", ObjDicOfTpperception)
+			Maxlenobj = 0
+		}
 	}
 
 }

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

@@ -8,6 +8,10 @@ import (
 	"time"
 )
 
+var (
+	Maxlenobj int32 = 0
+)
+
 // 定时任务触发器固定的
 func Topic() string {
 	return pjisuv_ticker.TickerTopic
@@ -28,7 +32,7 @@ func Rule(shareVars *sync.Map) {
 	// 1 使用goroutine
 	go func(shareVars *sync.Map) {
 		// 2 定义触发器的间隔时间
-		ticker := time.NewTicker(time.Duration(4) * time.Second)
+		ticker := time.NewTicker(time.Duration(2) * time.Second)
 		defer ticker.Stop()
 		// 3 运行一个无限循环
 		for {
@@ -67,6 +71,7 @@ func FinalCallback(shareVars *sync.Map) {
 
 	if ok && ok1 && OutsideWorkshopFlag.(bool) == true {
 		for _, objValue := range ObjDic {
+			Maxlenobj = max(Maxlenobj, int32(len(objValue[0])))
 			if len(ObjDic[0]) <= 10 || !isCuttingIn(objValue, AngularVelocityZ) {
 				continue
 			}
@@ -75,8 +80,11 @@ func FinalCallback(shareVars *sync.Map) {
 			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
 		}
 
-		ObjDicOfTpperception = make(map[uint32][][]float32)
-		shareVars.Store("ObjDicOfTpperception", ObjDicOfTpperception)
+		if Maxlenobj >= 100 {
+			ObjDicOfTpperception = make(map[uint32][][]float32)
+			shareVars.Store("ObjDicOfTpperception", ObjDicOfTpperception)
+			Maxlenobj = 0
+		}
 	}
 
 }

trigger/pjisuv/cicv_ticker/HuaLong/main/HuaLong.go

@@ -32,7 +32,7 @@ func Rule(shareVars *sync.Map) {
 	// 1 使用goroutine
 	go func(shareVars *sync.Map) {
 		// 2 定义触发器的间隔时间
-		ticker := time.NewTicker(time.Duration(6) * time.Second)
+		ticker := time.NewTicker(time.Duration(2) * time.Second)
 		defer ticker.Stop()
 		// 3 运行一个无限循环
 		for {
@@ -77,9 +77,10 @@ func FinalCallback(shareVars *sync.Map) {
 			fmt.Println(event_lable)
 			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
 		}
-
-		egoSteeringRealOfDataRead = []float64{}
-		shareVars.Store("egoSteeringRealOfDataRead", egoSteeringRealOfDataRead)
+		if len(egoSteeringRealOfDataRead.([]float64)) >= 600 {
+			egoSteeringRealOfDataRead = []float64{}
+			shareVars.Store("egoSteeringRealOfDataRead", egoSteeringRealOfDataRead)
+		}
 
 	}
 

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

@@ -8,6 +8,10 @@ import (
 	"time"
 )
 
+var (
+	Maxlenobj int32 = 0
+)
+
 // 定时任务触发器固定的
 func Topic() string {
 	return pjisuv_ticker.TickerTopic
@@ -28,7 +32,7 @@ func Rule(shareVars *sync.Map) {
 	// 1 使用goroutine
 	go func(shareVars *sync.Map) {
 		// 2 定义触发器的间隔时间
-		ticker := time.NewTicker(time.Duration(4) * time.Second)
+		ticker := time.NewTicker(time.Duration(2) * time.Second)
 		defer ticker.Stop()
 		// 3 运行一个无限循环
 		for {
@@ -61,6 +65,7 @@ func FinalCallback(shareVars *sync.Map) {
 
 	if ok && ok1 && OutsideWorkshopFlag.(bool) == true {
 		for _, objValue := range ObjDic {
+			Maxlenobj = max(Maxlenobj, int32(len(objValue[0])))
 			if len(ObjDic[0]) <= 10 || !isApproach(objValue) {
 				continue
 			}
@@ -69,8 +74,11 @@ func FinalCallback(shareVars *sync.Map) {
 			pjisuv_ticker.TickerChan <- pjisuv_ticker.TickInfo{FaultLabel: Label(), FaultHappenTime: pjisuv_ticker.GetNowTimeCustom()}
 		}
 
-		ObjDicOfTpperception = make(map[uint32][][]float32)
-		shareVars.Store("ObjDicOfTpperception", ObjDicOfTpperception)
+		if Maxlenobj >= 100 {
+			ObjDicOfTpperception = make(map[uint32][][]float32)
+			shareVars.Store("ObjDicOfTpperception", ObjDicOfTpperception)
+			Maxlenobj = 0
+		}
 	}
 
 }

trigger/pjisuv/tpperception/UnknownBigTargetAhead/main/UnknownBigTargetAhead.go

@@ -24,17 +24,17 @@ type Object struct {
 
 var objectsStability = make(map[uint32]Object)
 
-// typeCheck 目标物类型检测
+// objTypeCheck 目标物类型检测
 // × 金龙车：CAR_TYPE=0, TRUCK_TYPE=1, PEDESTRIAN_TYPE=2, CYCLIST_TYPE=3, UNKNOWN_TYPE=4, UNKNOWN_MOVABLE_TYPE=5, UNKNOWN_UNMOVABLE_TYPE=6
 // √ 多功能车：UNKNOWN TYPE=O, PEDESTRIAN TYPE=1, CAR TYPE=2, TRUCK TYPE=3, Bicycle TYPE=4, Tricycle TYPE=5, Traffic Cone TYPE=6
-func typeCheck(obj *pjisuv_msgs.PerceptionObject) bool {
+func objTypeCheck(obj *pjisuv_msgs.PerceptionObject) bool {
 	const targetType uint8 = 0
 
 	return targetType == obj.Type
 }
 
-// sizeCheck 目标物大小检测
-func sizeCheck(obj *pjisuv_msgs.PerceptionObject) bool {
+// objSizeCheck 目标物大小检测
+func objSizeCheck(obj *pjisuv_msgs.PerceptionObject) bool {
 	// 多功能车
 	const targetMinLength = 3.6   // m
 	const targetMinWidth = 1.605  // m
@@ -60,13 +60,24 @@ func sizeCheck(obj *pjisuv_msgs.PerceptionObject) bool {
 	return counter > 1
 }
 
-// posCheck 判断目标物位置关系
-func posCheck(obj *pjisuv_msgs.PerceptionObject) bool {
+// objPosCheck 判断目标物位置关系
+func objPosCheck(obj *pjisuv_msgs.PerceptionObject) bool {
 	const laneWidth = 3.5 // m
 
 	return obj.X > 0 && math.Abs(float64(obj.Y)) < laneWidth*1.5
 }
 
+// isEgoStationary 判断主车是否“静止”（速度低于阈值）
+func isEgoStationary(shareVars *sync.Map) bool {
+	const minSpeed = 1 // m/s
+
+	VelocityXOfCicvLocation, _ := shareVars.Load("VelocityXOfCicvLocation") // float64
+	VelocityYOfCicvLocation, _ := shareVars.Load("VelocityYOfCicvLocation")
+	speed := math.Sqrt(math.Pow(VelocityXOfCicvLocation.(float64), 2) + math.Pow(VelocityYOfCicvLocation.(float64), 2))
+
+	return speed > minSpeed
+}
+
 // Rule 检测前方未知大目标物
 func Rule(shareVars *sync.Map, msg *pjisuv_msgs.PerceptionObjects) string {
 	defer func() {
@@ -75,14 +86,24 @@ func Rule(shareVars *sync.Map, msg *pjisuv_msgs.PerceptionObjects) string {
 		}
 	}()
 
+	// 判断主车所在位置 & 行驶速度
+	outsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+	if !outsideWorkshopFlag.(bool) || !isEgoStationary(shareVars) {
+		return ""
+	}
+
+	//const Cgcs2000X = 456256.260152
+	//const Cgcs2000Y = 4397809.886833
 	const MinStableFrameCount = 5   // frame
 	const MaxTimeBetweenFrame = 0.5 // second
-	OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
-	OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+
+	//fmt.Println("\n", time.Unix(int64(msg.Header.TimeStamp), int64(msg.Header.TimeStamp*1e9)%1e9).Format(time.StampNano))
 	for _, obj := range msg.Objs {
 
 		// 判断目标物是否满足筛选条件
-		if typeCheck(&obj) && posCheck(&obj) && sizeCheck(&obj) {
+		if objTypeCheck(&obj) && objPosCheck(&obj) && objSizeCheck(&obj) {
+			//fmt.Println(fmt.Sprintf("id: [%d], type: [%d], x/yrel: [%f, %f], x/yabs: [%f, %f], speed: [%f], size: [%f/%f/%f]",
+			//	obj.Id, obj.Type, obj.X, obj.Y, obj.Xabs-Cgcs2000X, obj.Yabs-Cgcs2000Y, obj.Speed, obj.Length, obj.Width, obj.Height))
 
 			// 目标物初见存档
 			if prevObjRecord, exists := objectsStability[obj.Id]; exists == false {
@@ -91,6 +112,7 @@ func Rule(shareVars *sync.Map, msg *pjisuv_msgs.PerceptionObjects) string {
 					StableFrame:      1,
 					LastExistingTime: msg.Header.TimeStamp,
 				}
+				//fmt.Println("---------- create ----------")
 			} else {
 				currObjRecord := Object{
 					ID:               obj.Id,
@@ -101,10 +123,15 @@ func Rule(shareVars *sync.Map, msg *pjisuv_msgs.PerceptionObjects) string {
 				// 目标物确认/更新/重置
 				if currObjRecord.LastExistingTime-prevObjRecord.LastExistingTime <= MaxTimeBetweenFrame { // 目标物消失不超过0.5s
 					currObjRecord.StableFrame = prevObjRecord.StableFrame + 1
-					if currObjRecord.StableFrame == MinStableFrameCount && OutsideWorkshopFlag == true {
+					if currObjRecord.StableFrame == MinStableFrameCount {
 						delete(objectsStability, obj.Id)
+						//fmt.Println("!!!!!!!!!! found !!!!!!!!!!")
 						return Label()
+					} else {
+						//fmt.Println("---------- update ----------")
 					}
+				} else {
+					//fmt.Println("---------- reset ----------")
 				}
 				objectsStability[obj.Id] = currObjRecord
 			}

trigger/pjisuv/tpperception/lowSpdTruckAhead/main/lowSpdTruckAhead.go

@@ -24,30 +24,41 @@ func Label() string {
 	return "LowSpdTruckAhead"
 }
 
-// speedCheck 目标物速度检测
-func speedCheck(obj *pjisuv_msgs.PerceptionObject) bool {
+// objSpeedCheck 目标物速度检测
+func objSpeedCheck(obj *pjisuv_msgs.PerceptionObject) bool {
 	const targetMinSpeed = 2 / 3.6  // m/s
 	const targetMaxSpeed = 20 / 3.6 // m/s
 
 	return targetMinSpeed < obj.Speed && obj.Speed < targetMaxSpeed
 }
 
-// typeCheck 目标物类型检测
+// objTypeCheck 目标物类型检测
 // × 金龙车：CAR_TYPE=0, TRUCK_TYPE=1, PEDESTRIAN_TYPE=2, CYCLIST_TYPE=3, UNKNOWN_TYPE=4, UNKNOWN_MOVABLE_TYPE=5, UNKNOWN_UNMOVABLE_TYPE=6
 // √ 多功能车：UNKNOWN TYPE=O, PEDESTRIAN TYPE=1, CAR TYPE=2, TRUCK TYPE=3, Bicycle TYPE=4, Tricycle TYPE=5, Traffic Cone TYPE=6
-func typeCheck(obj *pjisuv_msgs.PerceptionObject) bool {
+func objTypeCheck(obj *pjisuv_msgs.PerceptionObject) bool {
 	const targetType uint8 = 3
 
 	return targetType == obj.Type
 }
 
-// posCheck 判断目标物位置关系
-func posCheck(obj *pjisuv_msgs.PerceptionObject) bool {
+// objPosCheck 判断目标物位置关系
+func objPosCheck(obj *pjisuv_msgs.PerceptionObject) bool {
 	const laneWidth = 3.5 // m
 
 	return obj.X > 0 && math.Abs(float64(obj.Y)) < laneWidth*1.5
 }
 
+// isEgoStationary 判断主车是否“静止”（速度低于阈值）
+func isEgoStationary(shareVars *sync.Map) bool {
+	const minSpeed = 1 // m/s
+
+	VelocityXOfCicvLocation, _ := shareVars.Load("VelocityXOfCicvLocation") // float64
+	VelocityYOfCicvLocation, _ := shareVars.Load("VelocityYOfCicvLocation")
+	speed := math.Sqrt(math.Pow(VelocityXOfCicvLocation.(float64), 2) + math.Pow(VelocityYOfCicvLocation.(float64), 2))
+
+	return speed > minSpeed
+}
+
 // Rule 检测前方大车低速行驶
 func Rule(shareVars *sync.Map, msg *pjisuv_msgs.PerceptionObjects) string {
 	defer func() {
@@ -55,15 +66,25 @@ func Rule(shareVars *sync.Map, msg *pjisuv_msgs.PerceptionObjects) string {
 			fmt.Println(fmt.Sprintf("Recovered from panic: [%s], [%#v]", Label(), r))
 		}
 	}()
-	OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
-	OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+
+	// 判断主车所在位置 & 行驶速度
+	outsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+	if !outsideWorkshopFlag.(bool) || !isEgoStationary(shareVars) {
+		return ""
+	}
+
+	//const Cgcs2000X = 456256.260152
+	//const Cgcs2000Y = 4397809.886833
 	const MinStableFrameCount = 5
 	const MaxTimeBetweenFrame = 0.5
 
+	//fmt.Println("\n", time.Unix(int64(msg.Header.TimeStamp), int64(msg.Header.TimeStamp*1e9)%1e9).Format(time.StampNano))
 	for _, obj := range msg.Objs {
 
 		// 判断目标物是否满足筛选条件
-		if speedCheck(&obj) && posCheck(&obj) && typeCheck(&obj) {
+		if objSpeedCheck(&obj) && objPosCheck(&obj) && objTypeCheck(&obj) {
+			//fmt.Println(fmt.Sprintf("id: [%d], type: [%d], x/yrel: [%f, %f], x/yabs: [%f, %f], speed: [%f], size: [%f/%f/%f]",
+			//	obj.Id, obj.Type, obj.X, obj.Y, obj.Xabs-Cgcs2000X, obj.Yabs-Cgcs2000Y, obj.Speed, obj.Length, obj.Width, obj.Height))
 
 			// 目标物初见存档
 			if prevObjRecord, exists := objectsStability[obj.Id]; exists == false {
@@ -72,6 +93,7 @@ func Rule(shareVars *sync.Map, msg *pjisuv_msgs.PerceptionObjects) string {
 					StableAge:     1,
 					LastExistTime: msg.Header.TimeStamp,
 				}
+				//fmt.Println("---------- create ----------")
 			} else {
 				currObjRecord := Object{
 					ID:            obj.Id,
@@ -82,10 +104,15 @@ func Rule(shareVars *sync.Map, msg *pjisuv_msgs.PerceptionObjects) string {
 				// 目标物确认/更新/重置
 				if currObjRecord.LastExistTime-prevObjRecord.LastExistTime <= MaxTimeBetweenFrame { // 目标物消失不超过0.5s
 					currObjRecord.StableAge = prevObjRecord.StableAge + 1
-					if currObjRecord.StableAge == MinStableFrameCount && OutsideWorkshopFlag == true {
+					if currObjRecord.StableAge == MinStableFrameCount {
 						delete(objectsStability, obj.Id)
+						//fmt.Println("!!!!!!!!!! found !!!!!!!!!!")
 						return Label()
+					} else {
+						//fmt.Println("---------- update ----------")
 					}
+				} else {
+					//fmt.Println("---------- reset ----------")
 				}
 				objectsStability[obj.Id] = currObjRecord
 			}

trigger/pjisuv/tpperception/multiTargetAhead/main/multiTargetAhead.go → trigger/pjisuv/tpperception/multiTargetAhead/main/multiTrafficConeAhead.go

@@ -19,7 +19,7 @@ func Topic() string {
 // MultiTricycleAhead
 // MultiTrafficConeAhead
 func Label() string {
-	return "MultiPedestrianAhead"
+	return "MultiTrafficConeAhead"
 }
 
 type Record struct {
@@ -29,22 +29,33 @@ type Record struct {
 
 var record *Record = nil
 
-// typeCheck 目标物类型检测
+// objTypeCheck 目标物类型检测
 // × 金龙车：CAR_TYPE=0, TRUCK_TYPE=1, PEDESTRIAN_TYPE=2, CYCLIST_TYPE=3, UNKNOWN_TYPE=4, UNKNOWN_MOVABLE_TYPE=5, UNKNOWN_UNMOVABLE_TYPE=6
 // √ 多功能车：UNKNOWN TYPE=O, PEDESTRIAN TYPE=1, CAR TYPE=2, TRUCK TYPE=3, Bicycle TYPE=4, Tricycle TYPE=5, Traffic Cone TYPE=6
-func typeCheck(obj *pjisuv_msgs.PerceptionObject) bool {
-	const targetType uint8 = 1
+func objTypeCheck(obj *pjisuv_msgs.PerceptionObject) bool {
+	const targetType uint8 = 6
 
 	return targetType == obj.Type
 }
 
-// posCheck 判断目标物位置关系
-func posCheck(obj *pjisuv_msgs.PerceptionObject) bool {
+// objPosCheck 判断目标物位置关系
+func objPosCheck(obj *pjisuv_msgs.PerceptionObject) bool {
 	const laneWidth = 3.5 // m
 
 	return obj.X > 0 && math.Abs(float64(obj.Y)) < laneWidth*1.5
 }
 
+// isEgoStationary 判断主车是否“静止”（速度低于阈值）
+func isEgoStationary(shareVars *sync.Map) bool {
+	const minSpeed = 1 // m/s
+
+	VelocityXOfCicvLocation, _ := shareVars.Load("VelocityXOfCicvLocation") // float64
+	VelocityYOfCicvLocation, _ := shareVars.Load("VelocityYOfCicvLocation")
+	speed := math.Sqrt(math.Pow(VelocityXOfCicvLocation.(float64), 2) + math.Pow(VelocityYOfCicvLocation.(float64), 2))
+
+	return speed > minSpeed
+}
+
 // Rule 检测前方指定目标物数量
 func Rule(shareVars *sync.Map, msg *pjisuv_msgs.PerceptionObjects) string {
 	defer func() {
@@ -53,21 +64,26 @@ func Rule(shareVars *sync.Map, msg *pjisuv_msgs.PerceptionObjects) string {
 		}
 	}()
 
-	const Cgcs2000X = 456256.260152
-	const Cgcs2000Y = 4397809.886833
+	outsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
+	if !outsideWorkshopFlag.(bool) || !isEgoStationary(shareVars) {
+		return ""
+	}
+
+	//const Cgcs2000X = 456256.260152
+	//const Cgcs2000Y = 4397809.886833
 	const MinStableFrameCount = 5   // frame
 	const MaxTimeBetweenFrame = 0.5 // second
-	const minTargetNum = 4
-	OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
-	OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
+	const minTargetNum = 3
 
 	if len(msg.Objs) >= minTargetNum {
 
 		currTargetNum := 0
+		//fmt.Println("\n", time.Unix(int64(msg.Header.TimeStamp), int64(msg.Header.TimeStamp*1e9)%1e9).Format(time.StampNano))
 		for _, obj := range msg.Objs {
-
 			// 判断目标物是否满足筛选条件
-			if typeCheck(&obj) && posCheck(&obj) {
+			if objTypeCheck(&obj) && objPosCheck(&obj) {
+				//fmt.Println(fmt.Sprintf("id: [%d], type: [%d], x/yrel: [%f, %f], x/yabs: [%f, %f], speed: [%f], size: [%f/%f/%f]",
+				//	obj.Id, obj.Type, obj.X, obj.Y, obj.Xabs-Cgcs2000X, obj.Yabs-Cgcs2000Y, obj.Speed, obj.Length, obj.Width, obj.Height))
 				currTargetNum++
 			}
 		}
@@ -78,6 +94,7 @@ func Rule(shareVars *sync.Map, msg *pjisuv_msgs.PerceptionObjects) string {
 					StableFrame:      1,
 					LastExistingTime: msg.Header.TimeStamp,
 				}
+				//fmt.Println("---------- create ----------")
 			} else {
 				newRecord := Record{
 					StableFrame:      1,
@@ -86,12 +103,15 @@ func Rule(shareVars *sync.Map, msg *pjisuv_msgs.PerceptionObjects) string {
 
 				if newRecord.LastExistingTime-record.LastExistingTime <= MaxTimeBetweenFrame {
 					newRecord.StableFrame = record.StableFrame + 1
-					if newRecord.StableFrame == MinStableFrameCount && OutsideWorkshopFlag == true {
+					if newRecord.StableFrame == MinStableFrameCount {
 						record = nil
+						//fmt.Println("!!!!!!!!!! found !!!!!!!!!!")
 						return Label()
 					} else {
+						//fmt.Println("---------- update ----------")
 					}
 				} else {
+					//fmt.Println("---------- reset ----------")
 				}
 				record = &newRecord
 			}