trigger/main/pji_DGNC/DescendingSteepHill/main/DescendingSteepHill.go

package main

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

// //道路坡度大于6%就可以被认为是陡坡
var (
	param      entity.PjisuvParam
	threshold  = -0.05
	AngleSlice = [][]float64{{}, {}, {}, {}}
	count1     = 0
)

func main() {
	ticker := time.NewTicker(2 * time.Second)
	defer ticker.Stop()
	go listener()
	for {
		select {
		case <-ticker.C:
			FinalCallback()
		}
	}
}

// QuaternionToEuler 将四元数转换为欧拉角
func QuaternionToEuler(x, y, z, w float64) float64 {
	// 归一化四元数
	length := math.Sqrt(x*x + y*y + z*z + w*w)
	x /= length
	y /= length
	z /= length
	w /= length
	// 计算欧拉角
	pitch := math.Asin(2 * (w*y - z*x))
	return pitch
}
func countChanges(AngleSlice [][]float64) int {
	num := 0
	for i := 0; i < len(AngleSlice[1]); i++ {
		pitch := QuaternionToEuler(AngleSlice[0][i], AngleSlice[1][i], AngleSlice[2][i], AngleSlice[3][i])
		if pitch < threshold {
			num++
		}
	}
	return num
}

func FinalCallback() {
	count := countChanges(AngleSlice)
	if count >= 5 {
		event_lable := "DescendingSteepHill"
		fmt.Println(event_lable)
	}
	AngleSlice = [][]float64{{}, {}, {}, {}}

}

func CallbackCicvLocation(data *pjisuv_msgs.PerceptionLocalization) {
	if count1%10 == 0 {
		AngleSlice[0] = append(AngleSlice[0], data.Qx)
		AngleSlice[1] = append(AngleSlice[1], data.Qy)
		AngleSlice[2] = append(AngleSlice[2], data.Qz)
		AngleSlice[3] = append(AngleSlice[3], data.Qw)
		count1 = 1
	}
	count1++

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

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