[feature] Receive geometry from vision

Author: g3force

config/ssl-game-controller.yaml

@@ -1,5 +1,6 @@
-publish:
-  address: 224.5.23.1:10003
+network:
+  publish-address: 224.5.23.1:10003
+  vision-address: 224.5.23.2:10006
 server:
   auto-ref:
     address: :10007
@@ -30,17 +31,17 @@ game:
     DivA:
       field-length: 12.0
       field-width: 9.0
-      penalty-area-depth: 1.2
-      penalty-area-width: 2.4
+      defense-area-depth: 1.2
+      defense-area-width: 2.4
       placement-offset-touch-line: 0.2
       placement-offset-goal-line: 0.2
       placement-offset-goal-line-goal-kick: 1.0
       placement-offset-defense-area: 1.0
     DivB:
       field-length: 9.0
       field-width: 6.0
-      penalty-area-depth: 1.0
-      penalty-area-width: 2.0
+      defense-area-depth: 1.0
+      defense-area-width: 2.0
       placement-offset-touch-line: 0.2
       placement-offset-goal-line: 0.2
       placement-offset-goal-line-goal-kick: 1.0

internal/app/controller/config.go

@@ -19,8 +19,8 @@ type ConfigSpecial struct {
 type ConfigGeometry struct {
 	FieldLength                     float64 `yaml:"field-length"`
 	FieldWidth                      float64 `yaml:"field-width"`
-	PenaltyAreaDepth                float64 `yaml:"penalty-area-depth"`
-	PenaltyAreaWidth                float64 `yaml:"penalty-area-width"`
+	DefenseAreaDepth                float64 `yaml:"defense-area-depth"`
+	DefenseAreaWidth                float64 `yaml:"defense-area-width"`
 	PlacementOffsetTouchLine        float64 `yaml:"placement-offset-touch-line"`
 	PlacementOffsetGoalLine         float64 `yaml:"placement-offset-goal-line"`
 	PlacementOffsetGoalLineGoalKick float64 `yaml:"placement-offset-goal-line-goal-kick"`
@@ -40,9 +40,10 @@ type ConfigGame struct {
 	MultiplePlacementFailures int                          `yaml:"multiple-placement-failures"`
 }
 
-// ConfigPublish holds configs for publishing the state and commands to the teams
-type ConfigPublish struct {
-	Address string `yaml:"address"`
+// ConfigNetwork holds configs for network communication
+type ConfigNetwork struct {
+	PublishAddress string `yaml:"publish-address"`
+	VisionAddress  string `yaml:"vision-address"`
 }
 
 // ConfigServer holds configs for the available server services
@@ -65,7 +66,7 @@ type ConfigServerTeam struct {
 
 // Config structure for the game controller
 type Config struct {
-	Publish ConfigPublish `yaml:"publish"`
+	Network ConfigNetwork `yaml:"network"`
 	Game    ConfigGame    `yaml:"game"`
 	Server  ConfigServer  `yaml:"server"`
 }
@@ -95,7 +96,8 @@ func LoadConfig(fileName string) (config Config, err error) {
 
 // DefaultConfig creates a config with default values
 func DefaultConfig() (c Config) {
-	c.Publish.Address = "224.5.23.1:10003"
+	c.Network.PublishAddress = "224.5.23.1:10003"
+	c.Network.VisionAddress = "224.5.23.2:10006"
 	c.Game.YellowCardDuration = 2 * time.Minute
 	c.Game.MultipleCardStep = 3
 	c.Game.MultipleFoulStep = 3
@@ -124,8 +126,8 @@ func DefaultConfig() (c Config) {
 	c.Game.DefaultGeometry[DivA] = new(ConfigGeometry)
 	c.Game.DefaultGeometry[DivA].FieldLength = 12
 	c.Game.DefaultGeometry[DivA].FieldWidth = 9
-	c.Game.DefaultGeometry[DivA].PenaltyAreaDepth = 1.2
-	c.Game.DefaultGeometry[DivA].PenaltyAreaWidth = 2.4
+	c.Game.DefaultGeometry[DivA].DefenseAreaDepth = 1.2
+	c.Game.DefaultGeometry[DivA].DefenseAreaWidth = 2.4
 	c.Game.DefaultGeometry[DivA].PlacementOffsetGoalLine = 0.2
 	c.Game.DefaultGeometry[DivA].PlacementOffsetGoalLineGoalKick = 1.0
 	c.Game.DefaultGeometry[DivA].PlacementOffsetTouchLine = 0.2
@@ -134,8 +136,8 @@ func DefaultConfig() (c Config) {
 	c.Game.DefaultGeometry[DivB] = new(ConfigGeometry)
 	c.Game.DefaultGeometry[DivB].FieldLength = 9
 	c.Game.DefaultGeometry[DivB].FieldWidth = 6
-	c.Game.DefaultGeometry[DivB].PenaltyAreaDepth = 1
-	c.Game.DefaultGeometry[DivB].PenaltyAreaWidth = 2
+	c.Game.DefaultGeometry[DivB].DefenseAreaDepth = 1
+	c.Game.DefaultGeometry[DivB].DefenseAreaWidth = 2
 	c.Game.DefaultGeometry[DivB].PlacementOffsetGoalLine = 0.2
 	c.Game.DefaultGeometry[DivB].PlacementOffsetGoalLineGoalKick = 1.0
 	c.Game.DefaultGeometry[DivB].PlacementOffsetTouchLine = 0.2

internal/app/controller/controller.go

@@ -4,8 +4,10 @@ import (
 	"github.com/RoboCup-SSL/ssl-game-controller/internal/app/rcon"
 	"github.com/RoboCup-SSL/ssl-game-controller/pkg/refproto"
 	"github.com/RoboCup-SSL/ssl-game-controller/pkg/timer"
+	"github.com/RoboCup-SSL/ssl-go-tools/pkg/sslproto"
 	"github.com/pkg/errors"
 	"log"
+	"math"
 	"sync"
 	"time"
 )
@@ -25,6 +27,7 @@ type GameController struct {
 	numRefereeEventsLastPublish int
 	outstandingTeamChoice       *TeamChoice
 	Mutex                       sync.Mutex
+	VisionReceiver              *VisionReceiver
 }
 
 type TeamChoice struct {
@@ -42,6 +45,9 @@ func NewGameController() (c *GameController) {
 	c.ApiServer = ApiServer{}
 	c.ApiServer.Consumer = c
 
+	c.VisionReceiver = NewVisionReceiver(c.Config.Network.VisionAddress)
+	c.VisionReceiver.GeometryCallback = c.ProcessGeometry
+
 	c.AutoRefServer = rcon.NewAutoRefServer()
 	c.AutoRefServer.LoadTrustedKeys(c.Config.Server.AutoRef.TrustedKeysDir)
 	c.AutoRefServer.ProcessRequest = c.ProcessAutoRefRequests
@@ -59,6 +65,34 @@ func NewGameController() (c *GameController) {
 	return
 }
 
+func (c *GameController) ProcessGeometry(data *sslproto.SSL_GeometryData) {
+	if int32(math.Round(c.Engine.Geometry.FieldWidth*1000.0)) != *data.Field.FieldWidth {
+		newFieldWidth := float64(*data.Field.FieldWidth) / 1000.0
+		log.Printf("FieldWidth changed from %v to %v", c.Engine.Geometry.FieldWidth, newFieldWidth)
+		c.Engine.Geometry.FieldWidth = newFieldWidth
+	}
+	if int32(math.Round(c.Engine.Geometry.FieldLength*1000)) != *data.Field.FieldLength {
+		newFieldLength := float64(*data.Field.FieldLength) / 1000.0
+		log.Printf("FieldLength changed from %v to %v", c.Engine.Geometry.FieldLength, newFieldLength)
+		c.Engine.Geometry.FieldLength = newFieldLength
+	}
+	for _, line := range data.Field.FieldLines {
+		if *line.Name == "LeftFieldLeftPenaltyStretch" {
+			defenseAreaDepth := math.Abs(float64(*line.P1.X-*line.P2.X)) / 1000.0
+			if math.Abs(defenseAreaDepth-c.Engine.Geometry.DefenseAreaDepth) > 1e-3 {
+				log.Printf("DefenseAreaDepth changed from %v to %v", c.Engine.Geometry.DefenseAreaDepth, defenseAreaDepth)
+				c.Engine.Geometry.DefenseAreaDepth = defenseAreaDepth
+			}
+		} else if *line.Name == "LeftPenaltyStretch" {
+			defenseAreaWidth := math.Abs(float64(*line.P1.Y-*line.P2.Y)) / 1000.0
+			if math.Abs(defenseAreaWidth-c.Engine.Geometry.DefenseAreaWidth) > 1e-3 {
+				log.Printf("DefenseAreaDepth changed from %v to %v", c.Engine.Geometry.DefenseAreaWidth, defenseAreaWidth)
+				c.Engine.Geometry.DefenseAreaWidth = defenseAreaWidth
+			}
+		}
+	}
+}
+
 func (c *GameController) ProcessAutoRefRequests(request refproto.AutoRefToControllerRequest) error {
 	c.Mutex.Lock()
 	defer c.Mutex.Unlock()
@@ -211,9 +245,9 @@ func (c *GameController) timeoutTeamChoice() {
 }
 
 func loadPublisher(config Config) Publisher {
-	publisher, err := NewPublisher(config.Publish.Address)
+	publisher, err := NewPublisher(config.Network.PublishAddress)
 	if err != nil {
-		log.Printf("Could not start publisher on %v. %v", config.Publish.Address, err)
+		log.Printf("Could not start publisher on %v. %v", config.Network.PublishAddress, err)
 	}
 	return publisher
 }

internal/app/controller/placementPos.go

@@ -72,7 +72,7 @@ func (e *Engine) BallPlacementPos() *Location {
 	case GameEventDefenderInDefenseArea:
 		teamInFavor := event.ByTeam().Opposite()
 		location := Location{}
-		location.X = (e.Geometry.FieldLength / 2.0) - e.Geometry.PenaltyAreaDepth
+		location.X = (e.Geometry.FieldLength / 2.0) - e.Geometry.DefenseAreaDepth
 		if e.State.TeamState[teamInFavor].OnPositiveHalf {
 			location.X *= -1
 		}
@@ -128,8 +128,8 @@ func (e *Engine) validateLocation(location *Location) *Location {
 }
 
 func (e *Engine) movePositionOutOfDefenseArea(location *Location) {
-	maxX := e.Geometry.FieldLength/2 - e.Geometry.PenaltyAreaDepth - e.Geometry.PlacementOffsetDefenseArea
-	minY := e.Geometry.PenaltyAreaWidth + e.Geometry.PlacementOffsetDefenseArea
+	maxX := e.Geometry.FieldLength/2 - e.Geometry.DefenseAreaDepth - e.Geometry.PlacementOffsetDefenseArea
+	minY := e.Geometry.DefenseAreaWidth + e.Geometry.PlacementOffsetDefenseArea
 	if math.Abs(location.X) > maxX && math.Abs(location.Y) < minY {
 		if math.Abs(maxX-math.Abs(location.X)) < math.Abs(minY-math.Abs(location.Y)) {
 			location.X = math.Copysign(maxX, location.X)

internal/app/controller/visionReceiver.go

@@ -0,0 +1,66 @@
+package controller
+
+import (
+	"github.com/RoboCup-SSL/ssl-go-tools/pkg/sslproto"
+	"github.com/golang/protobuf/proto"
+	"log"
+	"net"
+	"time"
+)
+
+type VisionReceiver struct {
+	DetectionCallback func(*sslproto.SSL_DetectionFrame)
+	GeometryCallback  func(*sslproto.SSL_GeometryData)
+}
+
+func NewVisionReceiver(address string) (v *VisionReceiver) {
+	v = new(VisionReceiver)
+	v.DetectionCallback = func(*sslproto.SSL_DetectionFrame) {}
+	v.GeometryCallback = func(data *sslproto.SSL_GeometryData) {}
+
+	addr, err := net.ResolveUDPAddr("udp", address)
+	if err != nil {
+		log.Print(err)
+		return
+	}
+	conn, err := net.ListenMulticastUDP("udp", nil, addr)
+	if err != nil {
+		log.Print(err)
+		return
+	}
+
+	if err := conn.SetReadBuffer(maxDatagramSize); err != nil {
+		log.Printf("Could not set read buffer to %v.", maxDatagramSize)
+	}
+	log.Println("Receiving vision from", address)
+
+	go v.receive(conn)
+	return
+}
+
+func (v *VisionReceiver) receive(conn *net.UDPConn) {
+	b := make([]byte, maxDatagramSize)
+	for {
+		n, err := conn.Read(b)
+		if err != nil {
+			log.Print("Could not read", err)
+			time.Sleep(1 * time.Second)
+			continue
+		}
+		if n >= maxDatagramSize {
+			log.Fatal("Buffer size too small")
+		}
+		wrapper := sslproto.SSL_WrapperPacket{}
+		if err := proto.Unmarshal(b[0:n], &wrapper); err != nil {
+			log.Println("Could not unmarshal referee message")
+			continue
+		}
+
+		if wrapper.Geometry != nil {
+			v.GeometryCallback(wrapper.Geometry)
+		}
+		if wrapper.Detection != nil {
+			v.DetectionCallback(wrapper.Detection)
+		}
+	}
+}