add navfailed trigger for Pji_guide

kinglong_msgs/control_msgs.go

@@ -48,6 +48,7 @@ type JinlongControlCommand struct {
 	ASHighBeamStartReq            int16   `rosname:"AS_HighBeam_Start_Req"`
 	TargetX                       float64
 	TargetY                       float64
+	LateralError                  float64
 	Velocity                      float64
 	AutoMode                      int16
 }

trigger/pjibot_guide/move_base_status/navfailed/main/main.go

@@ -0,0 +1,22 @@
+package main
+
+import (
+	"github.com/bluenviron/goroslib/v2/pkg/msgs/actionlib_msgs"
+)
+
+func Topic() string {
+	return "/move_base/status"
+}
+
+func Label() string {
+	return "navfailed"
+}
+
+func Rule(data *actionlib_msgs.GoalStatusArray) string {
+	for _, status := range data.StatusList {
+		if status.Status == 5 {
+			return "navfailed"
+		}
+	}
+	return ""
+}

trigger/pjisuv/cicv_ticker/CBFA/main/CBFA.go

@@ -73,8 +73,10 @@ func isCrossAndOcclusion(id uint32, ObjectList [][]float32, ObjectSlice map[uint
 							this_startFrame_index2 := findIndex(objValue[3], startFrame2)
 							this_type := objValue[6][0]
 							//fmt.Println(objValue[0][this_startFrame_index2], xj)
-							if objValue[0][this_startFrame_index1] >= 2 && objValue[0][this_startFrame_index1] < xi-1 && objValue[1][this_startFrame_index1] > 0 && objValue[1][this_startFrame_index1] < yi && objValue[0][this_startFrame_index2] >= 1 && objValue[0][this_startFrame_index2] < xj-1 && objValue[1][this_startFrame_index2] > 0 && (this_type == 2.0 || this_type == 3.0) {
-								return true
+							if this_startFrame_index1 != -1 && this_startFrame_index2 != -1 {
+								if objValue[0][this_startFrame_index1] >= 2 && objValue[0][this_startFrame_index1] < xi-1 && objValue[1][this_startFrame_index1] > 0 && objValue[1][this_startFrame_index1] < yi && objValue[0][this_startFrame_index2] >= 1 && objValue[0][this_startFrame_index2] < xj-1 && objValue[1][this_startFrame_index2] > 0 && (this_type == 2.0 || this_type == 3.0) {
+									return true
+								}
 							}
 						} else {
 							continue

trigger/pjisuv/cicv_ticker/CBNA/main/CBNA.go

@@ -73,8 +73,10 @@ func isCrossAndOcclusion(id uint32, ObjectList [][]float32, ObjectSlice map[uint
 							this_startFrame_index2 := findIndex(objValue[3], startFrame2)
 							this_type := objValue[6][0]
 							//fmt.Println(objValue[0][this_startFrame_index2], xj)
-							if objValue[0][this_startFrame_index1] >= 2 && objValue[0][this_startFrame_index1] < xi-1 && objValue[1][this_startFrame_index1] < 0 && objValue[1][this_startFrame_index1] > yi && objValue[0][this_startFrame_index2] >= 1 && objValue[0][this_startFrame_index2] < xj-1 && objValue[1][this_startFrame_index2] < 0 && (this_type == 2.0 || this_type == 3.0) {
-								return true
+							if this_startFrame_index1 != -1 && this_startFrame_index2 != -1 {
+								if objValue[0][this_startFrame_index1] >= 2 && objValue[0][this_startFrame_index1] < xi-1 && objValue[1][this_startFrame_index1] < 0 && objValue[1][this_startFrame_index1] > yi && objValue[0][this_startFrame_index2] >= 1 && objValue[0][this_startFrame_index2] < xj-1 && objValue[1][this_startFrame_index2] < 0 && (this_type == 2.0 || this_type == 3.0) {
+									return true
+								}
 							}
 						} else {
 							continue

trigger/pjisuv/cicv_ticker/CBNAO/main/CBNAO.go

@@ -73,8 +73,10 @@ func isCrossAndOcclusion(id uint32, ObjectList [][]float32, ObjectSlice map[uint
 							this_startFrame_index2 := findIndex(objValue[3], startFrame2)
 							this_type := objValue[6][0]
 							//fmt.Println(objValue[0][this_startFrame_index2], xj)
-							if objValue[0][this_startFrame_index1] >= 2 && objValue[0][this_startFrame_index1] < xi-5 && objValue[1][this_startFrame_index1] < 0 && objValue[1][this_startFrame_index1] > yi && objValue[0][this_startFrame_index2] >= 1 && objValue[0][this_startFrame_index2] < xj-3 && objValue[1][this_startFrame_index2] < 0 && (this_type == 2.0 || this_type == 3.0) {
-								return true
+							if this_startFrame_index1 != -1 && this_startFrame_index2 != -1 {
+								if objValue[0][this_startFrame_index1] >= 2 && objValue[0][this_startFrame_index1] < xi-5 && objValue[1][this_startFrame_index1] < 0 && objValue[1][this_startFrame_index1] > yi && objValue[0][this_startFrame_index2] >= 1 && objValue[0][this_startFrame_index2] < xj-3 && objValue[1][this_startFrame_index2] < 0 && (this_type == 2.0 || this_type == 3.0) {
+									return true
+								}
 							}
 						} else {
 							continue

trigger/pjisuv/cicv_ticker/CCCscpo/main/CCCscpo.go

@@ -74,9 +74,10 @@ func isCrossAndOcclusion(id uint32, ObjectList [][]float32, ObjectSlice map[uint
 							this_startFrame_index2 := findIndex(objValue[5], startFrame2)
 							this_type := objValue[6][0]
 							//fmt.Println(objValue[0][this_startFrame_index2], xj)
-
-							if objValue[0][this_startFrame_index1] >= 2 && objValue[0][this_startFrame_index1] < xi-1 && objValue[1][this_startFrame_index1] < 0 && objValue[1][this_startFrame_index1] > yi && objValue[0][this_startFrame_index2] >= 1 && objValue[0][this_startFrame_index2] < xj-1 && objValue[1][this_startFrame_index2] < 0 && (this_type == 2.0 || this_type == 3.0) {
-								return true
+							if this_startFrame_index1 != -1 && this_startFrame_index2 != -1 {
+								if objValue[0][this_startFrame_index1] >= 2 && objValue[0][this_startFrame_index1] < xi-1 && objValue[1][this_startFrame_index1] < 0 && objValue[1][this_startFrame_index1] > yi && objValue[0][this_startFrame_index2] >= 1 && objValue[0][this_startFrame_index2] < xj-1 && objValue[1][this_startFrame_index2] < 0 && (this_type == 2.0 || this_type == 3.0) {
+									return true
+								}
 							}
 						} else {
 							continue

trigger/pjisuv/cicv_ticker/CPNCO/main/CPNCO.go

@@ -72,9 +72,11 @@ func isCrossAndOcclusion(id uint32, ObjectList [][]float32, ObjectSlice map[uint
 							this_startFrame_index1 := findIndex(objValue[3], startFrame1)
 							this_startFrame_index2 := findIndex(objValue[3], startFrame2)
 							this_type := objValue[6][0]
-							//fmt.Println(objValue[0][this_startFrame_index2], xj)
-							if objValue[0][this_startFrame_index1] >= 2 && objValue[0][this_startFrame_index1] < xi-1 && objValue[1][this_startFrame_index1] < 0 && objValue[1][this_startFrame_index1] > yi && objValue[0][this_startFrame_index2] >= 1 && objValue[0][this_startFrame_index2] < xj-1 && objValue[1][this_startFrame_index2] < 0 && (this_type == 2.0 || this_type == 3.0) {
-								return true
+							if this_startFrame_index1 != -1 && this_startFrame_index2 != -1 {
+								//fmt.Println(objValue[0][this_startFrame_index2], xj)
+								if objValue[0][this_startFrame_index1] >= 2 && objValue[0][this_startFrame_index1] < xi-1 && objValue[1][this_startFrame_index1] < 0 && objValue[1][this_startFrame_index1] > yi && objValue[0][this_startFrame_index2] >= 1 && objValue[0][this_startFrame_index2] < xj-1 && objValue[1][this_startFrame_index2] < 0 && (this_type == 2.0 || this_type == 3.0) {
+									return true
+								}
 							}
 						} else {
 							continue

trigger/pjisuv/tpperception/CCFtap/main/CCFtap.go

@@ -31,9 +31,11 @@ func Rule(shareVars *sync.Map, data *pjisuv_msgs.PerceptionObjects) string {
 
 		for _, obj := range data.Objs {
 			if obj.Type == 2 || obj.Type == 3 {
+
 				Distance := math.Sqrt(math.Pow(PositionXOfCicvLocation.(float64)-obj.Xabs, 2) + math.Pow(PositionYOfCicvLocation.(float64)-obj.Yabs, 2))
+				absspeed := 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 && (math.Abs(float64(obj.Y)) <= 4.0 || Distance <= 35.0) {
+				if obj.X >= 2.0 && obj.X <= 30.0 && (math.Abs(float64(obj.Y)) <= 4.0 || Distance <= 35.0) && absspeed > 1.5 {
 					event_lable := "CCFtap"
 					fmt.Println(event_lable)
 					return Label()