adjust pjisuv FindTrafficLight trigger

add pji_deliver PassManholeCover / EnterTjunction trigger

trigger/pjibot_delivery/robot_pose/EnterTjunction/main/EnterTjunction.go

@@ -0,0 +1,66 @@
+package main
+
+import (
+	"fmt"
+	"github.com/bluenviron/goroslib/v2/pkg/msgs/geometry_msgs"
+	"math"
+)
+
+func Topic() string {
+	return "/robot_pose"
+}
+
+// Label todo 禁止存在下划线_
+func Label() string {
+	return "EnterTjunction"
+}
+
+type Point struct {
+	X float64
+	Y float64
+}
+
+var (
+
+	//定义园区部门Ｔ字路口ＵＴＭ坐标
+	point3 = Point{35.5711, 150.49}
+	point4 = Point{108.108, 197.537}
+	point5 = Point{76.0262, 78.9898}
+	point6 = Point{149.308, 118.211}
+	point7 = Point{105.141, 21.5495}
+	point8 = Point{180.037, 64.8318}
+
+	pointlist = []Point{point3, point4, point5, point6, point7, point8}
+)
+
+func Rule(data *geometry_msgs.PoseStamped) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	enterflag := IfEnter(pointlist, 15, data.Pose.Position.X, data.Pose.Position.Y)
+	if enterflag {
+		return "EnterTjunction"
+	}
+
+	return ""
+}
+func IfEnter(pointlist []Point, radius float64, x, y float64) bool {
+	// 判断是否进入点列表中的区域
+	point1 := Point{X: x, Y: y}
+	for _, point := range pointlist {
+		d := distance(point1, point)
+		if d <= radius {
+			return true
+		}
+	}
+	return false
+}
+
+// 计算两点之间的距离（米）
+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
+}

trigger/pjibot_delivery/robot_pose/PassManholeCover/main/PassManholeCover.go

@@ -0,0 +1,65 @@
+package main
+
+import (
+	"fmt"
+	"github.com/bluenviron/goroslib/v2/pkg/msgs/geometry_msgs"
+	"math"
+)
+
+func Topic() string {
+	return "/robot_pose"
+}
+
+// Label todo 禁止存在下划线_
+func Label() string {
+	return "PassManholeCover"
+}
+
+type Point struct {
+	X float64
+	Y float64
+}
+
+var (
+
+	//定义园区部分井盖ＵＴＭ坐标
+	point3 = Point{-29.5885, 108.087}
+	point4 = Point{209.923, 210.394}
+	point5 = Point{16.441, -37.0252}
+	point6 = Point{-53.9421, -74.9124}
+	point7 = Point{232.502, 172.084}
+
+	pointlist = []Point{point3, point4, point5, point6, point7}
+)
+
+func Rule(data *geometry_msgs.PoseStamped) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+	enterflag := IfEnter(pointlist, 1.5, data.Pose.Position.X, data.Pose.Position.Y)
+	if enterflag {
+		return "PassManholeCover"
+	}
+
+	return ""
+}
+func IfEnter(pointlist []Point, radius float64, x, y float64) bool {
+	// 判断是否进入点列表中的区域
+	point1 := Point{X: x, Y: y}
+	for _, point := range pointlist {
+		d := distance(point1, point)
+		if d <= radius {
+			return true
+		}
+	}
+	return false
+}
+
+// 计算两点之间的距离（米）
+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
+}

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

@@ -40,7 +40,7 @@ func Rule(data *pjisuv_msgs.PerceptionLocalization) string {
 	}()
 	if count1%10 == 0 {
 
-		enterflag := IfEnter(pointlist, 15.0, data.Latitude, data.Longitude)
+		enterflag := IfEnter(pointlist, 25.0, data.Latitude, data.Longitude)
 		if enterflag {
 			//eventLabel := "FindTrafficLight"
 			//fmt.Println(eventLabel)