|
@@ -12,7 +12,6 @@ import (
|
|
"github.com/bluenviron/goroslib/v2/pkg/msgs/geometry_msgs"
|
|
"github.com/bluenviron/goroslib/v2/pkg/msgs/geometry_msgs"
|
|
"github.com/bluenviron/goroslib/v2/pkg/msgs/nav_msgs"
|
|
"github.com/bluenviron/goroslib/v2/pkg/msgs/nav_msgs"
|
|
"github.com/bluenviron/goroslib/v2/pkg/msgs/sensor_msgs"
|
|
"github.com/bluenviron/goroslib/v2/pkg/msgs/sensor_msgs"
|
|
- "github.com/bluenviron/goroslib/v2/pkg/msgs/std_msgs"
|
|
|
|
"github.com/bluenviron/goroslib/v2/pkg/msgs/tf2_msgs"
|
|
"github.com/bluenviron/goroslib/v2/pkg/msgs/tf2_msgs"
|
|
"github.com/bluenviron/goroslib/v2/pkg/msgs/visualization_msgs"
|
|
"github.com/bluenviron/goroslib/v2/pkg/msgs/visualization_msgs"
|
|
"sync"
|
|
"sync"
|
|
@@ -988,267 +987,6 @@ func PrepareTimeWindowProducerQueue() {
|
|
})
|
|
})
|
|
}
|
|
}
|
|
// 36
|
|
// 36
|
|
- if topic == masterConfig.TopicOfCameraObstacles && len(masterConfig.RuleOfCameraObstacles) > 0 {
|
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
|
- Node: commonConfig.RosNode,
|
|
|
|
- Topic: topic,
|
|
|
|
- Callback: func(data *pjisuv_msgs.CameraObjectList) {
|
|
|
|
- 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.RuleOfCameraObstacles {
|
|
|
|
- faultLabel = f(data)
|
|
|
|
- if faultLabel != "" {
|
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
|
- break
|
|
|
|
- }
|
|
|
|
- }
|
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
|
- }
|
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
|
- },
|
|
|
|
- })
|
|
|
|
- }
|
|
|
|
- // 37
|
|
|
|
- if topic == masterConfig.TopicOfCamRes && len(masterConfig.RuleOfCamRes) > 0 {
|
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
|
- Node: commonConfig.RosNode,
|
|
|
|
- Topic: topic,
|
|
|
|
- Callback: func(data *sensor_msgs.Image) {
|
|
|
|
- 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.RuleOfCamRes {
|
|
|
|
- faultLabel = f(data)
|
|
|
|
- if faultLabel != "" {
|
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
|
- break
|
|
|
|
- }
|
|
|
|
- }
|
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
|
- }
|
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
|
- },
|
|
|
|
- })
|
|
|
|
- }
|
|
|
|
- // 38
|
|
|
|
- if topic == masterConfig.TopicOfCamObjects && len(masterConfig.RuleOfCamObjects) > 0 {
|
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
|
- Node: commonConfig.RosNode,
|
|
|
|
- Topic: topic,
|
|
|
|
- Callback: func(data *pjisuv_msgs.CameraObjectList) {
|
|
|
|
- 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.RuleOfCamObjects {
|
|
|
|
- faultLabel = f(data)
|
|
|
|
- if faultLabel != "" {
|
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
|
- break
|
|
|
|
- }
|
|
|
|
- }
|
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
|
- }
|
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
|
- },
|
|
|
|
- })
|
|
|
|
- }
|
|
|
|
- // 39
|
|
|
|
- if topic == masterConfig.TopicOfTftrafficlight && len(masterConfig.RuleOfTftrafficlight) > 0 {
|
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
|
- Node: commonConfig.RosNode,
|
|
|
|
- Topic: topic,
|
|
|
|
- Callback: func(data *pjisuv_msgs.TrafficLightDetection) {
|
|
|
|
- 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.RuleOfTftrafficlight {
|
|
|
|
- faultLabel = f(data)
|
|
|
|
- if faultLabel != "" {
|
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
|
- break
|
|
|
|
- }
|
|
|
|
- }
|
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
|
- }
|
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
|
- },
|
|
|
|
- })
|
|
|
|
- }
|
|
|
|
- // 40
|
|
|
|
- if topic == masterConfig.TopicOfStationStatus && len(masterConfig.RuleOfStationStatus) > 0 {
|
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
|
- Node: commonConfig.RosNode,
|
|
|
|
- Topic: topic,
|
|
|
|
- Callback: func(data *std_msgs.Bool) {
|
|
|
|
- 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.RuleOfStationStatus {
|
|
|
|
- faultLabel = f(data)
|
|
|
|
- if faultLabel != "" {
|
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
|
- break
|
|
|
|
- }
|
|
|
|
- }
|
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
|
- }
|
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
|
- },
|
|
|
|
- })
|
|
|
|
- }
|
|
|
|
- // 41
|
|
|
|
- if topic == masterConfig.TopicOfDestinationPose && len(masterConfig.RuleOfDestinationPose) > 0 {
|
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
|
- Node: commonConfig.RosNode,
|
|
|
|
- Topic: topic,
|
|
|
|
- Callback: func(data *visualization_msgs.MarkerArray) {
|
|
|
|
- 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.RuleOfDestinationPose {
|
|
|
|
- faultLabel = f(data)
|
|
|
|
- if faultLabel != "" {
|
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
|
- break
|
|
|
|
- }
|
|
|
|
- }
|
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
|
- }
|
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
|
- },
|
|
|
|
- })
|
|
|
|
- }
|
|
|
|
- // 42
|
|
|
|
- if topic == masterConfig.TopicOfMapDisplay && len(masterConfig.RuleOfMapDisplay) > 0 {
|
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
|
- Node: commonConfig.RosNode,
|
|
|
|
- Topic: topic,
|
|
|
|
- Callback: func(data *nav_msgs.Path) {
|
|
|
|
- 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.RuleOfMapDisplay {
|
|
|
|
- faultLabel = f(data)
|
|
|
|
- if faultLabel != "" {
|
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
|
- break
|
|
|
|
- }
|
|
|
|
- }
|
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
|
- }
|
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
|
- },
|
|
|
|
- })
|
|
|
|
- }
|
|
|
|
- // 43
|
|
|
|
- if topic == masterConfig.TopicOfRoutingRviz && len(masterConfig.RuleOfRoutingRviz) > 0 {
|
|
|
|
- subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
|
- Node: commonConfig.RosNode,
|
|
|
|
- Topic: topic,
|
|
|
|
- Callback: func(data *nav_msgs.Path) {
|
|
|
|
- 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.RuleOfRoutingRviz {
|
|
|
|
- faultLabel = f(data)
|
|
|
|
- if faultLabel != "" {
|
|
|
|
- saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
|
- subscribersTimes[i] = time.Now()
|
|
|
|
- break
|
|
|
|
- }
|
|
|
|
- }
|
|
|
|
- subscribersMutexes[i].Unlock()
|
|
|
|
- }
|
|
|
|
- subscribersTimeMutexes[i].Unlock()
|
|
|
|
- },
|
|
|
|
- })
|
|
|
|
- }
|
|
|
|
- // 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 {
|
|
if topic == masterConfig.TopicOfDataRead && len(masterConfig.RuleOfDataRead) > 0 {
|
|
subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
Node: commonConfig.RosNode,
|
|
Node: commonConfig.RosNode,
|