Skip to content

Commit 521e095

Browse files
g3forceg3force
committed
Add robot count maintainer
1 parent 6930fef commit 521e095

File tree

4 files changed

+257
-3
lines changed

4 files changed

+257
-3
lines changed

‎README.md‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -7,9 +7,9 @@
77

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

1515
## Usage

‎internal/geom/rectangle.go‎

Lines changed: 74 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,74 @@
1+
package geom
2+
3+
import "math"
4+
5+
type Rectangle struct {
6+
center *Vector2
7+
xExtent float64
8+
yExtent float64
9+
}
10+
11+
// NewRectangleFromCenter creates a new rectangle from a center and the x- and y-extent
12+
func NewRectangleFromCenter(center *Vector2, xExtent, yExtent float64) (r *Rectangle) {
13+
return &Rectangle{
14+
center: center,
15+
xExtent: xExtent,
16+
yExtent: yExtent,
17+
}
18+
}
19+
20+
// NewRectangleFromPoints creates a new rectangle from two points
21+
func NewRectangleFromPoints(p1, p2 *Vector2) (r *Rectangle) {
22+
r = new(Rectangle)
23+
r.xExtent = math.Abs(p1.X64() - p2.X64())
24+
r.yExtent = math.Abs(p1.Y64() - p2.Y64())
25+
r.center = NewVector2(
26+
math.Min(p1.X64(), p2.X64())+r.xExtent/2.0,
27+
math.Min(p1.Y64(), p2.Y64())+r.yExtent/2.0,
28+
)
29+
return
30+
}
31+
32+
// WithMargin creates a new rectangle with the added/subtracted margin
33+
func (r *Rectangle) WithMargin(margin float64) *Rectangle {
34+
return &Rectangle{
35+
center: r.center,
36+
xExtent: math.Max(0, r.xExtent+2*margin),
37+
yExtent: math.Max(0, r.yExtent+2*margin),
38+
}
39+
}
40+
41+
// MaxX returns the largest x value
42+
func (r *Rectangle) MaxX() float64 {
43+
return r.center.X64() + r.xExtent/2.0
44+
}
45+
46+
// MaxX returns the smallest x value
47+
func (r *Rectangle) MinX() float64 {
48+
return r.center.X64() - r.xExtent/2.0
49+
}
50+
51+
// MaxX returns the largest y value
52+
func (r *Rectangle) MaxY() float64 {
53+
return r.center.Y64() + r.yExtent/2.0
54+
}
55+
56+
// MaxX returns the smallest y value
57+
func (r *Rectangle) MinY() float64 {
58+
return r.center.Y64() - r.yExtent/2.0
59+
}
60+
61+
// IsPointInside returns true if the given point is inside the rectangle
62+
func (r *Rectangle) IsPointInside(p *Vector2) bool {
63+
return isBetween(p.X64(), r.MinX(), r.MaxX()) &&
64+
isBetween(p.Y64(), r.MinY(), r.MaxY())
65+
}
66+
67+
// isBetween returns true, if x is between min and max.
68+
// min can be larger than max, in which case the meaning of min and max is switched.
69+
func isBetween(x, min, max float64) bool {
70+
if max > min {
71+
return (x >= min) && (x <= max)
72+
}
73+
return (x >= max) && (x <= min)
74+
}

‎internal/simctl/maintain_robot_count.go‎

Lines changed: 177 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,177 @@
1+
package simctl
2+
3+
import (
4+
"github.com/RoboCup-SSL/ssl-simulation-controller/internal/geom"
5+
"github.com/RoboCup-SSL/ssl-simulation-controller/internal/referee"
6+
"github.com/RoboCup-SSL/ssl-simulation-controller/internal/tracker"
7+
"github.com/golang/protobuf/proto"
8+
"log"
9+
"math"
10+
"sort"
11+
"time"
12+
)
13+
14+
type RobotCountMaintainer struct {
15+
c *SimulationController
16+
17+
lastTimeSendCommand time.Time
18+
haltTime *time.Time
19+
}
20+
21+
func (r *RobotCountMaintainer) handleRobotCount() {
22+
23+
if time.Now().Sub(r.lastTimeSendCommand) < 500*time.Millisecond {
24+
// Placed ball just recently
25+
return
26+
}
27+
28+
if *r.c.lastRefereeMsg.Command != referee.Referee_HALT &&
29+
len(r.c.lastTrackedFrame.TrackedFrame.Balls) > 0 &&
30+
math.Abs(float64(*r.c.lastTrackedFrame.TrackedFrame.Balls[0].Pos.X)) < 2 {
31+
// Rule: The ball must be at least 2 meters away from the halfway line.
32+
return
33+
}
34+
35+
var blueRobots []*tracker.TrackedRobot
36+
var yellowRobots []*tracker.TrackedRobot
37+
for _, robot := range r.c.lastTrackedFrame.TrackedFrame.Robots {
38+
if *robot.RobotId.Team == referee.Team_BLUE {
39+
blueRobots = append(blueRobots, robot)
40+
} else if *robot.RobotId.Team == referee.Team_YELLOW {
41+
yellowRobots = append(yellowRobots, robot)
42+
}
43+
}
44+
45+
r.updateRobotCount(blueRobots, int(*r.c.lastRefereeMsg.Blue.MaxAllowedBots), referee.Team_BLUE)
46+
r.updateRobotCount(yellowRobots, int(*r.c.lastRefereeMsg.Yellow.MaxAllowedBots), referee.Team_YELLOW)
47+
}
48+
49+
func (r *RobotCountMaintainer) updateRobotCount(robots []*tracker.TrackedRobot, maxRobots int, team referee.Team) {
50+
substCenterPos := geom.NewVector2(0, float64(*r.c.fieldSize.FieldWidth)/2000+float64(*r.c.fieldSize.BoundaryWidth)/2000.0-0.1)
51+
substCenterNeg := geom.NewVector2Float32(0, -*substCenterPos.Y)
52+
substRectPos := geom.NewRectangleFromCenter(substCenterPos, 2, float64(*r.c.fieldSize.BoundaryWidth)/1000+0.2)
53+
substRectNeg := geom.NewRectangleFromCenter(substCenterNeg, 2, float64(*r.c.fieldSize.BoundaryWidth)/1000+0.2)
54+
if len(robots) > 0 && len(robots) > maxRobots {
55+
r.sortRobotsByDistanceToSubstitutionPos(robots)
56+
if *r.c.lastRefereeMsg.Command == referee.Referee_HALT ||
57+
substRectPos.IsPointInside(robots[0].Pos) ||
58+
substRectNeg.IsPointInside(robots[0].Pos) {
59+
r.removeRobot(robots[0].RobotId)
60+
}
61+
}
62+
if len(robots) < maxRobots {
63+
y := float64(*r.c.fieldSize.FieldWidth) / 2000.0
64+
x := 0.1
65+
for i := 0; i < 100; i++ {
66+
pos := geom.NewVector2(x, y)
67+
if r.isFreeOfObstacles(pos) {
68+
id := r.nextFreeRobotId(team)
69+
r.addRobot(id, pos)
70+
break
71+
}
72+
x *= -1
73+
if x > 0 {
74+
x += 0.1
75+
}
76+
}
77+
}
78+
}
79+
80+
func (r *RobotCountMaintainer) nextFreeRobotId(team referee.Team) *referee.RobotId {
81+
for i := 0; i < 16; i++ {
82+
id := uint32(i)
83+
robotId := &referee.RobotId{
84+
Id: &id,
85+
Team: &team,
86+
}
87+
if r.isRobotIdFree(robotId) {
88+
return robotId
89+
}
90+
}
91+
return nil
92+
}
93+
94+
func (r *RobotCountMaintainer) isRobotIdFree(id *referee.RobotId) bool {
95+
96+
for _, robot := range r.c.lastTrackedFrame.TrackedFrame.Robots {
97+
if *robot.RobotId.Id == *id.Id && *robot.RobotId.Team == *id.Team {
98+
return false
99+
}
100+
}
101+
return true
102+
}
103+
104+
func (r *RobotCountMaintainer) isFreeOfObstacles(pos *geom.Vector2) bool {
105+
for _, robot := range r.c.lastTrackedFrame.TrackedFrame.Robots {
106+
if robot.Pos.DistanceTo(pos) < 0.2 {
107+
return false
108+
}
109+
}
110+
for _, ball := range r.c.lastTrackedFrame.TrackedFrame.Balls {
111+
pos2d := geom.NewVector2Float32(*ball.Pos.X, *ball.Pos.Y)
112+
if pos2d.DistanceTo(pos) < 0.1 {
113+
return false
114+
}
115+
}
116+
return true
117+
}
118+
119+
func (r *RobotCountMaintainer) sortRobotsByDistanceToSubstitutionPos(robots []*tracker.TrackedRobot) {
120+
negSubstPos := geom.NewVector2(0, -float64(*r.c.fieldSize.FieldWidth)/2000)
121+
posSubstPos := geom.NewVector2(0, +float64(*r.c.fieldSize.FieldWidth)/2000)
122+
sort.Slice(robots, func(i, j int) bool {
123+
distI := math.Min(robots[i].Pos.DistanceTo(negSubstPos), robots[i].Pos.DistanceTo(posSubstPos))
124+
distJ := math.Min(robots[j].Pos.DistanceTo(negSubstPos), robots[j].Pos.DistanceTo(posSubstPos))
125+
return distI < distJ
126+
})
127+
}
128+
129+
func (r *RobotCountMaintainer) removeRobot(id *referee.RobotId) {
130+
log.Printf("Remove robot %v", id)
131+
132+
present := false
133+
command := SimulatorCommand{
134+
Control: &SimulatorControl{
135+
TeleportRobot: []*TeleportRobot{
136+
{
137+
Id: id,
138+
Present: &present,
139+
},
140+
},
141+
},
142+
}
143+
144+
r.sendControlCommand(&command)
145+
}
146+
147+
func (r *RobotCountMaintainer) addRobot(id *referee.RobotId, pos *geom.Vector2) {
148+
log.Printf("Add robot %v @ %v", id, pos)
149+
150+
present := true
151+
orientation := float32(0)
152+
command := SimulatorCommand{
153+
Control: &SimulatorControl{
154+
TeleportRobot: []*TeleportRobot{
155+
{
156+
Id: id,
157+
X: pos.X,
158+
Y: pos.Y,
159+
Orientation: &orientation,
160+
Present: &present,
161+
},
162+
},
163+
},
164+
}
165+
166+
r.sendControlCommand(&command)
167+
}
168+
169+
func (r *RobotCountMaintainer) sendControlCommand(command *SimulatorCommand) {
170+
171+
if data, err := proto.Marshal(command); err != nil {
172+
log.Println("Could not marshal command: ", err)
173+
} else {
174+
r.c.simControlClient.Send(data)
175+
r.lastTimeSendCommand = time.Now()
176+
}
177+
}

‎internal/simctl/simctl.go‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -22,7 +22,8 @@ type SimulationController struct {
2222
lastRefereeMsg *referee.Referee
2323
fieldSize *vision.SSL_GeometryFieldSize
2424

25-
ballReplacer BallReplacer
25+
ballReplacer BallReplacer
26+
robotCountMaintainer RobotCountMaintainer
2627
}
2728

2829
func NewSimulationController(visionAddress, refereeAddress, trackerAddress string, simControlPort string) (c *SimulationController) {
@@ -32,6 +33,7 @@ func NewSimulationController(visionAddress, refereeAddress, trackerAddress strin
3233
c.trackerServer = sslnet.NewMulticastServer(trackerAddress, c.onNewTrackerData)
3334
c.simControlPort = simControlPort
3435
c.ballReplacer.c = c
36+
c.robotCountMaintainer.c = c
3537
return
3638
}
3739

@@ -98,6 +100,7 @@ func (c *SimulationController) handle() {
98100
}
99101

100102
c.ballReplacer.handleReplaceBall()
103+
c.robotCountMaintainer.handleRobotCount()
101104
}
102105

103106
func (c *SimulationController) Start() {

0 commit comments

Comments
 (0)