Merge branch 'master' of http://36.110.106.156:13000/cicv/cicv_data_closed_loop

aarch64/pjisuv/README.md

@@ -5,3 +5,8 @@
 ## master 102 程序
 - trigger_init.go：102 初始化加载触发器插件
 ## slave 103 程序
+
+
+
+# 添加共享变量
+./master/service/produce_window.go

test/goroslib_test.go

@@ -0,0 +1,117 @@
+package test
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"github.com/bluenviron/goroslib/v2"
+	"log"
+	"math"
+	"testing"
+	"time"
+)
+
+// speedCheck 目标物速度检测
+func speedCheck(obj *pjisuv_msgs.PerceptionObject) bool {
+	var targetMinSpeed float32 = 2 / 3.6  // m/s
+	var targetMaxSpeed float32 = 20 / 3.6 // m/s
+
+	if targetMinSpeed < obj.Speed && obj.Speed < targetMaxSpeed {
+		return true
+	}
+	return false
+}
+
+// typeCheck 目标物类型检测
+// × 金龙车：CAR_TYPE=0, TRUCK_TYPE=1, PEDESTRIAN_TYPE=2, CYCLIST_TYPE=3, UNKNOWN_TYPE=4, UNKNOWN_MOVABLE_TYPE=5, UNKNOWN_UNMOVABLE_TYPE=6
+// √ 多功能车：UNKNOWN TYPE=O, PEDESTRIAN TYPE=1, CAR TYPE=2, TRUCK TYPE=3, Bicycle TYPE=4, Tricycle TYPE=5, Traffic Cone TYPE=6
+func typeCheck(obj *pjisuv_msgs.PerceptionObject) bool {
+	var targetType uint8 = 3
+
+	if targetType == obj.Type {
+		return true
+	}
+	return false
+}
+
+// sizeCheck 目标物大小检测
+func sizeCheck(obj *pjisuv_msgs.PerceptionObject) bool {
+	// 多功能车
+	var targetMinLength float32 = 3.6   // m
+	var targetMinWidth float32 = 1.605  // m
+	var targetMinHeight float32 = 1.995 // m
+
+	// 金龙车
+	//targetMinLength := 5.99
+	//targetMinWidth := 2.065
+	//targetMinHeight := 2.82
+
+	if obj.Length > targetMinLength || obj.Width > targetMinWidth || obj.Height > targetMinHeight {
+		return true
+	}
+	return false
+}
+
+// posCheck 判断目标物位置关系
+func posCheck(obj *pjisuv_msgs.PerceptionObject) bool {
+	laneWidth := 3.5 // m
+
+	if obj.X > 0 && math.Abs(float64(obj.Y)) < laneWidth*1.5 {
+		return true
+	}
+	return false
+}
+
+// Rule 感知算法识别大车、有车辆在前方车道且低速行驶
+func Rule(msg *pjisuv_msgs.PerceptionObjects) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+
+	CGCS2000_X := 456256.260152
+	CGCS2000_Y := 4397809.886833
+
+	fmt.Println()
+	fmt.Println(time.Unix(int64(msg.Header.TimeStamp), 0).Format(time.DateTime))
+	for _, obj := range msg.Objs {
+		fmt.Println(fmt.Sprintf("id: [%d], type: [%d], x/yrel: [%f, %f], x/yabs: [%f, %f], speed: [%f], size: [%f/%f/%f]",
+			obj.Id, obj.Type, obj.X, obj.Y, obj.Xabs-CGCS2000_X, obj.Yabs-CGCS2000_Y, obj.Speed, obj.Length, obj.Width, obj.Height))
+
+		// todo: 对各判断条件单独进行稳定性校验？
+		if speedCheck(&obj) && posCheck(&obj) && (typeCheck(&obj) || sizeCheck(&obj)) {
+			fmt.Println("!!!")
+			//return "LowSpdTruckAhead"
+		}
+	}
+	return ""
+}
+
+func TestGoRosLib(t *testing.T) {
+	defer func() {
+		if err := recover(); err != nil {
+			log.Println(err, fmt.Sprintf("recover: [%#v]", err), false)
+		}
+	}()
+
+	rosNode, err := goroslib.NewNode(goroslib.NodeConf{
+		Name:          "eyTest",
+		MasterAddress: "localhost:11311",
+	})
+	if err != nil {
+		log.Panicln(err, fmt.Sprintf("failed to create rosNode: [%#v]", err), false)
+	}
+
+	_, err = goroslib.NewSubscriber(goroslib.SubscriberConf{
+		Node:  rosNode,
+		Topic: "/tpperception",
+		Callback: func(msg *pjisuv_msgs.PerceptionObjects) {
+			Rule(msg)
+		},
+	})
+	if err != nil {
+		log.Panicln(err, fmt.Sprintf("failed to create subscriber: [%#v]", err), false)
+	}
+
+	select {}
+}

trigger/pjisuv/cicv_location/Brake/main/Brake.go

@@ -20,7 +20,7 @@ func Rule(data *pjisuv_msgs.PerceptionLocalization) string {
 			fmt.Println("Recovered from panic:", r)
 		}
 	}()
-	if data.AccelX*9.8 < -4.0 {
+	if data.AccelX*9.8 < -14.0 {
 		return "Brake"
 	} else {
 		return ""

trigger/pjisuv/cicv_location/OverSwing/main/OverSwing.go

@@ -2,6 +2,7 @@ package main
 
 import (
 	"cicv-data-closedloop/pjisuv_msgs"
+	"cicv-data-closedloop/pjisuv_param"
 	"fmt"
 	"math"
 )
@@ -34,13 +35,13 @@ func Label() string {
 	return "OverSwing"
 }
 
-func Rule(data *pjisuv_msgs.PerceptionLocalization) string {
+func Rule(data *pjisuv_msgs.PerceptionLocalization, param *pjisuv_param.PjisuvParam) string {
 	defer func() {
 		if r := recover(); r != nil {
 			fmt.Println("Recovered from panic:", r)
 		}
 	}()
-	if math.Abs(data.AngularVelocityZ) >= 27.0 {
+	if math.Abs(data.AngularVelocityZ) >= 27.0 && param.AutomodeOfPjVehicleFdbPub == 1 {
 		return "OverSwing"
 	} else {
 		return ""

trigger/pjisuv/tpperception/NumTargetsExceedThreshold/main/NumTargetsExceedThreshold.go

@@ -0,0 +1,39 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"cicv-data-closedloop/pjisuv_param"
+	"fmt"
+)
+
+func Topic() string {
+	return "/tpperception"
+}
+
+// Label todo 禁止存在下划线_
+func Label() string {
+	return "NumTargetsExceedThreshold"
+}
+func Rule(data *pjisuv_msgs.PerceptionObjects, param *pjisuv_param.PjisuvParam) string {
+	NumTargets := 0
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+
+	if param.VelocityXOfCicvLocation > 2.5 && len(data.Objs) > 6 {
+		for _, obj := range data.Objs {
+			if obj.Type != 0 && obj.X >= 2 {
+				NumTargets++
+			}
+		}
+		//fmt.Println(NumTargets)
+		if NumTargets >= 5 {
+			//event_lable := "NumTargetsExceedThreshold"
+			fmt.Printf("NumTargetsExceedThreshold,%d Targets were found\n", NumTargets)
+			return "NumTargetsExceedThreshold"
+		}
+	}
+	return ""
+}

trigger/pjisuv/tpperception/TrafficCongestion/main/TrafficCongestion.go

@@ -0,0 +1,41 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"cicv-data-closedloop/pjisuv_param"
+	"fmt"
+	"math"
+)
+
+func Topic() string {
+	return "/tpperception"
+}
+
+// Label todo 禁止存在下划线_
+func Label() string {
+	return "TrafficCongestion"
+}
+func Rule(data *pjisuv_msgs.PerceptionObjects, param *pjisuv_param.PjisuvParam) string {
+	NumTargets := 0
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+
+	if param.VelocityXOfCicvLocation < 3.5 && len(data.Objs) > 3 {
+		for _, obj := range data.Objs {
+			if (obj.Type == 2 || obj.Type == 3) && math.Abs(float64(obj.Y)) <= 2.5 && math.Abs(float64(obj.X)) <= 20 && obj.Speed <= 4 {
+				NumTargets++
+			}
+		}
+		//fmt.Println(NumTargets)
+		if NumTargets >= 3 {
+			return "TrafficCongestion"
+
+		}
+	}
+
+	return ""
+
+}

trigger/pjisuv/tpperception/lowSpdTruckAhead/main/lowSpdTruckAhead.go

@@ -0,0 +1,85 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
+)
+
+func Topic() string {
+	return "/tpperception"
+}
+
+// Label todo 禁止存在下划线_
+func Label() string {
+	return "LowSpdTruckAhead"
+}
+
+// speedCheck 目标物速度检测
+func speedCheck(obj *pjisuv_msgs.PerceptionObject) bool {
+	var targetMinSpeed float32 = 2 / 3.6  // m/s
+	var targetMaxSpeed float32 = 20 / 3.6 // m/s
+
+	if targetMinSpeed < obj.Speed && obj.Speed < targetMaxSpeed {
+		return true
+	}
+	return false
+}
+
+// typeCheck 目标物类型检测
+// × 金龙车：CAR_TYPE=0, TRUCK_TYPE=1, PEDESTRIAN_TYPE=2, CYCLIST_TYPE=3, UNKNOWN_TYPE=4, UNKNOWN_MOVABLE_TYPE=5, UNKNOWN_UNMOVABLE_TYPE=6
+// √ 多功能车：UNKNOWN TYPE=O, PEDESTRIAN TYPE=1, CAR TYPE=2, TRUCK TYPE=3, Bicycle TYPE=4, Tricycle TYPE=5, Traffic Cone TYPE=6
+func typeCheck(obj *pjisuv_msgs.PerceptionObject) bool {
+	var targetType uint8 = 3
+
+	if targetType == obj.Type {
+		return true
+	}
+	return false
+}
+
+// sizeCheck 目标物大小检测
+func sizeCheck(obj *pjisuv_msgs.PerceptionObject) bool {
+	// 多功能车
+	var targetMinLength float32 = 3.6   // m
+	var targetMinWidth float32 = 1.605  // m
+	var targetMinHeight float32 = 1.995 // m
+
+	// 金龙车
+	//targetMinLength := 5.99
+	//targetMinWidth := 2.065
+	//targetMinHeight := 2.82
+
+	if obj.Length > targetMinLength || obj.Width > targetMinWidth || obj.Height > targetMinHeight {
+		return true
+	}
+	return false
+}
+
+// posCheck 判断目标物位置关系
+func posCheck(obj *pjisuv_msgs.PerceptionObject) bool {
+	laneWidth := 3.5 // m
+
+	if obj.X > 0 && math.Abs(float64(obj.Y)) < laneWidth*1.5 {
+		return true
+	}
+	return false
+}
+
+// Rule 感知算法识别大车、有车辆在前方车道且低速行驶
+func Rule(msg *pjisuv_msgs.PerceptionObjects) string {
+	defer func() {
+		if r := recover(); r != nil {
+			fmt.Println("Recovered from panic:", r)
+		}
+	}()
+
+	for _, obj := range msg.Objs {
+
+		// todo: 对各判断条件单独进行稳定性校验？
+		if speedCheck(&obj) && posCheck(&obj) && (typeCheck(&obj) || sizeCheck(&obj)) {
+			return "LowSpdTruckAhead"
+		}
+	}
+	return ""
+}