Add continuation issues to UI

Author: g3force

internal/app/engine/common.go

@@ -26,15 +26,15 @@ func createGameEventChange(eventType state.GameEvent_Type, event *state.GameEven
 	}
 }
 
-func (e *Engine) robotsInsideRadius(robots []*Robot, pos *geom.Vector2, radius float64) bool {
+func (e *Engine) findRobotInsideRadius(robots []*Robot, pos *geom.Vector2, radius float64) *Robot {
 	for _, robot := range robots {
 		distance := robot.Pos.DistanceTo(pos)
 		if distance < radius {
-			return true
+			return robot
 		}
 	}
 
-	return false
+	return nil
 }
 
 func goDur(duration *durationpb.Duration) time.Duration {

internal/app/engine/process_continue.go

@@ -1,6 +1,7 @@
 package engine
 
 import (
+	"fmt"
 	"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"
@@ -15,98 +16,133 @@ func (e *Engine) processContinue() {
 		*e.currentState.Command.Type == state.Command_STOP ||
 		(e.gameConfig.ContinueFromHalt && *e.currentState.Command.Type == state.Command_HALT)) ||
 		e.gcState.TrackerStateGc.Ball == nil {
+		// Reset ready to continue
 		e.gcState.ReadyToContinue = nil
+		e.gcState.ContinuationIssues = []string{}
 		return
 	}
 
-	readyToContinue := false
+	e.gcState.ContinuationIssues = e.findIssueForContinuation()
+	readyToContinue := len(e.gcState.ContinuationIssues) == 0
+	e.gcState.ReadyToContinue = &readyToContinue
 
-	defer func() {
-		// set at the end of this function when the flag has its final value
-		e.gcState.ReadyToContinue = &readyToContinue
-	}()
+	if readyToContinue && e.currentState.GetAutoContinue() {
+		e.Enqueue(&statemachine.Change{
+			Origin: &changeOriginEngine,
+			Change: &statemachine.Change_Continue{
+				Continue: &statemachine.Continue{},
+			},
+		})
+	}
+}
 
-	if e.timeSinceLastChange() < minPreparationTime {
-		// Too early
-		return
+func (e *Engine) findIssueForContinuation() (issues []string) {
+	preparationTimeLeft := e.timeSinceLastChange() - minPreparationTime
+	if preparationTimeLeft < 0 {
+		issues = append(issues, fmt.Sprintf("%.1fs left for preparation", -preparationTimeLeft.Seconds()))
 	}
 
 	if *e.currentState.Command.Type == state.Command_KICKOFF {
-		// ball in center circle
-		if e.gcState.TrackerStateGc.Ball.Pos.ToVector2().Length() > e.getGeometry().CenterCircleRadius {
-			return
-		}
-
-		// bots on wrong side
-		for _, robot := range e.gcState.TrackerStateGc.Robots {
-			if *e.currentState.TeamState[robot.Id.Team.String()].OnPositiveHalf {
-				if *robot.Pos.X < robotRadius {
-					return
-				}
-			} else if *robot.Pos.X > -robotRadius {
-				return
-			}
-		}
+		issues = append(issues, e.readyToContinueKickoff()...)
 	}
 
 	if *e.currentState.Command.Type == state.Command_PENALTY {
-		keeperId := e.penaltyKeeperId()
-		if keeperId == nil {
-			return
-		}
-		keeperPos := e.robotPos(keeperId)
-		if keeperPos == nil || !e.posInsideGoal(keeperPos) {
-			return
-		}
+		issues = append(issues, e.readyToContinuePenalty()...)
+	}
 
-		keeperTeamInfo := e.currentState.TeamState[keeperId.Team.String()]
-		ballPos := e.gcState.TrackerStateGc.Ball.Pos
+	if *e.currentState.Command.Type == state.Command_STOP {
+		issues = append(issues, e.readyToContinueFromStop()...)
+	}
 
-		numAttackersInFrontOfBall := 0
-		for _, robot := range e.gcState.TrackerStateGc.Robots {
-			if *robot.Id.Id == *keeperId.Id && *robot.Id.Team == *keeperId.Team {
-				// its the keeper
-				continue
-			}
-			if *keeperTeamInfo.OnPositiveHalf &&
-				*robot.Pos.X < *ballPos.X-distanceToBallDuringPenalty {
-				continue
-			} else if !*keeperTeamInfo.OnPositiveHalf &&
-				*robot.Pos.X > *ballPos.X+distanceToBallDuringPenalty {
-				continue
-			}
-			if *robot.Id.Team == *keeperId.Team {
-				return
-			} else if numAttackersInFrontOfBall >= 1 {
-				return
-			} else {
-				numAttackersInFrontOfBall++
+	return
+}
+
+func (e *Engine) readyToContinueKickoff() (issues []string) {
+	ballToCenterCircleDist := e.gcState.TrackerStateGc.Ball.Pos.ToVector2().Length() - e.getGeometry().CenterCircleRadius
+	if ballToCenterCircleDist > 0 {
+		issues = append(issues, fmt.Sprintf("Ball is %.1f m away from center circle", ballToCenterCircleDist))
+	}
+
+	for _, robot := range e.gcState.TrackerStateGc.Robots {
+		if *e.currentState.TeamState[robot.Id.Team.String()].OnPositiveHalf {
+			if *robot.Pos.X < robotRadius {
+				issues = append(issues, fmt.Sprintf("Robot %s is not in its own half", robot.Id.PrettyString()))
 			}
+		} else if *robot.Pos.X > -robotRadius {
+			issues = append(issues, fmt.Sprintf("Robot %s is not in its own half", robot.Id.PrettyString()))
 		}
 	}
+	return issues
+}
 
-	if *e.currentState.Command.Type == state.Command_STOP &&
-		(e.currentState.NextCommand == nil ||
-			!e.readyToContinueFromStop()) {
+func (e *Engine) readyToContinuePenalty() (issues []string) {
+	keeperId := e.penaltyKeeperId()
+	if keeperId == nil {
+		issues = append(issues, "There is no keeper")
 		return
 	}
-
-	if *e.currentState.Command.Type == state.Command_STOP &&
-		(e.currentState.NextCommand == nil ||
-			!e.readyToContinueFromHalt()) {
-		return
+	keeperPos := e.robotPos(keeperId)
+	if keeperPos == nil {
+		issues = append(issues, "Keeper position is unknown")
+	} else if !e.posInsideGoal(keeperPos) {
+		issues = append(issues, "Keeper is not inside goal")
 	}
 
-	readyToContinue = true
+	keeperTeamInfo := e.currentState.TeamState[keeperId.Team.String()]
+	ballPos := e.gcState.TrackerStateGc.Ball.Pos
 
-	if e.currentState.GetAutoContinue() {
-		e.Enqueue(&statemachine.Change{
-			Origin: &changeOriginEngine,
-			Change: &statemachine.Change_Continue{
-				Continue: &statemachine.Continue{},
-			},
-		})
+	numAttackersInFrontOfBall := 0
+	for _, robot := range e.gcState.TrackerStateGc.Robots {
+		if *robot.Id.Id == *keeperId.Id && *robot.Id.Team == *keeperId.Team {
+			// it's the keeper
+			continue
+		}
+		if *keeperTeamInfo.OnPositiveHalf &&
+			*robot.Pos.X < *ballPos.X-distanceToBallDuringPenalty {
+			continue
+		} else if !*keeperTeamInfo.OnPositiveHalf &&
+			*robot.Pos.X > *ballPos.X+distanceToBallDuringPenalty {
+			continue
+		}
+		if *robot.Id.Team == *keeperId.Team {
+			issues = append(issues, fmt.Sprintf(
+				"Robot %s does not keep required distance to ball", robot.Id.PrettyString()))
+		} else if numAttackersInFrontOfBall >= 1 {
+			issues = append(issues, fmt.Sprintf(
+				"Robot %s does not keep required distance to ball", robot.Id.PrettyString()))
+		} else {
+			numAttackersInFrontOfBall++
+		}
 	}
+	return
+}
+
+func (e *Engine) readyToContinueFromStop() (issues []string) {
+	if e.tooManyRobots(state.Team_YELLOW) {
+		issues = append(issues, "Yellow team has too many robots")
+	}
+	if e.tooManyRobots(state.Team_BLUE) {
+		issues = append(issues, "Blue team has too many robots")
+	}
+	radius := e.gameConfig.DistanceToBallInStop + robotRadius - distanceThreshold
+	robotNearBall := e.findRobotInsideRadius(e.gcState.TrackerStateGc.Robots, e.gcState.TrackerStateGc.Ball.Pos.ToVector2(), radius)
+	if robotNearBall != nil {
+		issues = append(issues, fmt.Sprintf("Robot %s is too close to ball", robotNearBall.Id.PrettyString()))
+	}
+	if e.currentState.PlacementPos != nil {
+		ballToPlacementPosDist := e.currentState.PlacementPos.DistanceTo(e.gcState.TrackerStateGc.Ball.Pos.ToVector2())
+		if ballToPlacementPosDist > e.gameConfig.BallPlacementTolerance {
+			issues = append(issues, fmt.Sprintf("Ball is %.1fm (>%.1fm) away from placement pos",
+				ballToPlacementPosDist, e.gameConfig.BallPlacementTolerance))
+		}
+	}
+	if !e.ballSteady() {
+		issues = append(issues, "Ball position is not steady")
+	}
+	if e.currentState.NextCommand == nil {
+		issues = append(issues, "No next command")
+	}
+	return
 }
 
 func (e *Engine) penaltyKeeperId() *state.RobotId {
@@ -136,28 +172,6 @@ func (e *Engine) posInsideGoal(pos *geom.Vector2) bool {
 	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) readyToContinueFromHalt() bool {
-	if e.gcState.TrackerStateGc.Ball == nil ||
-		e.currentState.PlacementPos == nil ||
-		!e.ballSteady() ||
-		e.currentState.PlacementPos.DistanceTo(e.gcState.TrackerStateGc.Ball.Pos.ToVector2()) > e.gameConfig.BallPlacementTolerance ||
-		e.tooManyRobots(state.Team_BLUE) ||
-		e.tooManyRobots(state.Team_YELLOW) {
-		return false
-	}
-	return true
-}
-
 func (e *Engine) tooManyRobots(team state.Team) bool {
 	maxAllowed := *e.currentState.TeamState[team.String()].MaxAllowedBots
 	current := numRobotsOfTeam(e.gcState.TrackerStateGc.Robots, team)