trigger/main/pji_DGNC/CPTA_f/main/CPTA_f.go

package main

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

var (
	LeftCurveFlag  = false
	RightCurveFlag = false
	param          entity.PjisuvParam
)

func main() {

	go listener()
	for {
		select {}
	}
}

func CallbackTpperception(data *pjisuv_msgs.PerceptionObjects) {
	if LeftCurveFlag == true {
		for _, obj := range data.Objs {
			//if obj.Type == 1 { //理论条件
			if obj.Type != 100 { //简化条件
				Distance := math.Sqrt(math.Pow(param.PositionXOfCicvLocation-obj.Xabs, 2) + math.Pow(param.PositionYOfCicvLocation-obj.Yabs, 2))
				Objspeed := math.Sqrt(math.Pow(float64(obj.Vxabs), 2) + math.Pow(float64(obj.Vyabs), 2))

				//fmt.Println("Distance:", Distance)
				if obj.X >= 2.0 && obj.X <= 30.0 && Distance <= 25.0 && Objspeed >= 0.7 {
					event_lable := "CPTA_f"
					fmt.Println(event_lable)
					break
				}
			}
		}
	}
}
func CallbackTrajectory(data *pjisuv_msgs.Trajectory) {
	Trajectorypoints := data.Trajectoryinfo.Trajectorypoints
	if len(Trajectorypoints) > 2 {
		StartHeading := Trajectorypoints[0].Heading
		EndHeading := Trajectorypoints[len(Trajectorypoints)-1].Heading
		diffHeading := StartHeading - EndHeading
		//fmt.Println(diffHeading)

		if diffHeading < -1.0 && diffHeading > -3.0 {
			LeftCurveFlag = true
			//fmt.Println(diffHeading)
		} else {
			LeftCurveFlag = false
		}
		if diffHeading > 1.0 && diffHeading < 3.0 {
			RightCurveFlag = true
			//fmt.Println(diffHeading)
		} else {
			RightCurveFlag = false
		}

	} else {
		LeftCurveFlag = false
		RightCurveFlag = false
	}
}
func CallbackCicvLocation(data *pjisuv_msgs.PerceptionLocalization) {
	param.PositionXOfCicvLocation = data.PositionX
	param.PositionYOfCicvLocation = data.PositionY
}

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()

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

		Node:     n,
		Topic:    "/cicv_amr_trajectory",
		Callback: CallbackTrajectory,
	})
	if err != nil {
		panic(err)
	}
	defer subTrajectory.Close()

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