commit

aarch64/pjisuv/master/package/config/master_trigger_config.go

@@ -191,6 +191,14 @@ var (
 	RuleOfRoutingRviz  []func(data *nav_msgs.Path) string
 
 	//44
+	TopicOfRouteMessage = "/route_message"
+	RuleOfRouteMessage  []func(data *pjisuv_msgs.Route) string
+
+	//45
+	TopicOfRouteResultMessage = "/route_result_message"
+	RuleOfRouteResultMessage  []func(data *pjisuv_msgs.Route) string
+
+	//46
 	TopicOfDataRead = "/data_read"
 	RuleOfDataRead  []func(data *pjisuv_msgs.Retrieval) string
 
@@ -538,7 +546,21 @@ func InitTriggerConfig() {
 				continue
 			}
 			RuleOfRoutingRviz = append(RuleOfRoutingRviz, f)
-		} else if TopicOfDataRead == topic2 { //44
+		} else if TopicOfRouteMessage == topic2 { //44
+			f, ok := rule.(func(data *pjisuv_msgs.Route) string)
+			if ok != true {
+				c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *pjisuv_msgs.Route) string):", err)
+				continue
+			}
+			RuleOfRouteMessage = append(RuleOfRouteMessage, f)
+		} else if TopicOfRouteResultMessage == topic2 { //45
+			f, ok := rule.(func(data *pjisuv_msgs.Route) string)
+			if ok != true {
+				c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *pjisuv_msgs.Route) string):", err)
+				continue
+			}
+			RuleOfRouteResultMessage = append(RuleOfRouteResultMessage, f)
+		} else if TopicOfDataRead == topic2 { //46
 			f, ok := rule.(func(data *pjisuv_msgs.Retrieval) string)
 			if ok != true {
 				c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *pjisuv_msgs.Retrieval) string):", err)

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

@@ -1195,8 +1195,60 @@ func PrepareTimeWindowProducerQueue() {
 				},
 			})
 		}
-
 		// 44
+		if topic == masterConfig.TopicOfRouteMessage && len(masterConfig.RuleOfRouteMessage) > 0 {
+			subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
+				Node:  commonConfig.RosNode,
+				Topic: topic,
+				Callback: func(data *pjisuv_msgs.Route) {
+					subscribersTimeMutexes[i].Lock()
+					if time.Since(subscribersTimes[i]).Seconds() > 1 {
+						subscribersMutexes[i].Lock()
+						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
+						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						var faultLabel string
+						for _, f := range masterConfig.RuleOfRouteMessage {
+							faultLabel = f(data)
+							if faultLabel != "" {
+								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
+								subscribersTimes[i] = time.Now()
+								break
+							}
+						}
+						subscribersMutexes[i].Unlock()
+					}
+					subscribersTimeMutexes[i].Unlock()
+				},
+			})
+		}
+		// 45
+		if topic == masterConfig.TopicOfRouteResultMessage && len(masterConfig.RuleOfRouteResultMessage) > 0 {
+			subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
+				Node:  commonConfig.RosNode,
+				Topic: topic,
+				Callback: func(data *pjisuv_msgs.Route) {
+					subscribersTimeMutexes[i].Lock()
+					if time.Since(subscribersTimes[i]).Seconds() > 1 {
+						subscribersMutexes[i].Lock()
+						faultHappenTime := util.GetNowTimeCustom()   // 获取当前故障发生时间
+						lastTimeWindow := entity.GetLastTimeWindow() // 获取最后一个时间窗口
+						var faultLabel string
+						for _, f := range masterConfig.RuleOfRouteResultMessage {
+							faultLabel = f(data)
+							if faultLabel != "" {
+								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
+								subscribersTimes[i] = time.Now()
+								break
+							}
+						}
+						subscribersMutexes[i].Unlock()
+					}
+					subscribersTimeMutexes[i].Unlock()
+				},
+			})
+		}
+
+		// 46
 		if topic == masterConfig.TopicOfDataRead && len(masterConfig.RuleOfDataRead) > 0 {
 			subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
 				Node:  commonConfig.RosNode,

pjisuv_msgs/route_engine.go

@@ -0,0 +1,16 @@
+package pjisuv_msgs
+
+import (
+	"github.com/bluenviron/goroslib/v2/pkg/msg"
+	"github.com/bluenviron/goroslib/v2/pkg/msgs/geometry_msgs"
+)
+
+type Route struct {
+	msg.Package          `ros:"route_engine"`
+	Start                geometry_msgs.Point   `rosname:"start"`
+	Destination          geometry_msgs.Point   `rosname:"destination"`
+	Path                 []geometry_msgs.Point `rosname:"path"`
+	Ids                  []string              `rosname:"ids"`
+	StartPointAcceptTime float64               `rosname:"start_point_accept_time"`
+	IsSuccess            bool                  `rosname:"isSuccess"`
+}