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

4
pkg/events/events.go
View File

@ -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

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
}

103
pkg/simulator/simulator.go
View File

@ -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"`
}