robocars/robocar-simulator
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Configure car and camera at startup

Browse Source
This commit is contained in:
Cyrille Nofficial
2021-02-16 17:46:10 +01:00
parent 3d81336c9b
commit 0fe45a3c9c
7 changed files with 423 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files

93
pkg/gateway/controls_test.go
View File

@ -4,8 +4,74 @@ import (
"github.com/cyrilix/robocar-protobuf/go/events"
"github.com/cyrilix/robocar-simulator/pkg/simulator"
"testing"
"time"
)
func TestGateway_Start(t *testing.T) {
simulatorMock := Gw2SimMock{}
err := simulatorMock.listen()
if err != nil {
t.Errorf("unable to start mock gw: %v", err)
}
defer func() {
if err := simulatorMock.Close(); err != nil {
t.Errorf("unable to stop simulator mock: %v", err)
}
}()
carConfig := simulator.CarConfigMsg{
MsgType: simulator.MsgTypeCarConfig,
BodyStyle: simulator.CarConfigBodyStyleCar01,
CarName: "car-test",
}
camConfig :=simulator.CamConfigMsg{
MsgType: simulator.MsgTypeCameraConfig,
Fov: "0",
FishEyeX: "0",
FishEyeY: "0",
ImgW: "120",
ImgH: "50",
ImgD: "0",
ImgEnc: simulator.CameraImageEncJpeg,
OffsetX: "0",
OffsetY: "0",
OffsetZ: "0",
RotX: "0",
}
gw := New(simulatorMock.Addr(),
&carConfig,
&simulator.RacerBioMsg{},
&camConfig,
)
if err != nil {
t.Fatalf("unable to init simulator gateway: %v", err)
}
go gw.Start()
defer gw.Close()
carNotify := simulatorMock.NotifyCar()
select {
case <- time.Tick(500 * time.Millisecond):
t.Errorf("a car configuration message should be send")
case msg := <-carNotify:
if *msg != carConfig {
t.Errorf("[carConfig] invalid car config: %v, wants %v", &msg, carConfig)
}
}
camNotify := simulatorMock.NotifyCamera()
select {
case <- time.Tick(500 * time.Millisecond):
t.Errorf("a camera configuration message should be send")
case msg := <-camNotify:
if *msg != camConfig {
t.Errorf("[carConfig] invalid camera config: %v, wants %v", &msg, camConfig)
}
}
}
func TestGateway_WriteSteering(t *testing.T) {
cases := []struct {
@ -64,19 +130,24 @@ func TestGateway_WriteSteering(t *testing.T) {
}
}()
gw := New(simulatorMock.Addr())
gw := New(simulatorMock.Addr(),
&simulator.CarConfigMsg{MsgType: simulator.MsgTypeCarConfig},
&simulator.RacerBioMsg{},
&simulator.CamConfigMsg{})
if err != nil {
t.Fatalf("unable to init simulator gateway: %v", err)
}
go gw.Start()
defer gw.Close()
//go gw.Start()
//defer gw.Close()
//<- simulatorMock.NotifyCar()
//<- simulatorMock.NotifyCamera()
for _, c := range cases {
gw.lastControl = c.previousMsg
gw.WriteSteering(c.msg)
ctrlMsg := <-simulatorMock.Notify()
ctrlMsg := <-simulatorMock.NotifyCtrl()
if *ctrlMsg != c.expectedMsg {
t.Errorf("[%v] bad messge received: %#v, wants %#v", c.name, ctrlMsg, c.expectedMsg)
}
@ -183,17 +254,21 @@ func TestGateway_WriteThrottle(t *testing.T) {
}
}()
gw := New(simulatorMock.Addr())
if err != nil {
t.Fatalf("unable to init simulator gateway: %v", err)
}
gw := New(simulatorMock.Addr(), &simulator.CarConfigMsg{MsgType: simulator.MsgTypeCarConfig},
&simulator.RacerBioMsg{},
&simulator.CamConfigMsg{})
//go gw.Start()
//<- simulatorMock.NotifyCar()
//<- simulatorMock.NotifyCamera()
for _, c := range cases {
gw.lastControl = c.previousMsg
gw.WriteThrottle(c.msg)
ctrlMsg := <-simulatorMock.Notify()
ctrlMsg := <-simulatorMock.NotifyCtrl()
if *ctrlMsg != c.expectedMsg {
t.Errorf("[%v] bad messge received: %#v, wants %#v", c.name, ctrlMsg, c.expectedMsg)
}

144
pkg/gateway/gateway.go
View File

@ -31,19 +31,23 @@ type ThrottleSource interface {
SubscribeThrottle() <-chan *events.ThrottleMessage
}
func New(addressSimulator string, car *simulator.CarConfigMsg) *Gateway {
func New(addressSimulator string, car *simulator.CarConfigMsg, racer *simulator.RacerBioMsg, camera *simulator.CamConfigMsg) *Gateway {
l := log.WithField("simulator", addressSimulator)
l.Info("run gateway from simulator")
return &Gateway{
address: addressSimulator,
log: l,
frameSubscribers: make(map[chan<- *events.FrameMessage]interface{}),
steeringSubscribers: make(map[chan<- *events.SteeringMessage]interface{}),
throttleSubscribers: make(map[chan<- *events.ThrottleMessage]interface{}),
address: addressSimulator,
log: l,
frameSubscribers: make(map[chan<- *events.FrameMessage]interface{}),
steeringSubscribers: make(map[chan<- *events.SteeringMessage]interface{}),
throttleSubscribers: make(map[chan<- *events.ThrottleMessage]interface{}),
telemetrySubscribers: make(map[chan *simulator.TelemetryMsg]interface{}),
carSubscribers: make(map[chan *simulator.Msg]interface{}),
carConfig: car,
carSubscribers: make(map[chan *simulator.Msg]interface{}),
racerSubscribers: make(map[chan *simulator.Msg]interface{}),
cameraSubscribers: make(map[chan *simulator.Msg]interface{}),
carConfig: car,
racer: racer,
cameraConfig: camera,
}
}
@ -54,8 +58,8 @@ type Gateway struct {
address string
muCommand sync.Mutex
muConn sync.Mutex
conn io.ReadWriteCloser
muConn sync.Mutex
conn io.ReadWriteCloser
muControl sync.Mutex
lastControl *simulator.ControlMsg
@ -67,9 +71,13 @@ type Gateway struct {
throttleSubscribers map[chan<- *events.ThrottleMessage]interface{}
telemetrySubscribers map[chan *simulator.TelemetryMsg]interface{}
carSubscribers map[chan *simulator.Msg]interface{}
carSubscribers map[chan *simulator.Msg]interface{}
racerSubscribers map[chan *simulator.Msg]interface{}
cameraSubscribers map[chan *simulator.Msg]interface{}
carConfig *simulator.CarConfigMsg
racer *simulator.RacerBioMsg
cameraConfig *simulator.CamConfigMsg
}
func (g *Gateway) Start() error {
@ -83,6 +91,14 @@ func (g *Gateway) Start() error {
if err != nil {
return fmt.Errorf("unable to configure car to server: %v", err)
}
err = g.writeRacerConfig()
if err != nil {
return fmt.Errorf("unable to configure racer to server: %v", err)
}
err = g.writeCameraConfig()
if err != nil {
return fmt.Errorf("unable to configure camera to server: %v", err)
}
for {
select {
@ -115,12 +131,6 @@ func (g *Gateway) Close() error {
for c := range g.throttleSubscribers {
close(c)
}
for c := range g.telemetrySubscribers {
close(c)
}
for c := range g.carSubscribers {
close(c)
}
if g.conn == nil {
g.log.Warn("no connection to close")
return nil
@ -192,8 +202,10 @@ func (g *Gateway) listen() error {
g.broadcastTelemetryMsg(rawLine)
case simulator.MsgTypeCarLoaded:
g.broadcastCarMsg(rawLine)
case simulator.MsgTypeRacerInfo:
g.broadcastRacerMsg(rawLine)
default:
log.Warnf("unmanaged simulator message: %v", rawLine)
log.Warnf("unmanaged simulator message: %v", string(rawLine))
}
}
}
@ -221,6 +233,27 @@ func (g *Gateway) broadcastCarMsg(rawLine []byte) {
}
}
func (g *Gateway) broadcastRacerMsg(rawLine []byte) {
for c := range g.racerSubscribers {
var tMsg simulator.Msg
err := json.Unmarshal(rawLine, &tMsg)
if err != nil {
g.log.Errorf("unable to unmarshal racer simulator msg '%v': %v", string(rawLine), err)
}
c <- &tMsg
}
}
func (g *Gateway) broadcastCameraMsg(rawLine []byte) {
for c := range g.cameraSubscribers {
var tMsg simulator.Msg
err := json.Unmarshal(rawLine, &tMsg)
if err != nil {
g.log.Errorf("unable to unmarshal camera simulator msg '%v': %v", string(rawLine), err)
}
c <- &tMsg
}
}
func (g *Gateway) publishFrame(msgSim *simulator.TelemetryMsg) *events.FrameRef {
now := time.Now()
frameRef := &events.FrameRef{
@ -237,12 +270,9 @@ func (g *Gateway) publishFrame(msgSim *simulator.TelemetryMsg) *events.FrameRef
}
log.Debugf("events frame '%v/%v'", msg.Id.Name, msg.Id.Id)
log.Infof("publish frame to %v receiver", len(g.frameSubscribers))
for fs := range g.frameSubscribers {
log.Info("publish frame")
fs <- msg
log.Info("frame published")
}
return frameRef
}
@ -308,6 +338,28 @@ func (g *Gateway) unsubscribeCarEvents(carChan chan *simulator.Msg) {
close(carChan)
}
func (g *Gateway) subscribeRacerEvents() chan *simulator.Msg {
racerChan := make(chan *simulator.Msg)
g.racerSubscribers[racerChan] = struct{}{}
return racerChan
}
func (g *Gateway) unsubscribeRacerEvents(racerChan chan *simulator.Msg) {
delete(g.racerSubscribers, racerChan)
close(racerChan)
}
func (g *Gateway) subscribeCameraEvents() chan *simulator.Msg {
cameraChan := make(chan *simulator.Msg)
g.cameraSubscribers[cameraChan] = struct{}{}
return cameraChan
}
func (g *Gateway) unsubscribeCameraEvents(cameraChan chan *simulator.Msg) {
delete(g.cameraSubscribers, cameraChan)
close(cameraChan)
}
var connect = func(address string) (io.ReadWriteCloser, error) {
conn, err := net.Dial("tcp", address)
if err != nil {
@ -404,7 +456,55 @@ func (g *Gateway) writeCarConfig() error {
return fmt.Errorf("unable to send car config to simulator: %v", err)
}
msg := <- carChan
msg := <-carChan
g.log.Infof("Car loaded: %v", msg)
return nil
}
func (g *Gateway) writeRacerConfig() error {
racerChan := g.subscribeRacerEvents()
defer g.unsubscribeRacerEvents(racerChan)
g.log.Info("Send racer configuration")
content, err := json.Marshal(g.racer)
if err != nil {
return fmt.Errorf("unable to marshall racer config msg \"%#v\": %v", g.lastControl, err)
}
err = g.writeCommand(content)
if err != nil {
return fmt.Errorf("unable to send racer config to simulator: %v", err)
}
select{
case msg := <-racerChan:
g.log.Infof("Racer loaded: %v", msg)
case <- time.Tick(25 * time.Millisecond):
}
return nil
}
func (g *Gateway) writeCameraConfig() error {
cameraChan := g.subscribeCameraEvents()
defer g.unsubscribeCameraEvents(cameraChan)
g.log.Info("Send camera configuration")
content, err := json.Marshal(g.cameraConfig)
if err != nil {
return fmt.Errorf("unable to marshall camera config msg \"%#v\": %v", g.lastControl, err)
}
err = g.writeCommand(content)
if err != nil {
return fmt.Errorf("unable to send camera config to simulator: %v", err)
}
select{
case msg := <-cameraChan:
g.log.Infof("Camera configured: %v", msg)
case <- time.Tick(25 * time.Millisecond):
}
return nil
}

10
pkg/gateway/gateway_test.go
View File

@ -2,7 +2,9 @@ package gateway
import (
"encoding/json"
"fmt"
"github.com/cyrilix/robocar-protobuf/go/events"
"github.com/cyrilix/robocar-simulator/pkg/simulator"
log "github.com/sirupsen/logrus"
"io/ioutil"
"strings"
@ -23,7 +25,11 @@ func TestGateway_ListenEvents(t *testing.T) {
}
}()
gw := New(simulatorMock.Addr())
gw := New(simulatorMock.Addr(),
&simulator.CarConfigMsg{MsgType: simulator.MsgTypeCarConfig},
&simulator.RacerBioMsg{},
&simulator.CamConfigMsg{},
)
go func() {
err := gw.Start()
if err != nil {
@ -40,6 +46,8 @@ func TestGateway_ListenEvents(t *testing.T) {
throttleChannel := gw.SubscribeThrottle()
simulatorMock.WaitConnection()
simulatorMock.EmitMsg(fmt.Sprintf("{\"msg_type\": \"%s\"}", simulator.MsgTypeCarLoaded))
log.Trace("read test data")
testContent, err := ioutil.ReadFile("testdata/msg.json")
lines := strings.Split(string(testContent), "\n")

94
pkg/gateway/util_test.go
View File

@ -12,16 +12,31 @@ import (
)
type Gw2SimMock struct {
initMsgsOnce sync.Once
initOnce sync.Once
ln net.Listener
notifyChan chan *simulator.ControlMsg
initNotifyChan sync.Once
notifyCtrlChan chan *simulator.ControlMsg
notifyCarChan chan *simulator.CarConfigMsg
notifyCamChan chan *simulator.CamConfigMsg
}
func (c *Gw2SimMock) Notify() <-chan *simulator.ControlMsg {
c.initNotifyChan.Do(func() { c.notifyChan = make(chan *simulator.ControlMsg) })
return c.notifyChan
func (c *Gw2SimMock) init(){
c.notifyCtrlChan = make(chan *simulator.ControlMsg)
c.notifyCarChan = make(chan *simulator.CarConfigMsg)
c.notifyCamChan = make(chan *simulator.CamConfigMsg)
}
func (c *Gw2SimMock) NotifyCtrl() <-chan *simulator.ControlMsg {
c.initOnce.Do(c.init)
return c.notifyCtrlChan
}
func (c *Gw2SimMock) NotifyCar() <-chan *simulator.CarConfigMsg {
c.initOnce.Do(c.init)
return c.notifyCarChan
}
func (c *Gw2SimMock) NotifyCamera() <-chan *simulator.CamConfigMsg {
c.initOnce.Do(c.init)
return c.notifyCamChan
}
func (c *Gw2SimMock) listen() error {
@ -50,10 +65,12 @@ func (c *Gw2SimMock) Addr() string {
}
func (c *Gw2SimMock) handleConnection(conn net.Conn) {
c.initNotifyChan.Do(func() { c.notifyChan = make(chan *simulator.ControlMsg) })
c.initOnce.Do(c.init)
reader := bufio.NewReader(conn)
writer := bufio.NewWriter(conn)
for {
rawCmd, err := reader.ReadBytes('\n')
rawMsg, err := reader.ReadBytes('\n')
if err != nil {
if err == io.EOF {
log.Debug("connection closed")
@ -62,15 +79,54 @@ func (c *Gw2SimMock) handleConnection(conn net.Conn) {
log.Errorf("unable to read request: %v", err)
return
}
var msg simulator.ControlMsg
err = json.Unmarshal(rawCmd, &msg)
var msg simulator.Msg
err = json.Unmarshal(rawMsg, &msg)
if err != nil {
log.Errorf("unable to unmarchal control msg \"%v\": %v", string(rawCmd), err)
log.Errorf("unable to unmarshal msg \"%v\": %v", string(rawMsg), err)
continue
}
c.notifyChan <- &msg
switch msg.MsgType {
case simulator.MsgTypeControl:
var msgControl simulator.ControlMsg
err = json.Unmarshal(rawMsg, &msgControl)
if err != nil {
log.Errorf("unable to unmarshal control msg \"%v\": %v", string(rawMsg), err)
continue
}
c.notifyCtrlChan <- &msgControl
case simulator.MsgTypeCarConfig:
var msgCar simulator.CarConfigMsg
err = json.Unmarshal(rawMsg, &msgCar)
if err != nil {
log.Errorf("unable to unmarshal car msg \"%v\": %v", string(rawMsg), err)
continue
}
c.notifyCarChan <- &msgCar
carLoadedMsg := simulator.Msg{MsgType: simulator.MsgTypeCarLoaded}
resp, err := json.Marshal(&carLoadedMsg)
if err != nil {
log.Errorf("unable to generate car loaded response: %v", err)
continue
}
_, err = writer.WriteString(string(resp) + "\n")
if err != nil {
log.Errorf("unable to write car loaded response: %v", err)
continue
}
err = writer.Flush()
if err != nil {
log.Errorf("unable to flush car loaded response: %v", err)
continue
}
case simulator.MsgTypeCameraConfig:
var msgCam simulator.CamConfigMsg
err = json.Unmarshal(rawMsg, &msgCam)
if err != nil {
log.Errorf("unable to unmarshal camera msg \"%v\": %v", string(rawMsg), err)
continue
}
c.notifyCamChan <- &msgCam
}
}
}
@ -80,8 +136,14 @@ func (c *Gw2SimMock) Close() error {
if err != nil {
return fmt.Errorf("unable to close mock server: %v", err)
}
if c.notifyChan != nil {
close(c.notifyChan)
if c.notifyCtrlChan != nil {
close(c.notifyCtrlChan)
}
if c.notifyCarChan != nil {
close(c.notifyCarChan)
}
if c.notifyCamChan != nil {
close(c.notifyCamChan)
}
return nil
}