Configure car and camera at startup
This commit is contained in:
		@@ -2,6 +2,7 @@ package main
 | 
			
		||||
 | 
			
		||||
import (
 | 
			
		||||
	"flag"
 | 
			
		||||
	"fmt"
 | 
			
		||||
	"github.com/cyrilix/robocar-base/cli"
 | 
			
		||||
	events2 "github.com/cyrilix/robocar-protobuf/go/events"
 | 
			
		||||
	"github.com/cyrilix/robocar-simulator/pkg/events"
 | 
			
		||||
@@ -11,6 +12,8 @@ import (
 | 
			
		||||
	"github.com/golang/protobuf/proto"
 | 
			
		||||
	log "github.com/sirupsen/logrus"
 | 
			
		||||
	"os"
 | 
			
		||||
	"strconv"
 | 
			
		||||
	"strings"
 | 
			
		||||
)
 | 
			
		||||
 | 
			
		||||
const DefaultClientId = "robocar-simulator"
 | 
			
		||||
@@ -34,6 +37,40 @@ func main() {
 | 
			
		||||
	flag.StringVar(&address, "simulator-address", "127.0.0.1:9091", "Simulator address")
 | 
			
		||||
	flag.BoolVar(&debug, "debug", false, "Debug logs")
 | 
			
		||||
 | 
			
		||||
	var carName, carStyle, carColor string
 | 
			
		||||
	var carFontSize int
 | 
			
		||||
	carStyles := []string{
 | 
			
		||||
		string(simulator.CarConfigBodyStyleDonkey),
 | 
			
		||||
		string(simulator.CarConfigBodyStyleBare),
 | 
			
		||||
		string(simulator.CarConfigBodyStyleCar01),
 | 
			
		||||
	}
 | 
			
		||||
	flag.StringVar(&carName, "car-name", "simulator-gateway", "Car name to display")
 | 
			
		||||
	flag.StringVar(&carStyle, "car-style", string(simulator.CarConfigBodyStyleDonkey), fmt.Sprintf("Car style, only %s", strings.Join(carStyles, ",")))
 | 
			
		||||
	flag.StringVar(&carColor, "car-color", "0,0,0", "Color car as rgb value")
 | 
			
		||||
	flag.IntVar(&carFontSize, "car-font-size", 0, "Car font size")
 | 
			
		||||
 | 
			
		||||
	var racerName, racerBio, racerCountry,  racerGuid string
 | 
			
		||||
	flag.StringVar(&racerName, "racer-name", "", "")
 | 
			
		||||
	flag.StringVar(&racerBio, "racer-bio",      "", "")
 | 
			
		||||
	flag.StringVar(&racerCountry, "racer-country",   "", "")
 | 
			
		||||
	flag.StringVar(&racerGuid, "racer-guid",     "", "")
 | 
			
		||||
 | 
			
		||||
	var cameraFov, cameraImgW, cameraImgH, cameraImgD int
 | 
			
		||||
	var cameraFishEyeX, cameraFishEyeY float64
 | 
			
		||||
	var cameraOffsetX, cameraOffsetY, cameraOffsetZ, cameraRotX float64
 | 
			
		||||
	var cameraImgEnc string
 | 
			
		||||
	flag.IntVar(&cameraFov, "camera-fov", 90, "")
 | 
			
		||||
	flag.Float64Var(&cameraFishEyeX, "camera-fish-eye-x", 0.4, "")
 | 
			
		||||
	flag.Float64Var(&cameraFishEyeY, "camera-fish-eye-y", 0.7, "")
 | 
			
		||||
	flag.IntVar(&cameraImgW, "camera-img-w", 160, "image width")
 | 
			
		||||
	flag.IntVar(&cameraImgH, "camera-img-h", 120, "image height")
 | 
			
		||||
	flag.IntVar(&cameraImgD, "camera-img-d", 3, "Image depth")
 | 
			
		||||
	flag.StringVar(&cameraImgEnc, "camera-img-enc", string(simulator.CameraImageEncJpeg), "")
 | 
			
		||||
	flag.Float64Var(&cameraOffsetX, "camera-offset-x", 0, "moves camera left/right")
 | 
			
		||||
	flag.Float64Var(&cameraOffsetY, "camera-offset-y", 1.120395, "moves camera up/down")
 | 
			
		||||
	flag.Float64Var(&cameraOffsetZ, "camera-offset-z", 0.5528488, "moves camera forward/back")
 | 
			
		||||
	flag.Float64Var(&cameraRotX, "camera-rot-x", 15.0, "rotate the camera")
 | 
			
		||||
 | 
			
		||||
	flag.Parse()
 | 
			
		||||
	if len(os.Args) <= 1 {
 | 
			
		||||
		flag.PrintDefaults()
 | 
			
		||||
@@ -49,18 +86,41 @@ func main() {
 | 
			
		||||
	}
 | 
			
		||||
	defer client.Disconnect(10)
 | 
			
		||||
 | 
			
		||||
	bodyColors := strings.Split(carColor, ",")
 | 
			
		||||
	carConfig := simulator.CarConfigMsg{
 | 
			
		||||
		MsgType:   simulator.MsgTypeCarConfig,
 | 
			
		||||
		BodyStyle: simulator.CarConfigBodyStyleDonkey,
 | 
			
		||||
		BodyR:     "0",
 | 
			
		||||
		BodyG:     "0",
 | 
			
		||||
		BodyB:     "255",
 | 
			
		||||
		FontSize:  "0",
 | 
			
		||||
		BodyStyle: simulator.CarStyle(carStyle),
 | 
			
		||||
		BodyR:     bodyColors[0],
 | 
			
		||||
		BodyG:     bodyColors[1],
 | 
			
		||||
		BodyB:     bodyColors[2],
 | 
			
		||||
		CarName:   carName,
 | 
			
		||||
		FontSize:  strconv.Itoa(carFontSize),
 | 
			
		||||
	}
 | 
			
		||||
	gtw := gateway.New(address, &carConfig)
 | 
			
		||||
	racer := simulator.RacerBioMsg{
 | 
			
		||||
		MsgType:   simulator.MsgTypeRacerInfo,
 | 
			
		||||
		RacerName: racerName,
 | 
			
		||||
		CarName:   carName,
 | 
			
		||||
		Bio:       racerBio,
 | 
			
		||||
		Country:   racerCountry,
 | 
			
		||||
		Guid:      racerGuid,
 | 
			
		||||
	}
 | 
			
		||||
	camera := simulator.CamConfigMsg{
 | 
			
		||||
		MsgType:  simulator.MsgTypeCameraConfig,
 | 
			
		||||
		Fov:      strconv.Itoa(cameraFov),
 | 
			
		||||
		FishEyeX: fmt.Sprintf("%f", cameraFishEyeX),
 | 
			
		||||
		FishEyeY: fmt.Sprintf("%f",cameraFishEyeY),
 | 
			
		||||
		ImgW:     strconv.Itoa(cameraImgW),
 | 
			
		||||
		ImgH:     strconv.Itoa(cameraImgH),
 | 
			
		||||
		ImgD:     strconv.Itoa(cameraImgD),
 | 
			
		||||
		ImgEnc:   simulator.CameraImageEnc(cameraImgEnc),
 | 
			
		||||
		OffsetX:  fmt.Sprintf("%f",cameraOffsetX),
 | 
			
		||||
		OffsetY: fmt.Sprintf("%f", cameraOffsetY),
 | 
			
		||||
		OffsetZ: fmt.Sprintf("%f", cameraOffsetZ),
 | 
			
		||||
		RotX:    fmt.Sprintf("%f", cameraOffsetZ),
 | 
			
		||||
	}
 | 
			
		||||
	gtw := gateway.New(address, &carConfig, &racer, &camera)
 | 
			
		||||
	defer gtw.Stop()
 | 
			
		||||
 | 
			
		||||
 | 
			
		||||
	msgPub := events.NewMsgPublisher(
 | 
			
		||||
		gtw,
 | 
			
		||||
		events.NewMqttPublisher(client),
 | 
			
		||||
 
 | 
			
		||||
@@ -111,6 +111,10 @@ func (m *MsgPublisher) listenFrame() {
 | 
			
		||||
	msgChan := m.srcEvents.SubscribeFrame()
 | 
			
		||||
	for {
 | 
			
		||||
		msg := <-msgChan
 | 
			
		||||
		if msg == nil {
 | 
			
		||||
			// channel closed
 | 
			
		||||
			break
 | 
			
		||||
		}
 | 
			
		||||
		logr.Debugf("new frame %v", msg.Id)
 | 
			
		||||
		if m.topicFrame == "" {
 | 
			
		||||
			return
 | 
			
		||||
 
 | 
			
		||||
@@ -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)
 | 
			
		||||
		}
 | 
			
		||||
 
 | 
			
		||||
@@ -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
 | 
			
		||||
}
 | 
			
		||||
 
 | 
			
		||||
@@ -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")
 | 
			
		||||
 
 | 
			
		||||
@@ -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
 | 
			
		||||
}
 | 
			
		||||
 
 | 
			
		||||
@@ -1,18 +1,22 @@
 | 
			
		||||
package simulator
 | 
			
		||||
 | 
			
		||||
const(
 | 
			
		||||
	MsgTypeControl =  "control"
 | 
			
		||||
	MsgTypeTelemetry =  "telemetry"
 | 
			
		||||
	MsgTypeCarConfig =  "car_config"
 | 
			
		||||
	MsgTypeCarLoaded =  "car_loaded"
 | 
			
		||||
type MsgType string
 | 
			
		||||
 | 
			
		||||
const (
 | 
			
		||||
	MsgTypeControl      = MsgType("control")
 | 
			
		||||
	MsgTypeTelemetry    = MsgType("telemetry")
 | 
			
		||||
	MsgTypeCarConfig    = MsgType("car_config")
 | 
			
		||||
	MsgTypeCarLoaded    = MsgType("car_loaded")
 | 
			
		||||
	MsgTypeRacerInfo    = MsgType("racer_info")
 | 
			
		||||
	MsgTypeCameraConfig = MsgType("cam_config")
 | 
			
		||||
)
 | 
			
		||||
 | 
			
		||||
type Msg struct {
 | 
			
		||||
	MsgType string `json:"msg_type"`
 | 
			
		||||
	MsgType MsgType `json:"msg_type"`
 | 
			
		||||
}
 | 
			
		||||
 | 
			
		||||
type TelemetryMsg struct {
 | 
			
		||||
	MsgType string `json:"msg_type"`
 | 
			
		||||
	MsgType       MsgType `json:"msg_type"`
 | 
			
		||||
	SteeringAngle float64 `json:"steering_angle"`
 | 
			
		||||
	Throttle      float64 `json:"throttle"`
 | 
			
		||||
	Speed         float64 `json:"speed"`
 | 
			
		||||
@@ -27,40 +31,43 @@ type TelemetryMsg struct {
 | 
			
		||||
 | 
			
		||||
/* Json msg used to control cars. MsgType must be filled with "control" */
 | 
			
		||||
type ControlMsg struct {
 | 
			
		||||
	MsgType string `json:"msg_type"`
 | 
			
		||||
	Steering string `json:"steering"`
 | 
			
		||||
	Throttle string `json:"throttle"`
 | 
			
		||||
	Brake    string `json:"brake"`
 | 
			
		||||
	MsgType  MsgType `json:"msg_type"`
 | 
			
		||||
	Steering string  `json:"steering"`
 | 
			
		||||
	Throttle string  `json:"throttle"`
 | 
			
		||||
	Brake    string  `json:"brake"`
 | 
			
		||||
}
 | 
			
		||||
 | 
			
		||||
type GetSceneNamesMsg struct {
 | 
			
		||||
	MsgType string `json:"msg_type"`
 | 
			
		||||
	SceneName string `json:"scene_name"`
 | 
			
		||||
	MsgType   MsgType `json:"msg_type"`
 | 
			
		||||
	SceneName string  `json:"scene_name"`
 | 
			
		||||
}
 | 
			
		||||
 | 
			
		||||
type LoadSceneMsg struct {
 | 
			
		||||
	MsgType string `json:"msg_type"`
 | 
			
		||||
	SceneName string `json:"scene_name"`
 | 
			
		||||
	MsgType   MsgType `json:"msg_type"`
 | 
			
		||||
	SceneName string  `json:"scene_name"`
 | 
			
		||||
}
 | 
			
		||||
 | 
			
		||||
const(
 | 
			
		||||
	CarConfigBodyStyleDonkey = "donkey"
 | 
			
		||||
	CarConfigBodyStyleBare = "bare"
 | 
			
		||||
	CarConfigBodyStyleCar01 = "car01"
 | 
			
		||||
type CarStyle string
 | 
			
		||||
 | 
			
		||||
const (
 | 
			
		||||
	CarConfigBodyStyleDonkey = CarStyle("donkey")
 | 
			
		||||
	CarConfigBodyStyleBare   = CarStyle("bare")
 | 
			
		||||
	CarConfigBodyStyleCar01  = CarStyle("car01")
 | 
			
		||||
)
 | 
			
		||||
 | 
			
		||||
/*
 | 
			
		||||
	# body_style = "donkey" | "bare" | "car01" choice of string
 | 
			
		||||
	# body_rgb  = (128, 128, 128) tuple of ints
 | 
			
		||||
	# car_name = "string less than 64 char"
 | 
			
		||||
*/
 | 
			
		||||
type CarConfigMsg struct {
 | 
			
		||||
	MsgType string `json:"msg_type"`
 | 
			
		||||
	BodyStyle string `json:"body_style"`
 | 
			
		||||
	BodyR     string `json:"body_r"`
 | 
			
		||||
	BodyG     string `json:"body_g"`
 | 
			
		||||
	BodyB     string `json:"body_b"`
 | 
			
		||||
	CarName   string `json:"car_name"`
 | 
			
		||||
	FontSize  string `json:"font_size"`
 | 
			
		||||
	MsgType   MsgType  `json:"msg_type"`
 | 
			
		||||
	BodyStyle CarStyle `json:"body_style"`
 | 
			
		||||
	BodyR     string   `json:"body_r"`
 | 
			
		||||
	BodyG     string   `json:"body_g"`
 | 
			
		||||
	BodyB     string   `json:"body_b"`
 | 
			
		||||
	CarName   string   `json:"car_name"`
 | 
			
		||||
	FontSize  string   `json:"font_size"`
 | 
			
		||||
}
 | 
			
		||||
 | 
			
		||||
/*
 | 
			
		||||
@@ -68,14 +75,22 @@ type CarConfigMsg struct {
 | 
			
		||||
# guid = "some random string"
 | 
			
		||||
*/
 | 
			
		||||
type RacerBioMsg struct {
 | 
			
		||||
	MsgType string `json:"msg_type"`
 | 
			
		||||
	RacerName string `json:"racer_name"`
 | 
			
		||||
	CarName   string `json:"car_name"`
 | 
			
		||||
	Bio       string `json:"bio"`
 | 
			
		||||
	Country   string `json:"country"`
 | 
			
		||||
	Guid      string `json:"guid"`
 | 
			
		||||
	MsgType   MsgType `json:"msg_type"`
 | 
			
		||||
	RacerName string  `json:"racer_name"`
 | 
			
		||||
	CarName   string  `json:"car_name"`
 | 
			
		||||
	Bio       string  `json:"bio"`
 | 
			
		||||
	Country   string  `json:"country"`
 | 
			
		||||
	Guid      string  `json:"guid"`
 | 
			
		||||
}
 | 
			
		||||
 | 
			
		||||
type CameraImageEnc string
 | 
			
		||||
 | 
			
		||||
const (
 | 
			
		||||
	CameraImageEncJpeg = CameraImageEnc("JPG")
 | 
			
		||||
	CameraImageEncPng  = CameraImageEnc("PNG")
 | 
			
		||||
	CameraImageEncTga  = CameraImageEnc("TGA")
 | 
			
		||||
)
 | 
			
		||||
 | 
			
		||||
/* Camera config
 | 
			
		||||
set any field to Zero to get the default camera setting.
 | 
			
		||||
offset_x moves camera left/right
 | 
			
		||||
@@ -86,16 +101,16 @@ with fish_eye_x/y == 0.0 then you get no distortion
 | 
			
		||||
img_enc can be one of JPG|PNG|TGA
 | 
			
		||||
*/
 | 
			
		||||
type CamConfigMsg struct {
 | 
			
		||||
	MsgType string `json:"msg_type"`
 | 
			
		||||
	Fov      string `json:"fov"`
 | 
			
		||||
	FishEyeX string `json:"fish_eye_x"`
 | 
			
		||||
	FishEyeY string `json:"fish_eye_Y"`
 | 
			
		||||
	ImgW     string `json:"img_w"`
 | 
			
		||||
	ImgH     string `json:"img_h"`
 | 
			
		||||
	ImgD     string `json:"img_d"`
 | 
			
		||||
	ImgEnc   string `json:"img_enc"`
 | 
			
		||||
	OffsetX  string `json:"offset_x"`
 | 
			
		||||
	OffsetY  string `json:"offset_y"`
 | 
			
		||||
	OffsetZ  string `json:"offset_z"`
 | 
			
		||||
	RotX     string `json:"rot_x"`
 | 
			
		||||
	MsgType  MsgType        `json:"msg_type"`
 | 
			
		||||
	Fov      string         `json:"fov"`
 | 
			
		||||
	FishEyeX string         `json:"fish_eye_x"`
 | 
			
		||||
	FishEyeY string         `json:"fish_eye_Y"`
 | 
			
		||||
	ImgW     string         `json:"img_w"`
 | 
			
		||||
	ImgH     string         `json:"img_h"`
 | 
			
		||||
	ImgD     string         `json:"img_d"`
 | 
			
		||||
	ImgEnc   CameraImageEnc `json:"img_enc"`
 | 
			
		||||
	OffsetX  string         `json:"offset_x"`
 | 
			
		||||
	OffsetY  string         `json:"offset_y"`
 | 
			
		||||
	OffsetZ  string         `json:"offset_z"`
 | 
			
		||||
	RotX     string         `json:"rot_x"`
 | 
			
		||||
}
 | 
			
		||||
 
 | 
			
		||||
		Reference in New Issue
	
	Block a user