trigger/main/pji_DGNC/FrontCarSideBySide/main/FrontCarSideBySide.go

package main

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

var (
	countSideBySide = 0
	param           entity.PjisuvParam
)

func main() {
	ticker := time.NewTicker(3 * time.Second)
	defer ticker.Stop()
	go listener()
	for {
		select {
		case <-ticker.C:
			FinalCallback()
		}
	}
}
func FinalCallback() {
	if countSideBySide >= 60 {
		event_lable := "FrontCarSideBySide"
		fmt.Println(event_lable)
	}
	countSideBySide = 0
}
func CallbackCicvLocation(data *pjisuv_msgs.PerceptionLocalization) {
	param.AbsSpeed = math.Sqrt(math.Pow(data.VelocityX, 2) + math.Pow(data.VelocityY, 2))

}
func CallbackTpperception(data *pjisuv_msgs.PerceptionObjects) {
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)
		if !(!(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)
				if !(!(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 && param.AbsSpeed > 2 {
						countSideBySide++
						break lable1
					}
				}

			}

		}

	}
}
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
}