Feature: Only consume one tracker source at a time

Author: g3force

internal/app/engine/consume_tracker.go

@@ -2,6 +2,8 @@ package engine
 
 import (
 	"github.com/RoboCup-SSL/ssl-game-controller/internal/app/tracker"
+	"log"
+	"time"
 )
 
 func (e *Engine) ProcessTrackerFrame(wrapperFrame *tracker.TrackerWrapperPacket) {
@@ -12,16 +14,48 @@ func (e *Engine) ProcessTrackerFrame(wrapperFrame *tracker.TrackerWrapperPacket)
 		return
 	}
 
+	now := e.timeProvider()
 	state := GcStateTracker{
 		SourceName: wrapperFrame.SourceName,
+		Uuid:       wrapperFrame.Uuid,
 		Ball:       convertBalls(wrapperFrame.TrackedFrame.Balls),
 		Robots:     convertRobots(wrapperFrame.TrackedFrame.Robots),
 	}
+	e.trackerLastUpdate[*wrapperFrame.Uuid] = now
 
 	e.gcState.TrackerState[*wrapperFrame.Uuid] = &state
 
-	// for now, all tracker sources update the GC state
-	e.gcState.TrackerStateGc = &state
+	for sourceId, state := range e.gcState.TrackerState {
+		if now.Sub(e.trackerLastUpdate[*state.Uuid]) > time.Second {
+			delete(e.gcState.TrackerState, sourceId)
+		}
+	}
+
+	e.updateTrackerSourcePriority(*wrapperFrame.SourceName)
+
+	if e.gcState.TrackerStateGc == nil {
+		e.gcState.TrackerStateGc = &state
+		log.Printf("Initial tracker source is %v (%v)", *e.gcState.TrackerStateGc.Uuid, *e.gcState.TrackerStateGc.SourceName)
+	} else if *e.gcState.TrackerStateGc.Uuid == *wrapperFrame.Uuid {
+		e.gcState.TrackerStateGc = &state
+	} else if now.Sub(e.trackerLastUpdate[*e.gcState.TrackerStateGc.Uuid]) > time.Second {
+		e.gcState.TrackerStateGc = e.findNewTrackerSourceState()
+		if e.gcState.TrackerStateGc == nil {
+			e.gcState.TrackerStateGc = &state
+		}
+		log.Printf("Switched tracker source to %v (%v)", *e.gcState.TrackerStateGc.Uuid, *e.gcState.TrackerStateGc.SourceName)
+	}
+}
+
+func (e *Engine) findNewTrackerSourceState() *GcStateTracker {
+	for _, sourceName := range e.config.TrackerSourcePriority {
+		for _, state := range e.gcState.TrackerState {
+			if *state.SourceName == sourceName {
+				return state
+			}
+		}
+	}
+	return nil
 }
 
 func convertRobots(robots []*tracker.TrackedRobot) (rs []*Robot) {
@@ -44,3 +78,12 @@ func convertBalls(balls []*tracker.TrackedBall) *Ball {
 	}
 	return &ball
 }
+
+func (e *Engine) updateTrackerSourcePriority(s string) {
+	for _, sourceName := range e.config.TrackerSourcePriority {
+		if sourceName == s {
+			return
+		}
+	}
+	e.config.TrackerSourcePriority = append(e.config.TrackerSourcePriority, s)
+}

internal/app/engine/engine.go

@@ -29,6 +29,7 @@ type Engine struct {
 	timeProvider             timer.TimeProvider
 	lastTimeUpdate           time.Time
 	gcState                  *GcState
+	trackerLastUpdate        map[string]time.Time
 	mutex                    sync.Mutex
 	noProgressDetector       NoProgressDetector
 	ballPlacementCoordinator BallPlacementCoordinator
@@ -53,7 +54,7 @@ func NewEngine(gameConfig config.Game, engineConfig config.Engine) (e *Engine) {
 	}
 	e.gcState.AutoRefState = map[string]*GcStateAutoRef{}
 	e.gcState.TrackerState = map[string]*GcStateTracker{}
-	e.gcState.TrackerStateGc = new(GcStateTracker)
+	e.trackerLastUpdate = map[string]time.Time{}
 	e.noProgressDetector = NoProgressDetector{gcEngine: e}
 	e.ballPlacementCoordinator = BallPlacementCoordinator{gcEngine: e}
 	e.tickChanProvider = func() <-chan time.Time {

internal/app/engine/ssl_gc_engine.pb.go

@@ -30,6 +30,9 @@ message GcStateTracker {
     // Name of the source
     optional string source_name = 1;
 
+    // UUID of the source
+    optional string uuid = 4;
+
     // Current ball
     optional Ball ball = 2;
 

internal/app/engine/ssl_gc_engine_config.pb.go

@@ -3,8 +3,9 @@ syntax = "proto2";
 option go_package = "github.com/RoboCup-SSL/ssl-game-controller/internal/app/engine";
 
 message Config {
-    map<string, Behavior> game_event_behavior = 3;
-    map<string, AutoRefConfig> auto_ref_configs = 4;
+    map<string, Behavior> game_event_behavior = 1;
+    map<string, AutoRefConfig> auto_ref_configs = 2;
+    repeated string tracker_source_priority = 3;
 
     enum Behavior {
         BEHAVIOR_UNKNOWN = 0;

proto/ssl_gc_engine.proto

@@ -13,17 +13,21 @@
             <b-tab v-for="autoRef in autoRefs" :title="autoRef" :key="autoRef">
                 <AutoRefConfig :auto-ref-name="autoRef"/>
             </b-tab>
+            <b-tab title="Tracker sources">
+                <TrackerConfig/>
+            </b-tab>
         </b-tabs>
     </div>
 </template>
 
 <script>
     import EventBehavior from "./EventBehavior";
     import AutoRefConfig from "./AutoRefConfig";
+    import TrackerConfig from "./TrackerConfig";
 
     export default {
         name: "AutonomousSettings",
-        components: {AutoRefConfig, EventBehavior},
+        components: {TrackerConfig, AutoRefConfig, EventBehavior},
         computed: {
             gcState() {
                 return this.$store.state.gcState