Feat: Ignore bots in substitution zone when counting bots during yellow cards

Author: g3force

internal/app/config/config.go

@@ -23,6 +23,7 @@ type Special struct {
 type Geometry struct {
 	FieldLength                     float64 `yaml:"field-length"`
 	FieldWidth                      float64 `yaml:"field-width"`
+	BoundaryWidth                   float64 `yaml:"boundary-width"`
 	DefenseAreaDepth                float64 `yaml:"defense-area-depth"`
 	DefenseAreaWidth                float64 `yaml:"defense-area-width"`
 	PenaltyKickDistToGoal           float64 `yaml:"penalty-kick-dist-to-goal"`
@@ -231,6 +232,7 @@ func DefaultControllerConfig() (c Controller) {
 	c.Game.DefaultGeometry[DivA] = Geometry{
 		FieldLength:                     12,
 		FieldWidth:                      9,
+		BoundaryWidth:                   0.3,
 		DefenseAreaDepth:                1.8,
 		DefenseAreaWidth:                3.6,
 		PenaltyKickDistToGoal:           8.0,
@@ -244,6 +246,7 @@ func DefaultControllerConfig() (c Controller) {
 	c.Game.DefaultGeometry[DivB] = Geometry{
 		FieldLength:                     9,
 		FieldWidth:                      6,
+		BoundaryWidth:                   0.3,
 		DefenseAreaDepth:                1,
 		DefenseAreaWidth:                2,
 		PenaltyKickDistToGoal:           6.0,

internal/app/engine/common.go

@@ -12,6 +12,7 @@ import (
 const ballSteadyThreshold = 0.2
 const robotRadius = 0.09
 const distanceThreshold = 0.05
+const botSubstitutionMaxX = 1.0
 
 func createGameEventChange(eventType state.GameEvent_Type, event *state.GameEvent) *statemachine.Change {
 	event.Type = &eventType
@@ -131,6 +132,30 @@ func (x *GcStateTracker) NumTeamRobots(team state.Team) (count int32) {
 	return
 }
 
+func (e *Engine) NumTeamRobotsExcludingSubstitutionZone(team state.Team) (count int32) {
+	boundaryWidth := e.getGeometry().BoundaryWidth
+	fieldMaxY := e.getGeometry().FieldWidth / 2
+	rects := []*geom.Rectangle{
+		geom.NewRectangleFromPoints(
+			geom.NewVector2(-(botSubstitutionMaxX+robotRadius), fieldMaxY-robotRadius),
+			geom.NewVector2(botSubstitutionMaxX+robotRadius, fieldMaxY+boundaryWidth),
+		),
+		geom.NewRectangleFromPoints(
+			geom.NewVector2(-(botSubstitutionMaxX+robotRadius), -(fieldMaxY-robotRadius)),
+			geom.NewVector2(botSubstitutionMaxX+robotRadius, -(fieldMaxY+boundaryWidth)),
+		),
+	}
+
+	for _, robot := range e.trackerStateGc.Robots {
+		if !rects[0].IsPointInside(robot.Pos) &&
+			!rects[1].IsPointInside(robot.Pos) &&
+			*robot.Id.Team == team {
+			count++
+		}
+	}
+	return
+}
+
 func (e *Engine) isBallInAnyDefenseArea() (bool, state.Team) {
 	for _, team := range state.BothTeams() {
 		defenseArea := geom.NewDefenseArea(e.getGeometry(), *e.currentState.TeamState[team.String()].OnPositiveHalf)

internal/app/engine/consume_geometry.go

@@ -15,6 +15,7 @@ func (e *Engine) ProcessGeometry(data *vision.SSL_GeometryData) {
 
 	newGeometry.FieldWidth = float64(*data.Field.FieldWidth) / 1000.0
 	newGeometry.FieldLength = float64(*data.Field.FieldLength) / 1000.0
+	newGeometry.BoundaryWidth = float64(*data.Field.BoundaryWidth) / 1000.0
 	newGeometry.GoalWidth = float64(*data.Field.GoalWidth) / 1000.0
 
 	if data.Field.PenaltyAreaWidth != nil {

internal/app/engine/process_botremoved.go

@@ -47,7 +47,9 @@ func (e *Engine) processBotNumberPerTeam(team state.Team) {
 		e.gameConfig.YellowCardDuration-removalTime,
 	)
 
-	numBots := e.trackerStateGc.NumTeamRobots(team)
+	// count number of bots, but exclude bots in substitution zone
+	// This allows the robot handler to remove bots from the field without stressing too much
+	numBots := e.NumTeamRobotsExcludingSubstitutionZone(team)
 	numBotsAllowed := *teamInfo.MaxAllowedBots + newCards
 	if numBots > numBotsAllowed {