trigger/main/pji_DGNC/CarFollowingTooCloseAtNight/main/CarFollowingTooCloseAtNight.go

package main

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

var (
	param       entity.PjisuvParam
	StartTime   int64
	count1      int64
	ObjectSlice = make(map[uint32][][]float32)
)

func Ifatnight() bool {
	// 获取当前时间
	now := time.Now()
	later := now.Add(0 * time.Hour)
	// 获取当前小时
	hour := later.Hour()
	// 判断当前时间是白天还是夜晚
	if hour >= 0 && hour < 5 || hour >= 20 && hour <= 23 {
		return true
	} else {
		return false
	}
}
func IfObstaclesNearby() bool {
	for _, obj := range ObjectSlice {
		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 CallbackCicvLocation(data *pjisuv_msgs.PerceptionLocalization) {
	//print(param.AutomodeOfPjVehicleFdbPub)

	if count1%10 == 0 && Ifatnight() {
		AbsSpeed := math.Sqrt(math.Pow(data.VelocityX, 2) + math.Pow(data.VelocityY, 2))
		flag := IfObstaclesNearby()
		//fmt.Println(ObjectSlice)
		if AbsSpeed >= 1 && flag {
			// 如果之前没有记录开始时间，记录当前时间
			if StartTime == 0 {
				StartTime = time.Now().Unix()
			}
			// 判断是否持续超过一分钟
			if time.Now().Unix()-StartTime > 3 {
				event_label := "CarFollowingTooCloseAtNight"
				fmt.Println(event_label)
			}
		} else {
			// 如果速度大于 0.1，重置开始时间和停止标志
			StartTime = 0
		}
		count1 = 1
	} else {
		count1++
	}
}

func main() {
	go listener()
	select {}
	//time.Sleep(10000)
}
func CallbackTpperception(data *pjisuv_msgs.PerceptionObjects) {
	for _, obj := range data.Objs {
		if obj.X <= -2 || math.Abs(float64(obj.Y)) >= 90 {
			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
}