Implement ready to continue for prepare and placement

Author: g3force

config/ssl-game-controller.yaml

@@ -33,6 +33,8 @@ game:
   ball-placement-required-distance: 1.0
   ball-placement-tolerance: 0.15
   ball-placement-min-distance-to-defense-area: 0.7
+  ball-placement-min-robot-distance: 0.05
+  distance-to-ball-in-stop: 0.5
   normal:
     half-duration: 5m
     half-time-duration: 5m

internal/app/config/config.go

@@ -54,6 +54,8 @@ type Game struct {
 	BallPlacementRequiredDistance         float64                    `yaml:"ball-placement-required-distance"`
 	BallPlacementMinDistanceToDefenseArea float64                    `yaml:"ball-placement-min-distance-to-defense-area"`
 	BallPlacementTolerance                float64                    `yaml:"ball-placement-tolerance"`
+	BallPlacementMinRobotDistance         float64                    `yaml:"ball-placement-min-robot-distance"`
+	DistanceToBallInStop                  float64                    `yaml:"distance-to-ball-in-stop"`
 }
 
 // Network holds configs for network communication
@@ -160,6 +162,8 @@ func DefaultControllerConfig() (c Controller) {
 	c.Game.BallPlacementRequiredDistance = 1.0
 	c.Game.BallPlacementMinDistanceToDefenseArea = 0.7
 	c.Game.BallPlacementTolerance = 0.15
+	c.Game.BallPlacementMinRobotDistance = 0.05
+	c.Game.DistanceToBallInStop = 0.5
 
 	c.Game.Normal.HalfDuration = 5 * time.Minute
 	c.Game.Normal.HalfTimeDuration = 5 * time.Minute

internal/app/config/testdata/config.yaml

@@ -33,6 +33,8 @@ game:
   ball-placement-required-distance: 1.0
   ball-placement-tolerance: 0.15
   ball-placement-min-distance-to-defense-area: 0.7
+  ball-placement-min-robot-distance: 0.05
+  distance-to-ball-in-stop: 0.5
   normal:
     half-duration: 5m
     half-time-duration: 5m

internal/app/engine/common.go

@@ -8,6 +8,8 @@ import (
 )
 
 const ballSteadyThreshold = 0.2
+const robotRadius = 0.09
+const distanceThreshold = 0.03
 
 func createGameEventChange(eventType state.GameEvent_Type, event state.GameEvent) *statemachine.Change {
 	event.Type = &eventType
@@ -22,14 +24,14 @@ func createGameEventChange(eventType state.GameEvent_Type, event state.GameEvent
 	}
 }
 
-func (e *Engine) readyToContinue() bool {
-	// robot to ball distance
-	// forTeam: > 0.05m
-	// opponent: > 0.5m
-
-	// ball is stationary
+func (e *Engine) robotsInsideRadius(robots []*Robot, pos *geom.Vector2, radius float64) bool {
+	for _, robot := range robots {
+		distance := robot.Pos.DistanceTo(pos)
+		if distance < radius {
+			return true
+		}
+	}
 
-	// TODO
 	return false
 }
 

internal/app/engine/process_ballplacement.go

@@ -20,30 +20,35 @@ func (c *BallPlacementCoordinator) process() {
 	}
 
 	if c.ballPlacementStartPos == nil || c.ballPlacementStartTime == nil {
-		c.ballPlacementStartPos = e.gcState.TrackerStateGc.BallPos
+		c.ballPlacementStartPos = e.gcState.TrackerStateGc.Ball.Pos
 		c.ballPlacementStartTime = new(time.Time)
 		*c.ballPlacementStartTime = e.timeProvider()
 	}
 
 	remainingDistance := c.remainingPlacementDistance()
-	if e.readyToContinue() &&
+
+	if e.ballSteady() && e.teamInFavorKeepsSufficientDistance() &&
 		float64(remainingDistance) <= e.gameConfig.BallPlacementTolerance {
-		duration := float32(e.timeProvider().Sub(*c.ballPlacementStartTime).Seconds())
-		var distance *float32
-		if c.ballPlacementStartPos != nil && e.gcState.TrackerStateGc.BallPos != nil {
-			distance = new(float32)
-			*distance = float32(c.ballPlacementStartPos.DistanceTo(e.gcState.TrackerStateGc.BallPos))
-		}
-		e.Enqueue(createGameEventChange(state.GameEvent_PLACEMENT_SUCCEEDED, state.GameEvent{
-			Event: &state.GameEvent_PlacementSucceeded_{
-				PlacementSucceeded: &state.GameEvent_PlacementSucceeded{
-					ByTeam:    e.currentState.Command.ForTeam,
-					TimeTaken: &duration,
-					Precision: &remainingDistance,
-					Distance:  distance,
+		// if opponent does not yet keep sufficient distance, that's not the placing teams fold,
+		// so just wait a bit more
+		if e.opponentTeamKeepsSufficientDistance() {
+			duration := float32(e.timeProvider().Sub(*c.ballPlacementStartTime).Seconds())
+			var distance *float32
+			if c.ballPlacementStartPos != nil && e.gcState.TrackerStateGc.Ball.Pos != nil {
+				distance = new(float32)
+				*distance = float32(c.ballPlacementStartPos.DistanceTo(e.gcState.TrackerStateGc.Ball.Pos))
+			}
+			e.Enqueue(createGameEventChange(state.GameEvent_PLACEMENT_SUCCEEDED, state.GameEvent{
+				Event: &state.GameEvent_PlacementSucceeded_{
+					PlacementSucceeded: &state.GameEvent_PlacementSucceeded{
+						ByTeam:    e.currentState.Command.ForTeam,
+						TimeTaken: &duration,
+						Precision: &remainingDistance,
+						Distance:  distance,
+					},
 				},
-			},
-		}))
+			}))
+		}
 	} else if goDur(e.currentState.CurrentActionTimeRemaining) <= 0 {
 		e.Enqueue(createGameEventChange(state.GameEvent_PLACEMENT_FAILED, state.GameEvent{
 			Event: &state.GameEvent_PlacementFailed_{
@@ -57,10 +62,39 @@ func (c *BallPlacementCoordinator) process() {
 }
 
 func (c *BallPlacementCoordinator) remainingPlacementDistance() float32 {
-	if c.gcEngine.currentState.PlacementPos == nil || c.gcEngine.gcState.TrackerStateGc.BallPos == nil {
+	if c.gcEngine.currentState.PlacementPos == nil || c.gcEngine.gcState.TrackerStateGc.Ball.Pos == nil {
 		return -1
 	}
 	placementPos := locationToVector2(c.gcEngine.currentState.PlacementPos)
-	ballPos := c.gcEngine.gcState.TrackerStateGc.BallPos
+	ballPos := c.gcEngine.gcState.TrackerStateGc.Ball.Pos
 	return float32(placementPos.DistanceTo(ballPos))
 }
+
+func (e *Engine) teamInFavorKeepsSufficientDistance() bool {
+	radius := e.gameConfig.BallPlacementMinRobotDistance + robotRadius + distanceThreshold
+	if e.currentState.NextCommand != nil && e.currentState.NextCommand.ForTeam == nil {
+		radius = e.gameConfig.DistanceToBallInStop + robotRadius + distanceThreshold
+	}
+
+	var robots []*Robot
+	for _, robot := range e.gcState.TrackerStateGc.Robots {
+		if *robot.Id.Team == *e.currentState.Command.ForTeam {
+			robots = append(robots, robot)
+		}
+	}
+
+	return !e.robotsInsideRadius(robots, e.gcState.TrackerStateGc.Ball.Pos, radius)
+}
+
+func (e *Engine) opponentTeamKeepsSufficientDistance() bool {
+	radius := e.gameConfig.DistanceToBallInStop + robotRadius + distanceThreshold
+
+	var robots []*Robot
+	for _, robot := range e.gcState.TrackerStateGc.Robots {
+		if *robot.Id.Team != *e.currentState.Command.ForTeam {
+			robots = append(robots, robot)
+		}
+	}
+
+	return !e.robotsInsideRadius(robots, e.gcState.TrackerStateGc.Ball.Pos, radius)
+}

internal/app/engine/process_noprogress.go

@@ -24,13 +24,13 @@ func (d *NoProgressDetector) process() {
 	}
 
 	if d.lastBallPos == nil {
-		d.lastBallPos = d.gcEngine.gcState.TrackerStateGc.BallPos
+		d.lastBallPos = d.gcEngine.gcState.TrackerStateGc.Ball.Pos
 		return
 	}
 
-	if d.lastBallPos.DistanceTo(d.gcEngine.gcState.TrackerStateGc.BallPos) > distanceTolerance {
+	if d.lastBallPos.DistanceTo(d.gcEngine.gcState.TrackerStateGc.Ball.Pos) > distanceTolerance {
 		d.lastTime = nil
-		d.lastBallPos = d.gcEngine.gcState.TrackerStateGc.BallPos
+		d.lastBallPos = d.gcEngine.gcState.TrackerStateGc.Ball.Pos
 		return
 	}
 
@@ -43,10 +43,10 @@ func (d *NoProgressDetector) process() {
 	timeSinceLastProgress := d.gcEngine.timeProvider().Sub(*d.lastTime)
 	if timeSinceLastProgress > d.gcEngine.gameConfig.NoProgressTimeout[d.gcEngine.currentState.Division.Div()] {
 		duration := float32(timeSinceLastProgress.Seconds())
-		location := vector2ToLocation(d.gcEngine.gcState.TrackerStateGc.BallPos)
+		location := vector2ToLocation(d.gcEngine.gcState.TrackerStateGc.Ball.Pos)
 		for _, team := range state.BothTeams() {
 			defenseArea := geom.NewDefenseArea(d.gcEngine.GetGeometry(), *d.gcEngine.currentState.TeamState[team.String()].OnPositiveHalf)
-			if defenseArea.IsPointInside(d.gcEngine.gcState.TrackerStateGc.BallPos) {
+			if defenseArea.IsPointInside(d.gcEngine.gcState.TrackerStateGc.Ball.Pos) {
 				d.gcEngine.Enqueue(createGameEventChange(state.GameEvent_KEEPER_HELD_BALL, state.GameEvent{
 					Event: &state.GameEvent_KeeperHeldBall_{
 						KeeperHeldBall: &state.GameEvent_KeeperHeldBall{

internal/app/engine/process_prepare.go

@@ -32,3 +32,12 @@ func (e *Engine) processPrepare() {
 		},
 	})
 }
+
+func (e *Engine) readyToContinue() bool {
+	radius := e.gameConfig.DistanceToBallInStop + robotRadius + distanceThreshold
+	if !e.ballSteady() ||
+		e.robotsInsideRadius(e.gcState.TrackerStateGc.Robots, e.gcState.TrackerStateGc.Ball.Pos, radius) {
+		return false
+	}
+	return false
+}

internal/app/engine/process_runningtostop.go

@@ -31,12 +31,12 @@ func (e *Engine) processRunningToStop() {
 }
 
 func (e *Engine) ballPlacementRequired() bool {
-	if e.currentState.PlacementPos == nil || e.gcState.TrackerStateGc.BallPos == nil {
+	if e.currentState.PlacementPos == nil || e.gcState.TrackerStateGc.Ball.Pos == nil {
 		// fallback if the fields are not set
 		return false
 	}
 	placementPos := locationToVector2(e.currentState.PlacementPos)
-	ballPos := e.gcState.TrackerStateGc.BallPos
+	ballPos := e.gcState.TrackerStateGc.Ball.Pos
 
 	// The ball is closer than 1m to the designated position.
 	if ballPos.DistanceTo(placementPos) > e.gameConfig.BallPlacementRequiredDistance {
@@ -67,8 +67,8 @@ func (e *Engine) ballPlacementRequired() bool {
 }
 
 func (e *Engine) ballSteady() bool {
-	if e.gcState.TrackerStateGc.BallVel == nil {
+	if e.gcState.TrackerStateGc.Ball.Vel == nil {
 		return true
 	}
-	return e.gcState.TrackerStateGc.BallVel.Length() < ballSteadyThreshold
+	return e.gcState.TrackerStateGc.Ball.Vel.Length() < ballSteadyThreshold
 }