Implement a common reset mechanism when simulator restart was detected

Author: g3force

internal/simctl/geometry.go

@@ -23,7 +23,18 @@ type GeometrySetter struct {
 	timeLastSent *time.Time
 }
 
-func (r *GeometrySetter) LoadGeometry() {
+func NewGeometrySetter(c *SimulationController) (r *GeometrySetter) {
+	r = new(GeometrySetter)
+	r.c = c
+	r.loadGeometry()
+	return r
+}
+
+func (r *GeometrySetter) Reset() {
+	r.timeLastSent = nil
+}
+
+func (r *GeometrySetter) loadGeometry() {
 	r.geometryDivA = new(vision.SSL_GeometryData)
 	if err := proto.UnmarshalText(geometryConfigDivA, r.geometryDivA); err != nil {
 		log.Println("Could not unmarshal geometry file: ", err)

internal/simctl/maintain_robot_count.go

@@ -16,10 +16,15 @@ type RobotCountMaintainer struct {
 
 	lastTimeSendCommand     time.Time
 	robotCountMismatchSince map[referee.Team]time.Time
-	lastUpdate              time.Time
 }
 
-func (r *RobotCountMaintainer) init() {
+func NewRobotCountMaintainer(c *SimulationController) (r *RobotCountMaintainer) {
+	r = new(RobotCountMaintainer)
+	r.c = c
+	return r
+}
+
+func (r *RobotCountMaintainer) Reset() {
 	r.lastTimeSendCommand = time.Now()
 	r.robotCountMismatchSince = map[referee.Team]time.Time{}
 	r.robotCountMismatchSince[referee.Team_BLUE] = time.Time{}
@@ -28,13 +33,6 @@ func (r *RobotCountMaintainer) init() {
 
 func (r *RobotCountMaintainer) handleRobotCount() {
 
-	preLastUpdate := r.lastUpdate
-	r.lastUpdate = time.Now()
-	if r.lastUpdate.Sub(preLastUpdate) > time.Second {
-		// First update since a second, probably a simulator restart
-		r.init()
-	}
-
 	if time.Now().Sub(r.lastTimeSendCommand) < 500*time.Millisecond {
 		// Placed ball just recently
 		return

internal/simctl/replace_ball.go

@@ -15,6 +15,17 @@ type BallReplacer struct {
 	haltTime           *time.Time
 }
 
+func NewBallReplacer(c *SimulationController) (r *BallReplacer) {
+	r = new(BallReplacer)
+	r.c = c
+	return r
+}
+
+func (r *BallReplacer) Reset() {
+	r.lastTimePlacedBall = time.Time{}
+	r.haltTime = nil
+}
+
 func (r *BallReplacer) handleReplaceBall() {
 	if *r.c.lastRefereeMsg.Command != referee.Referee_HALT {
 		// Only during HALT

internal/simctl/robot_specs.go

@@ -46,14 +46,24 @@ type RobotSpecSetter struct {
 	appliedTeams   map[referee.Team]string
 }
 
-func (r *RobotSpecSetter) LoadRobotSpecs(configFile string) {
+func NewRobotSpecSetter(c *SimulationController, configFile string) (r *RobotSpecSetter) {
+	r = new(RobotSpecSetter)
+	r.c = c
+	r.loadRobotSpecs(configFile)
+	return r
+}
+
+func (r *RobotSpecSetter) Reset() {
+	r.appliedTeams = map[referee.Team]string{}
+}
+
+func (r *RobotSpecSetter) loadRobotSpecs(configFile string) {
 	data, err := ioutil.ReadFile(configFile)
 	if err != nil {
 		log.Println("Could not read robot spec file: ", err)
 	} else if err := yaml.Unmarshal(data, &r.teamRobotSpecs); err != nil {
 		log.Println("Could not unmarshal robot spec file: ", err)
 	}
-	r.appliedTeams = map[referee.Team]string{}
 }
 
 func (r *RobotSpecSetter) handleRobotSpecs() {

internal/simctl/simctl.go

@@ -7,25 +7,30 @@ import (
 	"github.com/RoboCup-SSL/ssl-simulation-controller/internal/vision"
 	"github.com/golang/protobuf/proto"
 	"log"
+	"math"
 	"net"
+	"sync"
 )
 
 type SimulationController struct {
 	visionServer     *sslnet.MulticastServer
 	refereeServer    *sslnet.MulticastServer
 	trackerServer    *sslnet.MulticastServer
 	simControlClient *sslnet.UdpClient
+	mutex            sync.Mutex
 
-	simControlPort string
+	simControlPort     string
+	simulatorRestarted bool
 
-	lastTrackedFrame *tracker.TrackerWrapperPacket
-	lastRefereeMsg   *referee.Referee
-	fieldSize        *vision.SSL_GeometryFieldSize
+	lastTrackedFrame   *tracker.TrackerWrapperPacket
+	lastRefereeMsg     *referee.Referee
+	fieldSize          *vision.SSL_GeometryFieldSize
+	lastVisionFrameIds map[uint32]uint32
 
-	ballReplacer         BallReplacer
-	robotCountMaintainer RobotCountMaintainer
-	robotSpecsSetter     RobotSpecSetter
-	geometrySetter       GeometrySetter
+	ballReplacer         *BallReplacer
+	robotCountMaintainer *RobotCountMaintainer
+	robotSpecsSetter     *RobotSpecSetter
+	geometrySetter       *GeometrySetter
 }
 
 func NewSimulationController(visionAddress, refereeAddress, trackerAddress, simControlPort, robotSpecConfig string) (c *SimulationController) {
@@ -34,12 +39,18 @@ func NewSimulationController(visionAddress, refereeAddress, trackerAddress, simC
 	c.refereeServer = sslnet.NewMulticastServer(refereeAddress, c.onNewRefereeData)
 	c.trackerServer = sslnet.NewMulticastServer(trackerAddress, c.onNewTrackerData)
 	c.simControlPort = simControlPort
+	c.simulatorRestarted = true
+	c.lastVisionFrameIds = map[uint32]uint32{}
+
+	c.ballReplacer = NewBallReplacer(c)
+	c.robotCountMaintainer = NewRobotCountMaintainer(c)
+	c.robotSpecsSetter = NewRobotSpecSetter(c, robotSpecConfig)
+	c.geometrySetter = NewGeometrySetter(c)
+
 	c.ballReplacer.c = c
 	c.robotCountMaintainer.c = c
 	c.robotSpecsSetter.c = c
-	c.robotSpecsSetter.LoadRobotSpecs(robotSpecConfig)
 	c.geometrySetter.c = c
-	c.geometrySetter.LoadGeometry()
 	return
 }
 
@@ -51,7 +62,24 @@ func (c *SimulationController) onNewVisionData(data []byte, remoteAddr *net.UDPA
 	}
 
 	if wrapper.Geometry != nil && wrapper.Geometry.Field != nil {
+		c.mutex.Lock()
 		c.fieldSize = wrapper.Geometry.Field
+		c.mutex.Unlock()
+	}
+
+	if wrapper.Detection != nil {
+		c.mutex.Lock()
+		frameId := *wrapper.Detection.FrameNumber
+		lastFrameId, lastFrameIdPresent := c.lastVisionFrameIds[*wrapper.Detection.CameraId]
+		if lastFrameIdPresent && math.Abs(float64(frameId-lastFrameId)) > 100 {
+			// large frame id change: Simulator probably restarted
+			c.simulatorRestarted = true
+			c.lastVisionFrameIds = map[uint32]uint32{}
+			log.Printf("Simulator restart detected due to high frame id change (%d -> %d)",
+				lastFrameId, frameId)
+		}
+		c.lastVisionFrameIds[*wrapper.Detection.CameraId] = frameId
+		c.mutex.Unlock()
 	}
 
 	if c.simControlClient == nil {
@@ -68,7 +96,9 @@ func (c *SimulationController) onNewRefereeData(data []byte, _ *net.UDPAddr) {
 		log.Println("Could not unmarshal referee packet", err)
 		return
 	}
+	c.mutex.Lock()
 	c.lastRefereeMsg = refereeMsg
+	c.mutex.Unlock()
 }
 
 func (c *SimulationController) onNewTrackerData(data []byte, _ *net.UDPAddr) {
@@ -81,8 +111,10 @@ func (c *SimulationController) onNewTrackerData(data []byte, _ *net.UDPAddr) {
 		*c.lastTrackedFrame.Uuid == *frame.Uuid || // frame from same origin
 		// new frame is significantly newer than last frame
 		(*frame.TrackedFrame.Timestamp-*c.lastTrackedFrame.TrackedFrame.Timestamp) > 5 {
+		c.mutex.Lock()
 		c.lastTrackedFrame = frame
 		c.handle()
+		c.mutex.Unlock()
 	}
 }
 
@@ -105,6 +137,14 @@ func (c *SimulationController) handle() {
 		return
 	}
 
+	if c.simulatorRestarted {
+		c.ballReplacer.Reset()
+		c.robotCountMaintainer.Reset()
+		c.robotSpecsSetter.Reset()
+		c.geometrySetter.Reset()
+		c.simulatorRestarted = false
+	}
+
 	c.ballReplacer.handleReplaceBall()
 	c.robotCountMaintainer.handleRobotCount()
 	c.robotSpecsSetter.handleRobotSpecs()