commit

aarch64/pjisuv/master/package/service/produce_window.go

@@ -8,7 +8,7 @@ import (
 	"cicv-data-closedloop/kinglong/common/svc"
 	"cicv-data-closedloop/kinglong/common/util"
 	masterConfig "cicv-data-closedloop/kinglong/master/pkg/cfg"
-	"cicv-data-closedloop/kinglong_msgs"
+	"cicv-data-closedloop/pjisuv_msgs"
 	"github.com/bluenviron/goroslib/v2"
 	"os"
 	"sync"
@@ -36,7 +36,7 @@ func PrepareTimeWindowProducerQueue() {
 	subscriber0, err := goroslib.NewSubscriber(goroslib.SubscriberConf{
 		Node:  commonConfig.RosNode,
 		Topic: masterConfig.TopicOfNodeFaultInfo,
-		Callback: func(data *kinglong_msgs.FaultInfo) {
+		Callback: func(data *pjisuv_msgs.FaultInfo) {
 			if len(masterConfig.RuleOfNodefaultInfo) == 0 {
 				//log.GlobalLogger.Info("话题 nodefault_info没有触发器")
 				return
@@ -78,7 +78,7 @@ func PrepareTimeWindowProducerQueue() {
 	subscriber1, err := goroslib.NewSubscriber(goroslib.SubscriberConf{
 		Node:  commonConfig.RosNode,
 		Topic: masterConfig.TopicOfCicvLocation,
-		Callback: func(data *kinglong_msgs.PerceptionLocalization) {
+		Callback: func(data *pjisuv_msgs.PerceptionLocalization) {
 			if len(masterConfig.RuleOfCicvLocation) == 0 {
 				log.GlobalLogger.Info("话题 cicv_location 没有触发器")
 				return
@@ -116,7 +116,7 @@ func PrepareTimeWindowProducerQueue() {
 	subscriber2, err := goroslib.NewSubscriber(goroslib.SubscriberConf{
 		Node:  commonConfig.RosNode,
 		Topic: masterConfig.TopicOfTpperception,
-		Callback: func(data *kinglong_msgs.PerceptionObjects) {
+		Callback: func(data *pjisuv_msgs.PerceptionObjects) {
 			if len(masterConfig.RuleOfTpperception) == 0 {
 				log.GlobalLogger.Info("话题 tpperception 没有触发器")
 				return
@@ -157,7 +157,7 @@ func PrepareTimeWindowProducerQueue() {
 	subscriber3, err := goroslib.NewSubscriber(goroslib.SubscriberConf{
 		Node:  commonConfig.RosNode,
 		Topic: masterConfig.TopicOfFaultInfo,
-		Callback: func(data *kinglong_msgs.FaultVec) {
+		Callback: func(data *pjisuv_msgs.FaultVec) {
 			if len(masterConfig.RuleOfFaultInfo) == 0 {
 				log.GlobalLogger.Info("话题 fault_info 没有触发器")
 				return
@@ -200,7 +200,7 @@ func PrepareTimeWindowProducerQueue() {
 	subscriber4, err := goroslib.NewSubscriber(goroslib.SubscriberConf{
 		Node:  commonConfig.RosNode,
 		Topic: masterConfig.TopicOfDataRead,
-		Callback: func(data *kinglong_msgs.Retrieval) {
+		Callback: func(data *pjisuv_msgs.Retrieval) {
 			if len(masterConfig.RuleOfDataRead) == 0 {
 				//log.GlobalLogger.Info("话题 data_read 没有触发器")
 				return

kinglong/master/pkg/cfg/master_trigger_cfg.go

@@ -4,7 +4,7 @@ import (
 	"cicv-data-closedloop/kinglong/common/cfg"
 	"cicv-data-closedloop/kinglong/common/cutil"
 	"cicv-data-closedloop/kinglong/common/log"
-	"cicv-data-closedloop/kinglong_msgs"
+	"cicv-data-closedloop/pjisuv_msgs"
 	"plugin"
 	"strconv"
 	"strings"
@@ -16,11 +16,11 @@ var (
 	TopicOfTpperception  = "/tpperception"
 	TopicOfFaultInfo     = "/fault_info"
 	TopicOfDataRead      = "/data_read"
-	RuleOfNodefaultInfo  []func(data *kinglong_msgs.FaultInfo) string
-	RuleOfCicvLocation   []func(data *kinglong_msgs.PerceptionLocalization) string
-	RuleOfTpperception   []func(data *kinglong_msgs.PerceptionObjects, velocityX float64, velocityY float64, yaw float64) string
-	RuleOfFaultInfo      []func(data *kinglong_msgs.FaultVec) string
-	RuleOfDataRead       []func(data *kinglong_msgs.Retrieval) string
+	RuleOfNodefaultInfo  []func(data *pjisuv_msgs.FaultInfo) string
+	RuleOfCicvLocation   []func(data *pjisuv_msgs.PerceptionLocalization) string
+	RuleOfTpperception   []func(data *pjisuv_msgs.PerceptionObjects, velocityX float64, velocityY float64, yaw float64) string
+	RuleOfFaultInfo      []func(data *pjisuv_msgs.FaultVec) string
+	RuleOfDataRead       []func(data *pjisuv_msgs.Retrieval) string
 	LabelMapTriggerId    = make(map[string]string)
 )
 
@@ -63,35 +63,35 @@ func InitTriggerConfig() {
 			continue
 		}
 		if TopicOfNodeFaultInfo == topic2 {
-			f, ok := rule.(func(data *kinglong_msgs.FaultInfo) string)
+			f, ok := rule.(func(data *pjisuv_msgs.FaultInfo) string)
 			if ok != true {
 				log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *kinglong_msgs.FaultInfo) string):", err)
 				continue
 			}
 			RuleOfNodefaultInfo = append(RuleOfNodefaultInfo, f)
 		} else if TopicOfCicvLocation == topic2 {
-			f, ok := rule.(func(data *kinglong_msgs.PerceptionLocalization) string)
+			f, ok := rule.(func(data *pjisuv_msgs.PerceptionLocalization) string)
 			if ok != true {
 				log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *kinglong_msgs.PerceptionLocalization) string):", err)
 				continue
 			}
 			RuleOfCicvLocation = append(RuleOfCicvLocation, f)
 		} else if TopicOfTpperception == topic2 {
-			f, ok := rule.(func(data *kinglong_msgs.PerceptionObjects, velocityX float64, velocityY float64, yaw float64) string)
+			f, ok := rule.(func(data *pjisuv_msgs.PerceptionObjects, velocityX float64, velocityY float64, yaw float64) string)
 			if ok != true {
 				log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *kinglong_msgs.PerceptionObjects, velocityX float64, velocityY float64, yaw float64) string):", err)
 				continue
 			}
 			RuleOfTpperception = append(RuleOfTpperception, f)
 		} else if TopicOfFaultInfo == topic2 {
-			f, ok := rule.(func(data *kinglong_msgs.FaultVec) string)
+			f, ok := rule.(func(data *pjisuv_msgs.FaultVec) string)
 			if ok != true {
 				log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *kinglong_msgs.FaultVec) string):", err)
 				continue
 			}
 			RuleOfFaultInfo = append(RuleOfFaultInfo, f)
 		} else if TopicOfDataRead == topic2 {
-			f, ok := rule.(func(data *kinglong_msgs.Retrieval) string)
+			f, ok := rule.(func(data *pjisuv_msgs.Retrieval) string)
 			if ok != true {
 				log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *kinglong_msgs.Retrieval) string):", err)
 				continue

trigger/pjisuv/cicv_location/brake/main/brake.go

@@ -0,0 +1,21 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+func Label() string {
+	return "brake"
+}
+
+func Rule(data *pjisuv_msgs.PerceptionLocalization) string {
+	if data.AccelX*9.8 < -2.5 {
+		return "brake"
+	} else {
+		return ""
+	}
+}

trigger/pjisuv/tpperception/TTC/main/TTC.go

@@ -0,0 +1,33 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"math"
+)
+
+func Topic() string {
+	return "/tpperception"
+}
+
+func Label() string {
+	return "TTC"
+}
+
+func Rule(data *pjisuv_msgs.PerceptionObjects, velocityX float64, velocityY float64, yaw float64) string {
+	for _, obj := range data.Objs {
+		if math.Abs(float64(obj.Y)) <= 2 && obj.X >= 8 {
+			theta := DegreesToRadians(yaw)
+			vxrel := (float64(obj.Vxabs)-velocityX)*math.Cos(theta) + (float64(obj.Vyabs)-velocityY)*math.Sin(theta)
+			ttc := -((float64(obj.X) - 5.2) / (vxrel + 0.001))
+			if ttc >= 0 && ttc <= 3 {
+				return "TTC"
+			}
+		}
+	}
+	return ""
+}
+
+// DegreesToRadians 计算角度转弧度
+func DegreesToRadians(degrees float64) float64 {
+	return degrees * math.Pi / 180.0
+}