Handle readyToContinue correctly

Author: g3force

internal/app/engine/common.go

@@ -39,15 +39,6 @@ func (e *Engine) robotsInsideRadius(robots []*Robot, pos *geom.Vector2, radius f
 	return false
 }
 
-func (e *Engine) timeSinceLastChange() time.Duration {
-	if e.stateStore.LatestEntry() != nil {
-		lastChangeTs := goTime(e.stateStore.LatestEntry().Timestamp)
-		now := e.timeProvider()
-		return now.Sub(lastChangeTs)
-	}
-	return 0
-}
-
 func goDur(duration *duration.Duration) time.Duration {
 	goDur, err := ptypes.Duration(duration)
 	if err != nil {

internal/app/engine/consume_tracker.go

@@ -18,17 +18,10 @@ func (e *Engine) ProcessTrackerFrame(wrapperFrame *tracker.TrackerWrapperPacket)
 		Robots:     convertRobots(wrapperFrame.TrackedFrame.Robots),
 	}
 
-	readyToContinue := e.readyToContinue()
-
 	e.gcState.TrackerState[*wrapperFrame.Uuid] = &state
 
 	// for now, all tracker sources update the GC state
 	e.gcState.TrackerStateGc = &state
-
-	if e.gcState.ReadyToContinue == nil {
-		e.gcState.ReadyToContinue = new(bool)
-	}
-	*e.gcState.ReadyToContinue = readyToContinue
 }
 
 func convertRobots(robots []*tracker.TrackedRobot) (rs []*Robot) {
@@ -51,13 +44,3 @@ func convertBalls(balls []*tracker.TrackedBall) *Ball {
 	}
 	return &ball
 }
-
-func (e *Engine) readyToContinue() bool {
-	radius := e.gameConfig.DistanceToBallInStop + robotRadius + distanceThreshold
-	if e.gcState.TrackerStateGc.Ball == nil ||
-		!e.ballSteady() ||
-		e.robotsInsideRadius(e.gcState.TrackerStateGc.Robots, e.gcState.TrackerStateGc.Ball.Pos.ToVector2(), radius) {
-		return false
-	}
-	return false
-}

internal/app/engine/process_prepare.go renamed to internal/app/engine/process_continue.go

@@ -10,14 +10,23 @@ import (
 const minPreparationTime = time.Second * 2
 const distanceToBallDuringPenalty = 1.0
 
-func (e *Engine) processPrepare() {
-	if !e.currentState.Command.IsPrepare() ||
+func (e *Engine) processContinue() {
+	if !(e.currentState.Command.IsPrepare() || *e.currentState.Command.Type == state.Command_STOP) ||
 		e.gcState.TrackerStateGc.Ball == nil ||
-		e.gcState.ReadyToContinue == nil ||
-		!*e.gcState.ReadyToContinue ||
-		e.currentState.NextCommand == nil ||
-		!e.currentState.GetAutoContinue() ||
-		e.timeSinceLastChange() < minPreparationTime {
+		e.currentState.NextCommand == nil {
+		e.gcState.ReadyToContinue = nil
+		return
+	}
+
+	readyToContinue := false
+
+	defer func() {
+		// set at the end of this function when the flag has its final value
+		e.gcState.ReadyToContinue = &readyToContinue
+	}()
+
+	if e.timeSinceLastChange() < minPreparationTime {
+		// Too early
 		return
 	}
 
@@ -75,12 +84,21 @@ func (e *Engine) processPrepare() {
 		}
 	}
 
-	e.Enqueue(&statemachine.Change{
-		Origin: &changeOriginEngine,
-		Change: &statemachine.Change_Continue{
-			Continue: &statemachine.Continue{},
-		},
-	})
+	if *e.currentState.Command.Type == state.Command_STOP &&
+		!e.readyToContinueFromStop() {
+		return
+	}
+
+	readyToContinue = true
+
+	if e.currentState.GetAutoContinue() {
+		e.Enqueue(&statemachine.Change{
+			Origin: &changeOriginEngine,
+			Change: &statemachine.Change_Continue{
+				Continue: &statemachine.Continue{},
+			},
+		})
+	}
 }
 
 func (e *Engine) penaltyKeeperId() *state.RobotId {
@@ -109,3 +127,22 @@ func (e *Engine) posInsideGoal(pos *geom.Vector2) bool {
 	goalArea := geom.NewRectangleFromCenter(goalCenter, robotRadius*2, e.getGeometry().GoalWidth)
 	return goalArea.IsPointInside(pos)
 }
+
+func (e *Engine) readyToContinueFromStop() bool {
+	radius := e.gameConfig.DistanceToBallInStop + robotRadius + distanceThreshold
+	if e.gcState.TrackerStateGc.Ball == nil ||
+		!e.ballSteady() ||
+		e.robotsInsideRadius(e.gcState.TrackerStateGc.Robots, e.gcState.TrackerStateGc.Ball.Pos.ToVector2(), radius) {
+		return false
+	}
+	return true
+}
+
+func (e *Engine) timeSinceLastChange() time.Duration {
+	if e.stateStore.LatestEntry() != nil {
+		lastChangeTs := goTime(e.stateStore.LatestEntry().Timestamp)
+		now := e.timeProvider()
+		return now.Sub(lastChangeTs)
+	}
+	return 0
+}

internal/app/engine/process_runningtostop.go

@@ -2,6 +2,7 @@ package engine
 
 import (
 	"github.com/RoboCup-SSL/ssl-game-controller/internal/app/geom"
+	"github.com/RoboCup-SSL/ssl-game-controller/internal/app/state"
 	"github.com/RoboCup-SSL/ssl-game-controller/internal/app/statemachine"
 	"log"
 )
@@ -21,12 +22,23 @@ func (e *Engine) processRunningToStop() {
 		})
 	} else {
 		log.Printf("Ball is already placed, no need for ball placement")
-		e.Enqueue(&statemachine.Change{
-			Origin: &changeOriginEngine,
-			Change: &statemachine.Change_Continue{
-				Continue: &statemachine.Continue{},
-			},
-		})
+		if e.currentState.GetAutoContinue() {
+			e.Enqueue(&statemachine.Change{
+				Origin: &changeOriginEngine,
+				Change: &statemachine.Change_Continue{
+					Continue: &statemachine.Continue{},
+				},
+			})
+		} else {
+			e.Enqueue(&statemachine.Change{
+				Origin: &changeOriginEngine,
+				Change: &statemachine.Change_NewCommand{
+					NewCommand: &statemachine.NewCommand{
+						Command: state.NewCommandNeutral(state.Command_STOP),
+					},
+				},
+			})
+		}
 	}
 }
 

internal/app/engine/process_tick.go

@@ -40,7 +40,7 @@ func (e *Engine) processTick() {
 
 	e.noProgressDetector.process()
 	e.ballPlacementCoordinator.process()
-	e.processPrepare()
+	e.processContinue()
 	e.processBotNumber()
 
 	stateCopy := e.currentState.Clone()

internal/app/statemachine/change_gameevent.go

@@ -132,7 +132,7 @@ func (s *StateMachine) processChangeAddGameEvent(newState *state.State, change *
 		byTeam.Known() &&
 		*newState.TeamInfo(byTeam).BallPlacementFailures > 0 {
 		*newState.TeamInfo(byTeam).BallPlacementFailures--
-		if byTeam == *newState.NextCommand.ForTeam {
+		if byTeam == *newState.NextCommand.ForTeam && newState.GetAutoContinue() {
 			log.Printf("Placement succeeded by team %v, which is also in favor. Can continue.", byTeam)
 			changes = append(changes, &Change{
 				Change: &Change_Continue{