add comments

main/pji_DGNC/CCCscp_f/main/CCCscp_f.go

@@ -50,8 +50,8 @@ func isCross(ObjectList [][]float32) bool {
 	}
 	return false
 }
-func FinalCallback() {
 
+func FinalCallback() {
 	for _, objValue := range ObjectSlice {
 		//fmt.Println(objValue)
 		//fmt.Println("------------------------------------------------")
@@ -63,18 +63,19 @@ func FinalCallback() {
 		fmt.Println(event_lable)
 		ObjectSlice = make(map[uint32][][]float32)
 	}
-
 }
 
 func CallbackCicvLocation(data *pjisuv_msgs.PerceptionLocalization) {
 	param.YawOfCicvLocation = data.Yaw
-
+	fmt.Println("FrameUnmber", data.FrameUnmber)
 }
+
 func CallbackTpperception(data *pjisuv_msgs.PerceptionObjects) {
 	Framenum += 1
-	//fmt.Println(len(data.Objs))
+	fmt.Println("obj length", len(data.Objs))
+	fmt.Println("Framenum", Framenum)
 	for _, obj := range data.Objs {
-		//fmt.Println("ID", obj.Id, "Type", obj.Type)
+		fmt.Println("ID", obj.Id, "Type", obj.Type)
 
 		if math.Abs(float64(obj.X)) >= 80 || math.Abs(float64(obj.Y)) >= 30 {
 			continue
@@ -85,6 +86,7 @@ func CallbackTpperception(data *pjisuv_msgs.PerceptionObjects) {
 		//absspeed := math.Sqrt(math.Pow(float64(obj.Vxabs), 2)+math.Pow(float64(obj.Vyabs), 2)) * 3.6
 		ObjectSlice[obj.Id][0] = append(ObjectSlice[obj.Id][0], obj.X)
 		ObjectSlice[obj.Id][1] = append(ObjectSlice[obj.Id][1], obj.Y)
+		// 计算自车与目标的最小角度差（范围归到0到180度）
 		diffh := (float64(obj.Heading - float32(param.YawOfCicvLocation)))
 		if diffh < -180.0 {
 			diffh = 360.0 + diffh
@@ -99,17 +101,17 @@ func CallbackTpperception(data *pjisuv_msgs.PerceptionObjects) {
 		ObjectSlice[obj.Id][4] = append(ObjectSlice[obj.Id][4], float32(obj.Type))
 
 		//ObjectSlice[obj.Id][3] = append(ObjectSlice[obj.Id][3], float32(absspeed))
+		// 滑动窗口（保持切片大小为200，大于等于200则持续丢弃第一个，整体向前移动一位）
 		if len(ObjectSlice[obj.Id][0]) >= 200 {
 			ObjectSlice[obj.Id][0] = ObjectSlice[obj.Id][0][1:]
 			ObjectSlice[obj.Id][1] = ObjectSlice[obj.Id][1][1:]
 			ObjectSlice[obj.Id][2] = ObjectSlice[obj.Id][2][1:]
 			ObjectSlice[obj.Id][3] = ObjectSlice[obj.Id][3][1:]
 			ObjectSlice[obj.Id][4] = ObjectSlice[obj.Id][4][1:]
-
 		}
-
 	}
 }
+
 func listener() {
 	// create a node and connect to the master
 	n, err := goroslib.NewNode(goroslib.NodeConf{
@@ -134,7 +136,6 @@ func listener() {
 
 	// create a subscriber
 	subTpperception, err := goroslib.NewSubscriber(goroslib.SubscriberConf{
-
 		Node:     n,
 		Topic:    "tpperception",
 		Callback: CallbackTpperception,