Read robot specs from config and send it to simulator

Author: g3force

cmd/ssl-simulation-controller/main.go

@@ -12,10 +12,11 @@ var visionAddress = flag.String("visionAddress", "224.5.23.2:10006", "The addres
 var trackerAddress = flag.String("trackerAddress", "224.5.23.2:10010", "The address (ip+port) from which tracker packages are received")
 var refereeAddress = flag.String("refereeAddress", "224.5.23.1:10003", "The address (ip+port) from which referee packages are received")
 var simControlPort = flag.String("simControlPort", "10300", "The port to which simulation control packets are send")
+var robotSpecConfig = flag.String("robotSpecConfig", "config/robot-specs.yaml", "The robot specs config file")
 
 func main() {
 	flag.Parse()
-	ctl := simctl.NewSimulationController(*visionAddress, *refereeAddress, *trackerAddress, *simControlPort)
+	ctl := simctl.NewSimulationController(*visionAddress, *refereeAddress, *trackerAddress, *simControlPort, *robotSpecConfig)
 	ctl.Start()
 
 	signals := make(chan os.Signal, 1)

config/robot-specs.yaml

@@ -0,0 +1,29 @@
+robot-specs:
+  "TIGERs Mannheim":
+    radius: 0.09
+    height: 0.15
+    mass: 2
+    max_linear_kick_speed: 8
+    max_chip_kick_speed: 8
+    center_to_dribbler: 0.073
+    limits:
+      acc_speedup_absolute_max: 3
+      acc_speedup_angular_max: 50
+      acc_brake_absolute_max: 6
+      acc_brake_angular_max: 50
+      vel_absolute_max: 3
+      vel_angular_max: 20
+  "ER-Force":
+    radius: 0.09
+    height: 0.15
+    mass: 2
+    max_linear_kick_speed: 8
+    max_chip_kick_speed: 8
+    center_to_dribbler: 0.073
+    limits:
+      acc_speedup_absolute_max: 3
+      acc_speedup_angular_max: 50
+      acc_brake_absolute_max: 6
+      acc_brake_angular_max: 50
+      vel_absolute_max: 3
+      vel_angular_max: 20

go.mod

@@ -5,4 +5,5 @@ go 1.15
 require (
 	github.com/golang/protobuf v1.4.3
 	google.golang.org/protobuf v1.25.0
+	gopkg.in/yaml.v2 v2.4.0
 )

go.sum

@@ -63,5 +63,9 @@ google.golang.org/protobuf v1.23.0/go.mod h1:EGpADcykh3NcUnDUJcl1+ZksZNG86OlYog2
 google.golang.org/protobuf v1.23.1-0.20200526195155-81db48ad09cc/go.mod h1:EGpADcykh3NcUnDUJcl1+ZksZNG86OlYog2l/sGQquU=
 google.golang.org/protobuf v1.25.0 h1:Ejskq+SyPohKW+1uil0JJMtmHCgJPJ/qWTxr8qp+R4c=
 google.golang.org/protobuf v1.25.0/go.mod h1:9JNX74DMeImyA3h4bdi1ymwjUzf21/xIlbajtzgsN7c=
+gopkg.in/check.v1 v0.0.0-20161208181325-20d25e280405 h1:yhCVgyC4o1eVCa2tZl7eS0r+SDo693bJlVdllGtEeKM=
+gopkg.in/check.v1 v0.0.0-20161208181325-20d25e280405/go.mod h1:Co6ibVJAznAaIkqp8huTwlJQCZ016jof/cbN4VW5Yz0=
+gopkg.in/yaml.v2 v2.4.0 h1:D8xgwECY7CYvx+Y2n4sBz93Jn9JRvxdiyyo8CTfuKaY=
+gopkg.in/yaml.v2 v2.4.0/go.mod h1:RDklbk79AGWmwhnvt/jBztapEOGDOx6ZbXqjP6csGnQ=
 honnef.co/go/tools v0.0.0-20190102054323-c2f93a96b099/go.mod h1:rf3lG4BRIbNafJWhAfAdb/ePZxsR/4RtNHQocxwk9r4=
 honnef.co/go/tools v0.0.0-20190523083050-ea95bdfd59fc/go.mod h1:rf3lG4BRIbNafJWhAfAdb/ePZxsR/4RtNHQocxwk9r4=

internal/geom/ssl_gc_geometry.pb.go

@@ -0,0 +1,115 @@
+package simctl
+
+import (
+	"github.com/RoboCup-SSL/ssl-simulation-controller/internal/referee"
+	"github.com/golang/protobuf/proto"
+	"gopkg.in/yaml.v2"
+	"io/ioutil"
+	"log"
+)
+
+type TeamRobotSpecs struct {
+	Teams map[string]RobotSpec `yaml:"robot-specs"`
+}
+
+type RobotSpec struct {
+	Radius             float32 `yaml:"radius"`
+	Height             float32 `yaml:"height"`
+	Mass               float32 `yaml:"mass"`
+	MaxLinearKickSpeed float32 `yaml:"max_linear_kick_speed"`
+	MaxChipKickSpeed   float32 `yaml:"max_chip_kick_speed"`
+	CenterToDribbler   float32 `yaml:"center_to_dribbler"`
+	Limits             Limits  `yaml:"limits"`
+}
+
+type Limits struct {
+	AccSpeedupAbsoluteMax float32 `yaml:"acc_speedup_absolute_max,omitempty"`
+	AccSpeedupAngularMax  float32 `yaml:"acc_speedup_angular_max,omitempty"`
+	AccBrakeAbsoluteMax   float32 `yaml:"acc_brake_absolute_max,omitempty"`
+	AccBrakeAngularMax    float32 `yaml:"acc_brake_angular_max,omitempty"`
+	VelAbsoluteMax        float32 `yaml:"vel_absolute_max,omitempty"`
+	VelAngularMax         float32 `yaml:"vel_angular_max,omitempty"`
+}
+
+type RobotSpecSetter struct {
+	c *SimulationController
+
+	teamRobotSpecs TeamRobotSpecs
+	appliedTeams   map[referee.Team]string
+}
+
+func (r *RobotSpecSetter) LoadRobotSpecs(configFile string) {
+	data, err := ioutil.ReadFile(configFile)
+	if err != nil {
+		log.Println("Could not read robot spec file: ", err)
+	}
+	if err := yaml.Unmarshal(data, &r.teamRobotSpecs); err != nil {
+		log.Println("Could not unmarshal robot spec file: ", err)
+	}
+	r.appliedTeams = map[referee.Team]string{}
+}
+
+func (r *RobotSpecSetter) handleRobotSpecs() {
+	if *r.c.lastRefereeMsg.Command != referee.Referee_HALT {
+		// Only during HALT
+		return
+	}
+
+	r.updateTeam(referee.Team_BLUE, *r.c.lastRefereeMsg.Blue.Name)
+	r.updateTeam(referee.Team_YELLOW, *r.c.lastRefereeMsg.Yellow.Name)
+}
+
+func (r *RobotSpecSetter) updateTeam(team referee.Team, teamName string) {
+	if r.appliedTeams[team] != teamName {
+		if spec, ok := r.teamRobotSpecs.Teams[teamName]; ok {
+			protoSpec := mapRobotSpec(spec)
+			protoSpec.Id = new(referee.RobotId)
+			protoSpec.Id.Id = new(uint32)
+			protoSpec.Id.Team = new(referee.Team)
+			*protoSpec.Id.Id = 0
+			*protoSpec.Id.Team = team
+			r.sendConfig(protoSpec)
+			r.appliedTeams[team] = teamName
+		}
+	}
+}
+
+func (r *RobotSpecSetter) sendConfig(robotSpec *RobotSpecs) {
+	log.Printf("Sending robot spec %v", robotSpec)
+
+	command := SimulatorCommand{
+		Config: &SimulatorConfig{
+			RobotSpecs: []*RobotSpecs{
+				robotSpec,
+			}},
+	}
+
+	if data, err := proto.Marshal(&command); err != nil {
+		log.Println("Could not marshal command: ", err)
+	} else {
+		r.c.simControlClient.Send(data)
+	}
+}
+
+func mapRobotSpec(spec RobotSpec) (protoSpec *RobotSpecs) {
+	protoSpec = new(RobotSpecs)
+	protoSpec.Radius = &spec.Radius
+	protoSpec.Height = &spec.Height
+	protoSpec.Mass = &spec.Mass
+	protoSpec.MaxLinearKickSpeed = &spec.MaxLinearKickSpeed
+	protoSpec.MaxChipKickSpeed = &spec.MaxChipKickSpeed
+	protoSpec.CenterToDribbler = &spec.CenterToDribbler
+	protoSpec.Limits = mapRobotLimits(spec.Limits)
+	return
+}
+
+func mapRobotLimits(limits Limits) (protoLimits *RobotLimits) {
+	protoLimits = new(RobotLimits)
+	protoLimits.AccSpeedupAbsoluteMax = &limits.AccSpeedupAbsoluteMax
+	protoLimits.AccSpeedupAngularMax = &limits.AccSpeedupAngularMax
+	protoLimits.AccBrakeAbsoluteMax = &limits.AccBrakeAbsoluteMax
+	protoLimits.AccBrakeAngularMax = &limits.AccBrakeAngularMax
+	protoLimits.VelAbsoluteMax = &limits.VelAbsoluteMax
+	protoLimits.VelAngularMax = &limits.VelAngularMax
+	return
+}

internal/referee/ssl_gc_common.pb.go

@@ -24,16 +24,19 @@ type SimulationController struct {
 
 	ballReplacer         BallReplacer
 	robotCountMaintainer RobotCountMaintainer
+	robotSpecsSetter     RobotSpecSetter
 }
 
-func NewSimulationController(visionAddress, refereeAddress, trackerAddress string, simControlPort string) (c *SimulationController) {
+func NewSimulationController(visionAddress, refereeAddress, trackerAddress, simControlPort, robotSpecConfig string) (c *SimulationController) {
 	c = new(SimulationController)
 	c.visionServer = sslnet.NewMulticastServer(visionAddress, c.onNewVisionData)
 	c.refereeServer = sslnet.NewMulticastServer(refereeAddress, c.onNewRefereeData)
 	c.trackerServer = sslnet.NewMulticastServer(trackerAddress, c.onNewTrackerData)
 	c.simControlPort = simControlPort
 	c.ballReplacer.c = c
 	c.robotCountMaintainer.c = c
+	c.robotSpecsSetter.c = c
+	c.robotSpecsSetter.LoadRobotSpecs(robotSpecConfig)
 	return
 }
 
@@ -101,6 +104,7 @@ func (c *SimulationController) handle() {
 
 	c.ballReplacer.handleReplaceBall()
 	c.robotCountMaintainer.handleRobotCount()
+	c.robotSpecsSetter.handleRobotSpecs()
 }
 
 func (c *SimulationController) Start() {