trigger/main/pji_DGNC/RearVehiclesFrequentChangeLane/main/RearVehiclesFrequentChangeLane.go

package main

import (
	"awesomeProject/entity"
	"awesomeProject/pjisuv_msgs"
	"fmt"
	"github.com/bluenviron/goroslib/v2"
	"math"
	"os"
	"os/signal"
	"time"
)

var (
	Maxlenobj   int32 = 0
	param       entity.PjisuvParam
	ObjectSlice = make(map[uint32][][]float32)
)

func main() {
	ticker := time.NewTicker(4 * time.Second)
	defer ticker.Stop()
	go listener()
	for {
		select {
		case <-ticker.C:
			FinalCallback()
		}
	}
}
func countChanges(slice [][]float32) int {
	//fmt.Println(slice)
	count := 0
lable1:
	for i := 0; i < len(slice[1]); {
		xi := slice[0][i]
		yi := math.Abs(float64(slice[1][i]))
		if (yi >= 1.8 || yi < 0.7) && xi <= -2 && math.Abs(param.AngularVelocityZOfCicvLocation) <= 1.6 {
			for j := 0; j < len(slice[1])-i-1; j++ {
				xij := slice[0][1+i+j]
				yij := math.Abs(float64(slice[1][1+i+j]))
				if ((yi >= 1.8 && yij <= 0.7) || (yi < 0.7 && yij >= 1.8)) && xij <= -2 && math.Abs(param.AngularVelocityZOfCicvLocation) <= 1.6 {
					count++
					fmt.Println("here!!")
					i = i + j + 1
					continue lable1
				}
			}
			break lable1
		} else {
			i++
		}

	}
	return count
}

func FinalCallback() {

	for _, objValue := range ObjectSlice {
		//fmt.Println(objValue)
		//fmt.Println("------------------------------------------------")
		Maxlenobj = max(Maxlenobj, int32(len(objValue[0])))
		if len(objValue[0]) <= 10 || countChanges(objValue) < 2 {
			continue
		}
		event_lable := "RearVehiclesFrequentChangeLane"
		fmt.Println(event_lable)
		ObjectSlice = make(map[uint32][][]float32)
	}
	if Maxlenobj >= 250 {
		ObjectSlice = make(map[uint32][][]float32)
		Maxlenobj = 0
	}
}

func CallbackCicvLocation(data *pjisuv_msgs.PerceptionLocalization) {
	param.AngularVelocityZOfCicvLocation = data.AngularVelocityZ
}
func CallbackTpperception(data *pjisuv_msgs.PerceptionObjects) {
	for _, obj := range data.Objs {
		if math.Abs(float64(obj.X)) >= 60 || math.Abs(float64(obj.Y)) >= 10 {
			continue
		}
		if _, ok := ObjectSlice[obj.Id]; !ok {
			ObjectSlice[obj.Id] = [][]float32{{}, {}}
		}
		ObjectSlice[obj.Id][0] = append(ObjectSlice[obj.Id][0], obj.X)
		ObjectSlice[obj.Id][1] = append(ObjectSlice[obj.Id][1], obj.Y)
	}
}
func listener() {
	// create a node and connect to the master
	n, err := goroslib.NewNode(goroslib.NodeConf{
		Name:          "goroslib_sub",
		MasterAddress: "127.0.0.1:11311",
	})
	if err != nil {
		panic(err)
	}
	defer n.Close()

	// create a subscriber
	subCicvLocation, err := goroslib.NewSubscriber(goroslib.SubscriberConf{
		Node:     n,
		Topic:    "/cicv_location",
		Callback: CallbackCicvLocation,
	})
	if err != nil {
		panic(err)
	}
	defer subCicvLocation.Close()

	// create a subscriber
	subTpperception, err := goroslib.NewSubscriber(goroslib.SubscriberConf{

		Node:     n,
		Topic:    "tpperception",
		Callback: CallbackTpperception,
	})
	if err != nil {
		panic(err)
	}
	defer subTpperception.Close()

	// wait for CTRL-C
	c := make(chan os.Signal, 1)
	signal.Notify(c, os.Interrupt)
	<-c
}