Check positions during penalty preparation

Author: g3force

config/ssl-game-controller.yaml

@@ -55,6 +55,7 @@ game:
       defense-area-depth: 1.2
       defense-area-width: 2.4
       penalty-kick-dist-to-goal: 8.0
+      goal-width: 1.2
       placement-offset-touch-line: 0.2
       placement-offset-goal-line: 0.2
       placement-offset-goal-line-goal-kick: 1.0
@@ -65,6 +66,7 @@ game:
       defense-area-depth: 1.0
       defense-area-width: 2.0
       penalty-kick-dist-to-goal: 6.0
+      goal-width: 1.0
       placement-offset-touch-line: 0.2
       placement-offset-goal-line: 0.2
       placement-offset-goal-line-goal-kick: 1.0

internal/app/config/config.go

@@ -26,6 +26,7 @@ type Geometry struct {
 	DefenseAreaDepth                float64 `yaml:"defense-area-depth"`
 	DefenseAreaWidth                float64 `yaml:"defense-area-width"`
 	PenaltyKickDistToGoal           float64 `yaml:"penalty-kick-dist-to-goal"`
+	GoalWidth                       float64 `yaml:"goal-width"`
 	PlacementOffsetTouchLine        float64 `yaml:"placement-offset-touch-line"`
 	PlacementOffsetGoalLine         float64 `yaml:"placement-offset-goal-line"`
 	PlacementOffsetGoalLineGoalKick float64 `yaml:"placement-offset-goal-line-goal-kick"`
@@ -196,6 +197,7 @@ func DefaultControllerConfig() (c Controller) {
 		DefenseAreaDepth:                1.2,
 		DefenseAreaWidth:                2.4,
 		PenaltyKickDistToGoal:           8.0,
+		GoalWidth:                       1.2,
 		PlacementOffsetTouchLine:        0.2,
 		PlacementOffsetGoalLine:         0.2,
 		PlacementOffsetGoalLineGoalKick: 1.0,
@@ -207,6 +209,7 @@ func DefaultControllerConfig() (c Controller) {
 		DefenseAreaDepth:                1,
 		DefenseAreaWidth:                2,
 		PenaltyKickDistToGoal:           6.0,
+		GoalWidth:                       1.0,
 		PlacementOffsetTouchLine:        0.2,
 		PlacementOffsetGoalLine:         0.2,
 		PlacementOffsetGoalLineGoalKick: 1.0,

internal/app/config/testdata/config.yaml

@@ -56,6 +56,7 @@ game:
       defense-area-depth: 1.2
       defense-area-width: 2.4
       penalty-kick-dist-to-goal: 8.0
+      goal-width: 1.2
       placement-offset-touch-line: 0.2
       placement-offset-goal-line: 0.2
       placement-offset-goal-line-goal-kick: 1.0
@@ -66,6 +67,7 @@ game:
       defense-area-depth: 1.0
       defense-area-width: 2.0
       penalty-kick-dist-to-goal: 6.0
+      goal-width: 1.0
       placement-offset-touch-line: 0.2
       placement-offset-goal-line: 0.2
       placement-offset-goal-line-goal-kick: 1.0

internal/app/engine/consume_geometry.go

@@ -13,6 +13,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.GoalWidth = float64(*data.Field.GoalWidth) / 1000.0
 	for _, line := range data.Field.FieldLines {
 		if *line.Name == "LeftFieldLeftPenaltyStretch" {
 			newGeometry.DefenseAreaDepth = math.Abs(float64(*line.P1.X-*line.P2.X)) / 1000.0

internal/app/engine/process_prepare.go

@@ -1,12 +1,14 @@
 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"
 	"time"
 )
 
 const minPreparationTime = time.Second * 2
+const distanceToBallDuringPenalty = 1.0
 
 func (e *Engine) processPrepare() {
 	if !e.currentState.Command.IsPrepare() ||
@@ -38,13 +40,39 @@ func (e *Engine) processPrepare() {
 	}
 
 	if *e.currentState.Command.Type == state.Command_PENALTY {
-		// TODO check if conditions met for penalty
-		// keeper pos
-
-		// kicking team positions correct
+		keeperId := e.penaltyKeeperId()
+		if keeperId == nil {
+			return
+		}
+		keeperPos := e.robotPos(keeperId)
+		if keeperPos == nil || !e.posInsideGoal(keeperPos) {
+			return
+		}
 
-		// opponent team positions correct
+		keeperTeamInfo := e.currentState.TeamState[keeperId.Team.String()]
+		ballPos := e.gcState.TrackerStateGc.Ball.Pos
 
+		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++
+			}
+		}
 	}
 
 	e.Enqueue(&statemachine.Change{
@@ -54,3 +82,30 @@ func (e *Engine) processPrepare() {
 		},
 	})
 }
+
+func (e *Engine) penaltyKeeperId() *state.RobotId {
+	forTeam := e.currentState.Command.ForTeam.Opposite()
+	teamInfo := e.currentState.TeamState[forTeam.String()]
+	keeperId := uint32(*teamInfo.Goalkeeper)
+	return &state.RobotId{
+		Id:   &keeperId,
+		Team: &forTeam,
+	}
+}
+
+func (e *Engine) robotPos(robotId *state.RobotId) *geom.Vector2 {
+	for _, robot := range e.gcState.TrackerStateGc.Robots {
+		if *robot.Id.Id == *robotId.Id && *robot.Id.Team == *robotId.Team {
+			return robot.Pos
+		}
+	}
+	return nil
+}
+
+func (e *Engine) posInsideGoal(pos *geom.Vector2) bool {
+	forTeam := *e.currentState.Command.ForTeam
+	teamInfo := e.currentState.TeamState[forTeam.String()]
+	goalCenter := geom.GoalCenter(e.GetGeometry(), *teamInfo.OnPositiveHalf)
+	goalArea := geom.NewRectangleFromCenter(goalCenter, robotRadius*2, e.GetGeometry().GoalWidth)
+	return goalArea.IsPointInside(pos)
+}

internal/app/geom/goal.go

@@ -0,0 +1,18 @@
+package geom
+
+import (
+	"github.com/RoboCup-SSL/ssl-game-controller/internal/app/config"
+)
+
+// GoalCenter returns the goal center using the flag for the side
+func GoalCenter(geometry config.Geometry, onPositiveHalf bool) *Vector2 {
+	if onPositiveHalf {
+		return GoalCenterBySign(geometry, 1)
+	}
+	return GoalCenterBySign(geometry, -1)
+}
+
+// GoalCenterBySign returns the goal center using the sign
+func GoalCenterBySign(geometry config.Geometry, sign float64) *Vector2 {
+	return NewVector2(sign*geometry.FieldLength/2, 0)
+}