cicv_data_closed_loop/trigger/jili/cicv_location/LongDownhill/main/LongDownhill.go

package main

import (
	"cicv-data-closedloop/pjisuv_msgs"
	"fmt"
	"math"
	"sync"
)

var (
	threshold        = -0.05
	count1           = 0
	UphillPointSlice = [][]float64{{}, {}}
	CountNoneUphill  = 0
)

type Point struct {
	X float64
	Y float64
}

func Topic() string {
	return "/cicv_location"
}

// 禁止存在下划线_
func Label() string {
	return "LongDownhill"
}

// 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))
	if math.IsNaN(pitch) {
		return 0.0
	}
	return pitch
}

// 计算两点之间的距离（米）
func distance(point1, point2 Point) float64 {

	d := math.Sqrt((point2.X-point1.X)*(point2.X-point1.X) + (point2.Y-point1.Y)*(point2.Y-point1.Y))

	return d
}

func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionLocalization) string {
	defer func() {
		if r := recover(); r != nil {
			fmt.Println("Recovered from panic:", r)
		}
	}()
	OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
	OutsideWorkshopFlag1 := OutsideWorkshopFlag.(bool)
	if OutsideWorkshopFlag1 == true {
		if count1%10 == 0 {
			pitch := QuaternionToEuler(data.Qx, data.Qy, data.Qz, data.Qw)
			//fmt.Println(pitch)
			if pitch < threshold {
				UphillPointSlice[0] = append(UphillPointSlice[0], data.PositionX)
				UphillPointSlice[1] = append(UphillPointSlice[1], data.PositionY)
				if len(UphillPointSlice[0]) > 250 {

					P1 := Point{UphillPointSlice[0][0], UphillPointSlice[1][0]}
					P2 := Point{UphillPointSlice[0][len(UphillPointSlice[0])-1], UphillPointSlice[1][len(UphillPointSlice[1])-1]}
					distance := distance(P1, P2)
					if distance >= 100 {
						event_lable := "LongUphill"
						fmt.Println(event_lable)
						CountNoneUphill = 0
						UphillPointSlice = [][]float64{{}, {}}
						return Label()
					}

				}
			} else {
				CountNoneUphill++
				if CountNoneUphill == 3 {
					if len(UphillPointSlice[0]) > 1 {
						P1 := Point{UphillPointSlice[0][0], UphillPointSlice[1][0]}
						P2 := Point{UphillPointSlice[0][len(UphillPointSlice[0])-1], UphillPointSlice[1][len(UphillPointSlice[1])-1]}
						distance := distance(P1, P2)
						if distance >= 100 {
							event_lable := "LongUphill"
							fmt.Println(event_lable)
							return Label()
						}
						UphillPointSlice = [][]float64{{}, {}}
					}
					CountNoneUphill = 0
				}
			}
			count1 = 1
		}
		count1++
	}

	return ""
}