cicv_data_closed_loop/trigger/pjisuv/tpperception/FrontCarSideBySide/main/FrontCarSideBySide.go

package main

import (
	"cicv-data-closedloop/pjisuv_msgs"
	"fmt"
	"math"
	"sync"
)

// 该程序存在的意义是为cicv_ticker中的FrontCarSideBySide触发器提供countSideBySide的全局变量
func Topic() string {
	return "/tpperception"
}

func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionObjects) string {
	defer func() {
		if r := recover(); r != nil {
			fmt.Println("Recovered from panic:", r)
		}
	}()
	count1 := 0
	AbsSpeed, ok := shareVars.Load("AbsSpeed")
	OutsideWorkshopFlag, ok1 := shareVars.Load("OutsideWorkshopFlag")
	OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
	countSideBySide, ok2 := shareVars.Load("CountSideBySide")
	if ok && ok1 && ok2 {
	lable1:
		for _, obj := range data.Objs {
			if !(obj.Type != 2 || obj.Type != 3 || obj.Type != 4 || !(obj.X >= 3 && obj.X <= 17) || math.Abs(float64(obj.Y)) > 10) {
				//fmt.Println("here")
				obj1ID := obj.Id
				obj1x := obj.X
				obj1y := obj.Y
				//fmt.Println(obj1ID, obj1x, obj1y)
				for _, obj1 := range data.Objs {
					if !(obj1.Type != 2 || obj1.Type != 3 || obj1.Type != 4 || !(obj1.X >= 3 && obj1.X <= 17) || math.Abs(float64(obj1.Y)) > 10) {
						obj2ID := obj1.Id
						obj2x := obj1.X
						obj2y := obj1.Y
						if obj2ID != obj1ID && math.Abs(float64(obj2x-obj1x)) <= 2.2 && math.Abs(float64(obj2y-obj1y)) >= 2 && math.Abs(float64(obj2y-obj1y)) <= 4.0 && AbsSpeed.(float64) > 2 {
							count1 = countSideBySide.(int) + 1
							shareVars.Store("countSideBySide", count1)
							break lable1
						}
					}

				}

			}

		}
	}

	return ""
}