Add robot count maintainer

README.md

@@ -7,9 +7,9 @@
 
 A controller for SSL simulation tournaments, that:
  * places the ball automatically, if it can not be placed by the teams
- * TODO: Adds and removes robots, if the robot count does not fit
+ * Adds and removes robots, if the robot count does not match the max robot count in the referee message
    * During gameplay, if the [conditions](https://robocup-ssl.github.io/ssl-rules/sslrules.html#_robot_substitution) are met
-   * During stop by selecting robots nearest to either crossing of half-way line and touch line)
+   * During halt by selecting robots nearest to either crossing of half-way line and touch line)
  * TODO: Set robot limits based on some configuration
 
 ## Usage

internal/geom/rectangle.go

@@ -0,0 +1,74 @@
+package geom
+
+import "math"
+
+type Rectangle struct {
+	center  *Vector2
+	xExtent float64
+	yExtent float64
+}
+
+// NewRectangleFromCenter creates a new rectangle from a center and the x- and y-extent
+func NewRectangleFromCenter(center *Vector2, xExtent, yExtent float64) (r *Rectangle) {
+	return &Rectangle{
+		center:  center,
+		xExtent: xExtent,
+		yExtent: yExtent,
+	}
+}
+
+// NewRectangleFromPoints creates a new rectangle from two points
+func NewRectangleFromPoints(p1, p2 *Vector2) (r *Rectangle) {
+	r = new(Rectangle)
+	r.xExtent = math.Abs(p1.X64() - p2.X64())
+	r.yExtent = math.Abs(p1.Y64() - p2.Y64())
+	r.center = NewVector2(
+		math.Min(p1.X64(), p2.X64())+r.xExtent/2.0,
+		math.Min(p1.Y64(), p2.Y64())+r.yExtent/2.0,
+	)
+	return
+}
+
+// WithMargin creates a new rectangle with the added/subtracted margin
+func (r *Rectangle) WithMargin(margin float64) *Rectangle {
+	return &Rectangle{
+		center:  r.center,
+		xExtent: math.Max(0, r.xExtent+2*margin),
+		yExtent: math.Max(0, r.yExtent+2*margin),
+	}
+}
+
+// MaxX returns the largest x value
+func (r *Rectangle) MaxX() float64 {
+	return r.center.X64() + r.xExtent/2.0
+}
+
+// MaxX returns the smallest x value
+func (r *Rectangle) MinX() float64 {
+	return r.center.X64() - r.xExtent/2.0
+}
+
+// MaxX returns the largest y value
+func (r *Rectangle) MaxY() float64 {
+	return r.center.Y64() + r.yExtent/2.0
+}
+
+// MaxX returns the smallest y value
+func (r *Rectangle) MinY() float64 {
+	return r.center.Y64() - r.yExtent/2.0
+}
+
+// IsPointInside returns true if the given point is inside the rectangle
+func (r *Rectangle) IsPointInside(p *Vector2) bool {
+	return isBetween(p.X64(), r.MinX(), r.MaxX()) &&
+		isBetween(p.Y64(), r.MinY(), r.MaxY())
+}
+
+// isBetween returns true, if x is between min and max.
+// min can be larger than max, in which case the meaning of min and max is switched.
+func isBetween(x, min, max float64) bool {
+	if max > min {
+		return (x >= min) && (x <= max)
+	}
+	return (x >= max) && (x <= min)
+}

internal/simctl/maintain_robot_count.go

@@ -0,0 +1,177 @@
+package simctl
+
+import (
+	"github.com/RoboCup-SSL/ssl-simulation-controller/internal/geom"
+	"github.com/RoboCup-SSL/ssl-simulation-controller/internal/referee"
+	"github.com/RoboCup-SSL/ssl-simulation-controller/internal/tracker"
+	"github.com/golang/protobuf/proto"
+	"log"
+	"math"
+	"sort"
+	"time"
+)
+
+type RobotCountMaintainer struct {
+	c *SimulationController
+
+	lastTimeSendCommand time.Time
+	haltTime            *time.Time
+}
+
+func (r *RobotCountMaintainer) handleRobotCount() {
+
+	if time.Now().Sub(r.lastTimeSendCommand) < 500*time.Millisecond {
+		// Placed ball just recently
+		return
+	}
+
+	if *r.c.lastRefereeMsg.Command != referee.Referee_HALT &&
+		len(r.c.lastTrackedFrame.TrackedFrame.Balls) > 0 &&
+		math.Abs(float64(*r.c.lastTrackedFrame.TrackedFrame.Balls[0].Pos.X)) < 2 {
+		// Rule: The ball must be at least 2 meters away from the halfway line.
+		return
+	}
+
+	var blueRobots []*tracker.TrackedRobot
+	var yellowRobots []*tracker.TrackedRobot
+	for _, robot := range r.c.lastTrackedFrame.TrackedFrame.Robots {
+		if *robot.RobotId.Team == referee.Team_BLUE {
+			blueRobots = append(blueRobots, robot)
+		} else if *robot.RobotId.Team == referee.Team_YELLOW {
+			yellowRobots = append(yellowRobots, robot)
+		}
+	}
+
+	r.updateRobotCount(blueRobots, int(*r.c.lastRefereeMsg.Blue.MaxAllowedBots), referee.Team_BLUE)
+	r.updateRobotCount(yellowRobots, int(*r.c.lastRefereeMsg.Yellow.MaxAllowedBots), referee.Team_YELLOW)
+}
+
+func (r *RobotCountMaintainer) updateRobotCount(robots []*tracker.TrackedRobot, maxRobots int, team referee.Team) {
+	substCenterPos := geom.NewVector2(0, float64(*r.c.fieldSize.FieldWidth)/2000+float64(*r.c.fieldSize.BoundaryWidth)/2000.0-0.1)
+	substCenterNeg := geom.NewVector2Float32(0, -*substCenterPos.Y)
+	substRectPos := geom.NewRectangleFromCenter(substCenterPos, 2, float64(*r.c.fieldSize.BoundaryWidth)/1000+0.2)
+	substRectNeg := geom.NewRectangleFromCenter(substCenterNeg, 2, float64(*r.c.fieldSize.BoundaryWidth)/1000+0.2)
+	if len(robots) > 0 && len(robots) > maxRobots {
+		r.sortRobotsByDistanceToSubstitutionPos(robots)
+		if *r.c.lastRefereeMsg.Command == referee.Referee_HALT ||
+			substRectPos.IsPointInside(robots[0].Pos) ||
+			substRectNeg.IsPointInside(robots[0].Pos) {
+			r.removeRobot(robots[0].RobotId)
+		}
+	}
+	if len(robots) < maxRobots {
+		y := float64(*r.c.fieldSize.FieldWidth) / 2000.0
+		x := 0.1
+		for i := 0; i < 100; i++ {
+			pos := geom.NewVector2(x, y)
+			if r.isFreeOfObstacles(pos) {
+				id := r.nextFreeRobotId(team)
+				r.addRobot(id, pos)
+				break
+			}
+			x *= -1
+			if x > 0 {
+				x += 0.1
+			}
+		}
+	}
+}
+
+func (r *RobotCountMaintainer) nextFreeRobotId(team referee.Team) *referee.RobotId {
+	for i := 0; i < 16; i++ {
+		id := uint32(i)
+		robotId := &referee.RobotId{
+			Id:   &id,
+			Team: &team,
+		}
+		if r.isRobotIdFree(robotId) {
+			return robotId
+		}
+	}
+	return nil
+}
+
+func (r *RobotCountMaintainer) isRobotIdFree(id *referee.RobotId) bool {
+
+	for _, robot := range r.c.lastTrackedFrame.TrackedFrame.Robots {
+		if *robot.RobotId.Id == *id.Id && *robot.RobotId.Team == *id.Team {
+			return false
+		}
+	}
+	return true
+}
+
+func (r *RobotCountMaintainer) isFreeOfObstacles(pos *geom.Vector2) bool {
+	for _, robot := range r.c.lastTrackedFrame.TrackedFrame.Robots {
+		if robot.Pos.DistanceTo(pos) < 0.2 {
+			return false
+		}
+	}
+	for _, ball := range r.c.lastTrackedFrame.TrackedFrame.Balls {
+		pos2d := geom.NewVector2Float32(*ball.Pos.X, *ball.Pos.Y)
+		if pos2d.DistanceTo(pos) < 0.1 {
+			return false
+		}
+	}
+	return true
+}
+
+func (r *RobotCountMaintainer) sortRobotsByDistanceToSubstitutionPos(robots []*tracker.TrackedRobot) {
+	negSubstPos := geom.NewVector2(0, -float64(*r.c.fieldSize.FieldWidth)/2000)
+	posSubstPos := geom.NewVector2(0, +float64(*r.c.fieldSize.FieldWidth)/2000)
+	sort.Slice(robots, func(i, j int) bool {
+		distI := math.Min(robots[i].Pos.DistanceTo(negSubstPos), robots[i].Pos.DistanceTo(posSubstPos))
+		distJ := math.Min(robots[j].Pos.DistanceTo(negSubstPos), robots[j].Pos.DistanceTo(posSubstPos))
+		return distI < distJ
+	})
+}
+
+func (r *RobotCountMaintainer) removeRobot(id *referee.RobotId) {
+	log.Printf("Remove robot %v", id)
+
+	present := false
+	command := SimulatorCommand{
+		Control: &SimulatorControl{
+			TeleportRobot: []*TeleportRobot{
+				{
+					Id:      id,
+					Present: &present,
+				},
+			},
+		},
+	}
+
+	r.sendControlCommand(&command)
+}
+
+func (r *RobotCountMaintainer) addRobot(id *referee.RobotId, pos *geom.Vector2) {
+	log.Printf("Add robot %v @ %v", id, pos)
+
+	present := true
+	orientation := float32(0)
+	command := SimulatorCommand{
+		Control: &SimulatorControl{
+			TeleportRobot: []*TeleportRobot{
+				{
+					Id:          id,
+					X:           pos.X,
+					Y:           pos.Y,
+					Orientation: &orientation,
+					Present:     &present,
+				},
+			},
+		},
+	}
+
+	r.sendControlCommand(&command)
+}
+
+func (r *RobotCountMaintainer) sendControlCommand(command *SimulatorCommand) {
+
+	if data, err := proto.Marshal(command); err != nil {
+		log.Println("Could not marshal command: ", err)
+	} else {
+		r.c.simControlClient.Send(data)
+		r.lastTimeSendCommand = time.Now()
+	}
+}

internal/simctl/simctl.go

@@ -22,7 +22,8 @@ type SimulationController struct {
 	lastRefereeMsg   *referee.Referee
 	fieldSize        *vision.SSL_GeometryFieldSize
 
-	ballReplacer BallReplacer
+	ballReplacer         BallReplacer
+	robotCountMaintainer RobotCountMaintainer
 }
 
 func NewSimulationController(visionAddress, refereeAddress, trackerAddress string, simControlPort string) (c *SimulationController) {
@@ -32,6 +33,7 @@ func NewSimulationController(visionAddress, refereeAddress, trackerAddress strin
 	c.trackerServer = sslnet.NewMulticastServer(trackerAddress, c.onNewTrackerData)
 	c.simControlPort = simControlPort
 	c.ballReplacer.c = c
+	c.robotCountMaintainer.c = c
 	return
 }
 
@@ -98,6 +100,7 @@ func (c *SimulationController) handle() {
 	}
 
 	c.ballReplacer.handleReplaceBall()
+	c.robotCountMaintainer.handleRobotCount()
 }
 
 func (c *SimulationController) Start() {