add TargetAhead EnterTjunction FIndTrafficLight trigger

trigger/pjisuv/cicv_location/EnterTjunction/main/EnterTjunction.go

@@ -0,0 +1,77 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"math"
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// Label todo 禁止存在下划线_
+func Label() string {
+	return "EnterTjunction"
+}
+
+type Point struct {
+	Latitude  float64
+	Longitude float64
+}
+
+var (
+	count2 int = 0
+
+	//定义园区部门Ｔ字路口的经纬度坐标值
+	point3 = Point{39.73040966605621, 116.48995329696209}
+	point4 = Point{39.73083727413453, 116.49079780188244}
+	point5 = Point{39.72976753711939, 116.49043130389033}
+	point6 = Point{39.73012466515933, 116.49128381717591}
+	point7 = Point{39.729251498328246, 116.49077484625299}
+	point8 = Point{39.72964529630643, 116.49164592200161}
+
+	pointlist = []Point{point3, point4, point5, point6, point7, point8}
+)
+
+func Rule(data *pjisuv_msgs.PerceptionLocalization) string {
+	if count2%10 == 0 {
+
+		enterflag := IfEnter(pointlist, 12.0, data.Latitude, data.Longitude)
+		if enterflag {
+			//eventLabel := "EnterTjunction"
+			//fmt.Println(eventLabel)
+			return "EnterTjunction"
+		}
+	}
+	count2++
+	return ""
+}
+
+func IfEnter(pointlist []Point, radius float64, lat, lon float64) bool {
+	// 判断是否进入点列表中的区域
+	point1 := Point{Latitude: lat, Longitude: lon}
+	for _, point := range pointlist {
+		d := distance(point1, point)
+		if d <= radius {
+			return true
+		}
+	}
+	return false
+}
+
+// 计算两点之间的距离（米）
+func distance(point1, point2 Point) float64 {
+	// 经纬度转弧度
+	lat1 := point1.Latitude * math.Pi / 180
+	lon1 := point1.Longitude * math.Pi / 180
+	lat2 := point2.Latitude * math.Pi / 180
+	lon2 := point2.Longitude * math.Pi / 180
+
+	// 计算距离
+	dlon := lon2 - lon1
+	dlat := lat2 - lat1
+	a := math.Sin(dlat/2)*math.Sin(dlat/2) + math.Sin(dlon/2)*math.Sin(dlon/2)*math.Cos(lat1)*math.Cos(lat2)
+	c := 2 * math.Atan2(math.Sqrt(a), math.Sqrt(1-a))
+	d := 6371000 * c
+	return d
+}

trigger/pjisuv/cicv_location/FindTrafficLight/main/FindTrafficLight.go

@@ -0,0 +1,75 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"math"
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+// Label todo 禁止存在下划线_
+func Label() string {
+	return "FindTrafficLight"
+}
+
+type Point struct {
+	Latitude  float64
+	Longitude float64
+}
+
+var (
+	count1 int = 0
+
+	//定义园区４个信号灯的坐标
+	point2 = Point{39.72975930689718, 116.48861102824081}
+	point3 = Point{39.7288805296616, 116.48812315228867}
+	point4 = Point{39.73061430369551, 116.49225103553502}
+	point5 = Point{39.73077491578002, 116.49060085035634}
+
+	pointlist = []Point{point2, point3, point4, point5}
+)
+
+func Rule(data *pjisuv_msgs.PerceptionLocalization) string {
+	if count1%10 == 0 {
+
+		enterflag := IfEnter(pointlist, 15.0, data.Latitude, data.Longitude)
+		if enterflag {
+			//eventLabel := "FindTrafficLight"
+			//fmt.Println(eventLabel)
+			return "FindTrafficLight"
+		}
+	}
+	count1++
+	return ""
+}
+
+func IfEnter(pointlist []Point, radius float64, lat, lon float64) bool {
+	// 判断是否进入点列表中的区域
+	point1 := Point{Latitude: lat, Longitude: lon}
+	for _, point := range pointlist {
+		d := distance(point1, point)
+		if d <= radius {
+			return true
+		}
+	}
+	return false
+}
+
+// 计算两点之间的距离（米）
+func distance(point1, point2 Point) float64 {
+	// 经纬度转弧度
+	lat1 := point1.Latitude * math.Pi / 180
+	lon1 := point1.Longitude * math.Pi / 180
+	lat2 := point2.Latitude * math.Pi / 180
+	lon2 := point2.Longitude * math.Pi / 180
+
+	// 计算距离
+	dlon := lon2 - lon1
+	dlat := lat2 - lat1
+	a := math.Sin(dlat/2)*math.Sin(dlat/2) + math.Sin(dlon/2)*math.Sin(dlon/2)*math.Cos(lat1)*math.Cos(lat2)
+	c := 2 * math.Atan2(math.Sqrt(a), math.Sqrt(1-a))
+	d := 6371000 * c
+	return d
+}

trigger/pjisuv/tpperception/TargetAhead/main/TargetAhead.go

@@ -0,0 +1,27 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"cicv-data-closedloop/pjisuv_param"
+	"math"
+)
+
+func Topic() string {
+	return "/tpperception"
+}
+
+// Label todo 禁止存在下划线_
+func Label() string {
+	return "TargetAhead"
+}
+
+func Rule(data *pjisuv_msgs.PerceptionObjects, param pjisuv_param.PjisuvParam) string {
+	for _, obj := range data.Objs {
+		if math.Abs(float64(obj.Y)) <= 1.3 && obj.X >= 4 && obj.X <= 15 && param.AngularVelocityZOfCicvLocation < 5.5 {
+			//event_label := "TargetAhead" //测试车正前方１０米范围内有目标物（过滤掉测试车转弯的情况）
+			//fmt.Println(event_label)
+			return "TargetAhead"
+		}
+	}
+	return ""
+}