Add a config flag to continue from halt (ie after goal) automatically

Author: g3force

config/ssl-game-controller.yaml

@@ -45,6 +45,7 @@ game:
   ball-placement-min-robot-distance: 0.05
   distance-to-ball-in-stop: 0.5
   auto-approve-goals: false
+  continue-from-halt: false
   challenge-flags: 3
   emergency-stop-grace-period: 10s
   normal:

internal/app/config/config.go

@@ -59,6 +59,7 @@ type Game struct {
 	BallPlacementMinRobotDistance         float64                    `yaml:"ball-placement-min-robot-distance"`
 	DistanceToBallInStop                  float64                    `yaml:"distance-to-ball-in-stop"`
 	AutoApproveGoals                      bool                       `yaml:"auto-approve-goals"`
+	ContinueFromHalt                      bool                       `yaml:"continue-from-halt"`
 	ChallengeFlags                        int32                      `yaml:"challenge-flags"`
 	EmergencyStopGracePeriod              time.Duration              `yaml:"emergency-stop-grace-period"`
 }

internal/app/config/testdata/config.yaml

@@ -45,6 +45,7 @@ game:
   ball-placement-min-robot-distance: 0.05
   distance-to-ball-in-stop: 0.5
   auto-approve-goals: false
+  continue-from-halt: false
   challenge-flags: 3
   emergency-stop-grace-period: 10s
   normal:

internal/app/engine/process_continue.go

@@ -11,7 +11,9 @@ const minPreparationTime = time.Second * 2
 const distanceToBallDuringPenalty = 1.0
 
 func (e *Engine) processContinue() {
-	if !(e.currentState.Command.IsPrepare() || *e.currentState.Command.Type == state.Command_STOP) ||
+	if !(e.currentState.Command.IsPrepare() ||
+		*e.currentState.Command.Type == state.Command_STOP ||
+		(e.gameConfig.ContinueFromHalt && *e.currentState.Command.Type == state.Command_HALT)) ||
 		e.gcState.TrackerStateGc.Ball == nil {
 		e.gcState.ReadyToContinue = nil
 		return
@@ -89,6 +91,12 @@ func (e *Engine) processContinue() {
 		return
 	}
 
+	if *e.currentState.Command.Type == state.Command_STOP &&
+		(e.currentState.NextCommand == nil ||
+			!e.readyToContinueFromHalt()) {
+		return
+	}
+
 	readyToContinue = true
 
 	if e.currentState.GetAutoContinue() {
@@ -138,6 +146,24 @@ func (e *Engine) readyToContinueFromStop() bool {
 	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)
+	return current > maxAllowed
+}
+
 func (e *Engine) timeSinceLastChange() time.Duration {
 	if e.stateStore.LatestEntry() != nil {
 		lastChangeTs := goTime(e.stateStore.LatestEntry().Timestamp)