Implements Gateway for car controls

camera/camera.go

@@ -17,28 +17,31 @@ import (
 )
 
 func New(publisher Publisher, addressSimulator string) *Gateway {
-	log.Info("run camera camera")
+	l := log.WithField("simulator", addressSimulator)
+	l.Info("run camera from simulator")
 
 	return &Gateway{
-		address:          addressSimulator,
+		address:   addressSimulator,
-		publisher:        publisher,
+		publisher: publisher,
+		log:       l,
 	}
 }
 
 /* Simulator interface to publish camera frames into mqtt topic */
 type Gateway struct {
-	cancel           chan interface{}
+	cancel chan interface{}
 
 	address string
 	conn    io.ReadCloser
 
 	publisher Publisher
+	log       *log.Entry
 }
 
 func (p *Gateway) Start() error {
-	log.Info("connect to simulator")
+	p.log.Info("connect to simulator")
 	p.cancel = make(chan interface{})
-	msgChan := make(chan *simulator.SimulatorMsg)
+	msgChan := make(chan *simulator.TelemetryMsg)
 
 	go p.run(msgChan)
 
@@ -53,17 +56,17 @@ func (p *Gateway) Start() error {
 }
 
 func (p *Gateway) Stop() {
-	log.Info("close simulator gateway")
+	p.log.Info("close simulator gateway")
 	close(p.cancel)
 
 	if err := p.Close(); err != nil {
-		log.Printf("unexpected error while simulator connection is closed: %v", err)
+		p.log.Warnf("unexpected error while simulator connection is closed: %v", err)
 	}
 }
 
 func (p *Gateway) Close() error {
 	if p.conn == nil {
-		log.Warnln("no connection to close")
+		p.log.Warn("no connection to close")
 		return nil
 	}
 	if err := p.conn.Close(); err != nil {
@@ -72,19 +75,21 @@ func (p *Gateway) Close() error {
 	return nil
 }
 
-func (p *Gateway) run(msgChan chan<- *simulator.SimulatorMsg) {
+func (p *Gateway) run(msgChan chan<- *simulator.TelemetryMsg) {
 	err := retry.Do(func() error {
+		p.log.Info("connect to simulator")
 		conn, err := connect(p.address)
 		if err != nil {
 			return fmt.Errorf("unable to connect to simulator at %v", p.address)
 		}
 		p.conn = conn
+		p.log.Info("connection success")
 		return nil
 	},
 		retry.Delay(1*time.Second),
 	)
 	if err != nil {
-		log.Panicf("unable to connect to simulator: %v", err)
+		p.log.Panicf("unable to connect to simulator: %v", err)
 	}
 
 	reader := bufio.NewReader(p.conn)
@@ -93,25 +98,25 @@ func (p *Gateway) run(msgChan chan<- *simulator.SimulatorMsg) {
 		func() error { return p.listen(msgChan, reader) },
 	)
 	if err != nil {
-		log.Errorf("unable to connect to server: %v", err)
+		p.log.Errorf("unable to connect to server: %v", err)
 	}
 }
 
-func (p *Gateway) listen(msgChan chan<- *simulator.SimulatorMsg, reader *bufio.Reader) error {
+func (p *Gateway) listen(msgChan chan<- *simulator.TelemetryMsg, reader *bufio.Reader) error {
 	for {
 		rawLine, err := reader.ReadBytes('\n')
 		if err == io.EOF {
-			log.Info("Connection closed")
+			p.log.Info("Connection closed")
 			return err
 		}
 		if err != nil {
 			return fmt.Errorf("unable to read response: %v", err)
 		}
 
-		var msg simulator.SimulatorMsg
+		var msg simulator.TelemetryMsg
 		err = json.Unmarshal(rawLine, &msg)
 		if err != nil {
-			log.Errorf("unable to unmarshal simulator msg: %v", err)
+			p.log.Errorf("unable to unmarshal simulator msg: %v", err)
 		}
 		if "telemetry" != msg.MsgType {
 			continue
@@ -120,7 +125,7 @@ func (p *Gateway) listen(msgChan chan<- *simulator.SimulatorMsg, reader *bufio.R
 	}
 }
 
-func (p *Gateway) publishFrame(msgSim *simulator.SimulatorMsg) {
+func (p *Gateway) publishFrame(msgSim *simulator.TelemetryMsg) {
 	now := time.Now()
 	msg := &events.FrameMessage{
 		Id: &events.FrameRef{
@@ -136,9 +141,9 @@ func (p *Gateway) publishFrame(msgSim *simulator.SimulatorMsg) {
 
 	payload, err := proto.Marshal(msg)
 	if err != nil {
-		log.Errorf("unable to marshal protobuf message: %v", err)
+		p.log.Errorf("unable to marshal protobuf message: %v", err)
 	}
-	p.publisher.Publish(&payload)
+	p.publisher.Publish(payload)
 }
 
 var connect = func(address string) (io.ReadWriteCloser, error) {
@@ -150,23 +155,23 @@ var connect = func(address string) (io.ReadWriteCloser, error) {
 }
 
 type Publisher interface {
-	Publish(payload *[]byte)
+	Publish(payload []byte)
 }
 
 func NewMqttPublisher(client mqtt.Client, topic string) Publisher {
 	return &MqttPublisher{
 		client: client,
-		topic: topic,
+		topic:  topic,
 	}
 }
 
 type MqttPublisher struct {
 	client mqtt.Client
-	topic string
+	topic  string
 }
 
-func (m *MqttPublisher) Publish(payload *[]byte) {
+func (m *MqttPublisher) Publish(payload []byte) {
-	token := m.client.Publish(m.topic, 0, false, *payload)
+	token := m.client.Publish(m.topic, 0, false, payload)
 	token.WaitTimeout(10 * time.Millisecond)
 	if err := token.Error(); err != nil {
 		log.Errorf("unable to publish frame: %v", err)

camera/camera_test.go

@@ -12,44 +12,44 @@ import (
 	"strings"
 	"sync"
 	"testing"
-	"time"
 )
 
 type MockPublisher struct {
-	muPubEvents     sync.Mutex
+	notifyChan     chan []byte
-	publishedEvents []*[]byte
+	initNotifyChan sync.Once
 }
 
-func (p *MockPublisher) Publish(payload *[]byte) {
+func (p *MockPublisher) Close() error {
-	p.muPubEvents.Lock()
+	if p.notifyChan != nil {
-	defer p.muPubEvents.Unlock()
+		close(p.notifyChan)
-	p.publishedEvents = append(p.publishedEvents, payload)
+	}
+	return nil
 }
 
-func (p *MockPublisher) Events() []*[]byte {
+func (p *MockPublisher) Publish(payload []byte) {
-	p.muPubEvents.Lock()
+	p.notifyChan <- payload
-	defer p.muPubEvents.Unlock()
+}
-	eventsMsg := make([]*[]byte, len(p.publishedEvents), len(p.publishedEvents))
+
-	copy(eventsMsg, p.publishedEvents)
+func (p *MockPublisher) Notify() <-chan []byte {
-	return eventsMsg
+	p.initNotifyChan.Do(func() { p.notifyChan = make(chan []byte) })
+	return p.notifyChan
 }
 
 func TestPart_ListenEvents(t *testing.T) {
-	connMock := ConnMock{}
+	simulatorMock := SimulatorMock{}
-	err := connMock.Listen()
+	err := simulatorMock.Start()
 	if err != nil {
 		t.Errorf("unable to start mock server: %v", err)
 	}
 	defer func() {
-		if err := connMock.Close(); err != nil {
+		if err := simulatorMock.Close(); err != nil {
 			t.Errorf("unable to close mock server: %v", err)
 		}
 	}()
 
+	publisher := MockPublisher{}
 
-	publisher := MockPublisher{publishedEvents: make([]*[]byte, 0)}
+	part := New(&publisher, simulatorMock.Addr())
-
-	part := New(&publisher, connMock.Addr())
 	go func() {
 		err := part.Start()
 		if err != nil {
@@ -62,25 +62,20 @@ func TestPart_ListenEvents(t *testing.T) {
 		}
 	}()
 
+	simulatorMock.WaitConnection()
+	log.Trace("read test data")
 	testContent, err := ioutil.ReadFile("testdata/msg.json")
 	lines := strings.Split(string(testContent), "\n")
 
 	for idx, line := range lines {
-		time.Sleep(5 * time.Millisecond)
+		err = simulatorMock.EmitMsg(line)
-		err = connMock.WriteMsg(line)
 		if err != nil {
 			t.Errorf("[line %v/%v] unable to write line: %v", idx+1, len(lines), err)
 		}
-	}
 
-	time.Sleep(10 * time.Millisecond)
+		byteMsg := <-publisher.Notify()
-	expectedFrames := 16
-	if len(publisher.Events()) != expectedFrames {
-		t.Errorf("invalid number of frame emmitted: %v, wants %v", len(publisher.Events()), expectedFrames)
-	}
-	for _, byteMsg := range publisher.Events() {
 		var msg events.FrameMessage
-		err = proto.Unmarshal(*byteMsg, &msg)
+		err = proto.Unmarshal(byteMsg, &msg)
 		if err != nil {
 			t.Errorf("unable to unmarshal frame msg: %v", err)
 			continue
@@ -94,57 +89,80 @@ func TestPart_ListenEvents(t *testing.T) {
 	}
 }
 
-type ConnMock struct {
+type SimulatorMock struct {
-	ln     net.Listener
+	ln            net.Listener
-	conn   net.Conn
+	muConn        sync.Mutex
-	muWriter sync.Mutex
+	conn          net.Conn
-	writer *bufio.Writer
+	writer        *bufio.Writer
+	newConnection chan net.Conn
+	logger 		  *log.Entry
 }
 
-func (c *ConnMock) WriteMsg(p string) (err error) {
+func (c *SimulatorMock) EmitMsg(p string) (err error) {
-	c.muWriter.Lock()
+	c.muConn.Lock()
-	defer c.muWriter.Unlock()
+	defer c.muConn.Unlock()
 	_, err = c.writer.WriteString(p + "\n")
 	if err != nil {
-		log.Errorf("unable to write response: %v", err)
+		c.logger.Errorf("unable to write response: %v", err)
 	}
 	if err == io.EOF {
-		log.Info("Connection closed")
+		c.logger.Info("Connection closed")
 		return err
 	}
 	err = c.writer.Flush()
 	return err
 }
 
-func (c *ConnMock) Listen() error {
+func (c *SimulatorMock) WaitConnection() {
-	c.muWriter.Lock()
+	c.muConn.Lock()
-	defer c.muWriter.Unlock()
+	defer c.muConn.Unlock()
+	c.logger.Debug("simulator waiting connection")
+	if c.conn != nil {
+		return
+	}
+	c.logger.Debug("new connection")
+	conn := <-c.newConnection
+
+	c.conn = conn
+	c.writer = bufio.NewWriter(conn)
+}
+
+func (c *SimulatorMock) Start() error {
+	c.logger = log.WithField("simulator", "mock")
+	c.newConnection = make(chan net.Conn)
 	ln, err := net.Listen("tcp", "127.0.0.1:")
 	c.ln = ln
 	if err != nil {
-
 		return fmt.Errorf("unable to listen on port: %v", err)
 	}
-
 	go func() {
 		for {
-			c.conn, err = c.ln.Accept()
+			conn, err := c.ln.Accept()
 			if err != nil && err == io.EOF {
-				log.Infof("connection close: %v", err)
+				c.logger.Errorf("connection close: %v", err)
 				break
 			}
-			c.writer = bufio.NewWriter(c.conn)
+			if c.newConnection == nil {
+				break
+			}
+			c.newConnection <- conn
 		}
 	}()
 	return nil
 }
 
-func (c *ConnMock) Addr() string {
+func (c *SimulatorMock) Addr() string {
 	return c.ln.Addr().String()
 }
 
-func (c *ConnMock) Close() error {
+func (c *SimulatorMock) Close() error {
-	log.Infof("close mock server")
+	c.logger.Debug("close mock server")
+
+	if c == nil {
+		return nil
+	}
+	close(c.newConnection)
+	c.newConnection = nil
 	err := c.ln.Close()
 	if err != nil {
 		return fmt.Errorf("unable to close mock server: %v", err)

controls/controls.go

@@ -0,0 +1,123 @@
+package controls
+
+import (
+	"bufio"
+	"encoding/json"
+	"fmt"
+	"github.com/cyrilix/robocar-protobuf/go/events"
+	"github.com/cyrilix/robocar-simulator/simulator"
+	log "github.com/sirupsen/logrus"
+	"io"
+	"net"
+	"sync"
+)
+
+type SteeringController interface {
+ 	WriteSteering(message *events.SteeringMessage)
+}
+
+type ThrottleController interface {
+	WriteThrottle(message *events.ThrottleMessage)
+}
+
+func New(address string) (*Gateway, error) {
+	conn, err := net.Dial("tcp", address)
+	if err != nil {
+		return nil, fmt.Errorf("unable to connect to %v", address)
+	}
+	return &Gateway{
+		conn:  conn,
+	}, nil
+}
+
+/* Simulator interface to publish command controls from mqtt topic */
+type Gateway struct {
+	cancel           chan interface{}
+
+	muControl   sync.Mutex
+	lastControl *simulator.ControlMsg
+	conn        io.WriteCloser
+}
+
+func (g *Gateway) Start() {
+	log.Info("connect to simulator")
+	g.cancel = make(chan interface{})
+}
+
+func (g *Gateway) Stop() {
+	log.Info("close simulator gateway")
+	close(g.cancel)
+
+	if err := g.Close(); err != nil {
+		log.Printf("unexpected error while simulator connection is closed: %v", err)
+	}
+}
+
+func (g *Gateway) Close() error {
+	if g.conn == nil {
+		log.Warnln("no connection to close")
+		return nil
+	}
+	if err := g.conn.Close(); err != nil {
+		return fmt.Errorf("unable to close connection to simulator: %v", err)
+	}
+	return nil
+}
+
+func (g *Gateway) WriteSteering(message *events.SteeringMessage) {
+	g.muControl.Lock()
+	defer g.muControl.Unlock()
+	g.initLastControlMsg()
+
+	g.lastControl.Steering = message.Steering
+	g.writeContent()
+}
+
+func (g *Gateway) WriteThrottle(message *events.ThrottleMessage) {
+	g.muControl.Lock()
+	defer g.muControl.Unlock()
+	g.initLastControlMsg()
+
+	if message.Throttle > 0 {
+		g.lastControl.Throttle = message.Throttle
+		g.lastControl.Brake = 0.
+	} else {
+		g.lastControl.Throttle = 0.
+		g.lastControl.Brake = -1 * message.Throttle
+	}
+
+	g.writeContent()
+}
+
+func (g *Gateway) writeContent() {
+	w := bufio.NewWriter(g.conn)
+	content, err := json.Marshal(g.lastControl)
+	if err != nil {
+		log.Errorf("unable to marshall control msg \"%#v\": %v", g.lastControl, err)
+		return
+	}
+
+	_, err = w.Write(append(content, '\n'))
+	if err != nil {
+		log.Errorf("unable to write control msg \"%#v\" to simulator: %v", g.lastControl, err)
+		return
+	}
+	err = w.Flush()
+	if err != nil {
+		log.Errorf("unable to flush control msg \"%#v\" to simulator: %v", g.lastControl, err)
+		return
+	}
+}
+
+func (g *Gateway) initLastControlMsg() {
+	if g.lastControl != nil {
+		return
+	}
+	g.lastControl = &simulator.ControlMsg{
+		MsgType:  "control",
+		Steering: 0.,
+		Throttle: 0.,
+		Brake:    0.,
+	}
+}
+

controls/controls_test.go

@@ -0,0 +1,281 @@
+package controls
+
+import (
+	"bufio"
+	"encoding/json"
+	"fmt"
+	"github.com/cyrilix/robocar-protobuf/go/events"
+	"github.com/cyrilix/robocar-simulator/simulator"
+	log "github.com/sirupsen/logrus"
+	"io"
+	"net"
+	"sync"
+	"testing"
+)
+
+func TestGateway_WriteSteering(t *testing.T) {
+
+	cases := []struct {
+		name        string
+		msg         *events.SteeringMessage
+		previousMsg *simulator.ControlMsg
+		expectedMsg simulator.ControlMsg
+	}{
+		{"First Message",
+			&events.SteeringMessage{Steering: 0.5, Confidence: 1},
+			nil,
+			simulator.ControlMsg{
+				MsgType:  "control",
+				Steering: 0.5,
+				Throttle: 0,
+				Brake:    0,
+			}},
+		{"Update steering",
+			&events.SteeringMessage{Steering: -0.5, Confidence: 1},
+			&simulator.ControlMsg{
+				MsgType:  "control",
+				Steering: 0.2,
+				Throttle: 0,
+				Brake:    0,
+			},
+			simulator.ControlMsg{
+				MsgType:  "control",
+				Steering: -0.5,
+				Throttle: 0,
+				Brake:    0,
+			}},
+		{"Update steering shouldn't erase throttle value",
+			&events.SteeringMessage{Steering: -0.3, Confidence: 1},
+			&simulator.ControlMsg{
+				MsgType:  "control",
+				Steering: 0.2,
+				Throttle: 0.6,
+				Brake:    0.1,
+			},
+			simulator.ControlMsg{
+				MsgType:  "control",
+				Steering: -0.3,
+				Throttle: 0.6,
+				Brake:    0.1,
+			}},
+	}
+
+	simulatorMock := ConnMock{}
+	err := simulatorMock.listen()
+	if err != nil {
+		t.Errorf("unable to start mock server: %v", err)
+	}
+	defer func(){
+		if err := simulatorMock.Close(); err != nil {
+			t.Errorf("unable to stop simulator mock: %v", err)
+		}
+	}()
+
+	server, err := New(simulatorMock.Addr())
+	if err != nil {
+		t.Fatalf("unable to init simulator gateway: %v", err)
+	}
+
+	for _, c := range cases {
+		server.lastControl = c.previousMsg
+
+		server.WriteSteering(c.msg)
+
+		ctrlMsg := <- simulatorMock.Notify()
+		if *ctrlMsg != c.expectedMsg {
+			t.Errorf("[%v] bad messge received: %#v, wants %#v", c.name, ctrlMsg, c.expectedMsg)
+		}
+	}
+}
+
+func TestGateway_WriteThrottle(t *testing.T) {
+
+	cases := []struct {
+		name        string
+		msg         *events.ThrottleMessage
+		previousMsg *simulator.ControlMsg
+		expectedMsg simulator.ControlMsg
+	}{
+		{"First Message",
+			&events.ThrottleMessage{Throttle: 0.5, Confidence: 1},
+			nil,
+			simulator.ControlMsg{
+				MsgType:  "control",
+				Steering: 0,
+				Throttle: 0.5,
+				Brake:    0,
+			}},
+		{"Update Throttle",
+			&events.ThrottleMessage{Throttle: 0.6, Confidence: 1},
+			&simulator.ControlMsg{
+				MsgType:  "control",
+				Steering: 0,
+				Throttle: 0.4,
+				Brake:    0,
+			},
+			simulator.ControlMsg{
+				MsgType:  "control",
+				Steering: 0,
+				Throttle: 0.6,
+				Brake:    0,
+			}},
+		{"Update steering shouldn't erase throttle value",
+			&events.ThrottleMessage{Throttle: 0.3, Confidence: 1},
+			&simulator.ControlMsg{
+				MsgType:  "control",
+				Steering: 0.2,
+				Throttle: 0.6,
+				Brake:    0.,
+			},
+			simulator.ControlMsg{
+				MsgType:  "control",
+				Steering: 0.2,
+				Throttle: 0.3,
+				Brake:    0.,
+			}},
+		{"Throttle to brake",
+			&events.ThrottleMessage{Throttle: -0.7, Confidence: 1},
+			&simulator.ControlMsg{
+				MsgType:  "control",
+				Steering: 0.2,
+				Throttle: 0.6,
+				Brake:    0.,
+			},
+			simulator.ControlMsg{
+				MsgType:  "control",
+				Steering: 0.2,
+				Throttle: 0.,
+				Brake:    0.7,
+			}},
+		{"Update brake",
+			&events.ThrottleMessage{Throttle: -0.2, Confidence: 1},
+			&simulator.ControlMsg{
+				MsgType:  "control",
+				Steering: 0.2,
+				Throttle: 0.,
+				Brake:    0.5,
+			},
+			simulator.ControlMsg{
+				MsgType:  "control",
+				Steering: 0.2,
+				Throttle: 0.,
+				Brake:    0.2,
+			}},
+		{"Brake to throttle",
+			&events.ThrottleMessage{Throttle: 0.9, Confidence: 1},
+			&simulator.ControlMsg{
+				MsgType:  "control",
+				Steering: 0.2,
+				Throttle: 0.,
+				Brake:    0.4,
+			},
+			simulator.ControlMsg{
+				MsgType:  "control",
+				Steering: 0.2,
+				Throttle: 0.9,
+				Brake:    0.,
+			}},
+	}
+
+	simulatorMock := ConnMock{}
+	err := simulatorMock.listen()
+	if err != nil {
+		t.Errorf("unable to start mock server: %v", err)
+	}
+	defer func(){
+		if err := simulatorMock.Close(); err != nil {
+			t.Errorf("unable to stop simulator mock: %v", err)
+		}
+	}()
+
+	server, err := New(simulatorMock.Addr())
+	if err != nil {
+		t.Fatalf("unable to init simulator gateway: %v", err)
+	}
+
+	for _, c := range cases {
+		server.lastControl = c.previousMsg
+
+		server.WriteThrottle(c.msg)
+
+		ctrlMsg := <- simulatorMock.Notify()
+		if *ctrlMsg != c.expectedMsg {
+			t.Errorf("[%v] bad messge received: %#v, wants %#v", c.name, ctrlMsg, c.expectedMsg)
+		}
+	}
+}
+
+type ConnMock struct {
+	initMsgsOnce sync.Once
+
+	ln             net.Listener
+	notifyChan     chan *simulator.ControlMsg
+	initNotifyChan sync.Once
+}
+
+func (c *ConnMock) Notify() <-chan *simulator.ControlMsg {
+	c.initNotifyChan.Do(func() { c.notifyChan = make(chan *simulator.ControlMsg) })
+	return c.notifyChan
+}
+
+func (c *ConnMock) listen() error {
+	ln, err := net.Listen("tcp", "127.0.0.1:")
+	c.ln = ln
+	if err != nil {
+
+		return fmt.Errorf("unable to listen on port: %v", err)
+	}
+
+	go func() {
+		for {
+			conn, err := c.ln.Accept()
+			if err != nil {
+				log.Infof("connection close: %v", err)
+				break
+			}
+			go c.handleConnection(conn)
+		}
+	}()
+	return nil
+}
+
+func (c *ConnMock) Addr() string {
+	return c.ln.Addr().String()
+}
+
+func (c *ConnMock) handleConnection(conn net.Conn) {
+	c.initNotifyChan.Do(func() { c.notifyChan = make(chan *simulator.ControlMsg) })
+	reader := bufio.NewReader(conn)
+	for {
+		rawCmd, err := reader.ReadBytes('\n')
+		if err != nil {
+			if err == io.EOF {
+				log.Info("connection closed")
+				break
+			}
+			log.Errorf("unable to read request: %v", err)
+			return
+		}
+
+		var msg simulator.ControlMsg
+		err = json.Unmarshal(rawCmd, &msg)
+		if err != nil {
+			log.Errorf("unable to unmarchal control msg \"%v\": %v", string(rawCmd), err)
+			continue
+		}
+
+		c.notifyChan <- &msg
+	}
+}
+
+func (c *ConnMock) Close() error {
+	log.Infof("close mock server")
+	err := c.ln.Close()
+	if err != nil {
+		return fmt.Errorf("unable to close mock server: %v", err)
+	}
+	if c.notifyChan != nil {
+		close(c.notifyChan)
+	}
+	return nil
+}

simulator/simulator.go.go

@@ -1,6 +1,6 @@
 package simulator
 
-type SimulatorMsg struct {
+type TelemetryMsg struct {
 	MsgType       string  `json:"msg_type"`
 	SteeringAngle float64 `json:"steering_angle"`
 	Throttle      float64 `json:"throttle"`
@@ -13,3 +13,11 @@ type SimulatorMsg struct {
 	Time          float64 `json:"time"`
 	Cte           float64 `json:"cte"`
 }
+
+/* Json msg used to control cars. MsgType must be filled with "control" */
+type ControlMsg struct {
+	MsgType  string  `json:"msg_type"`
+	Steering float32 `json:"steering"`
+	Throttle float32 `json:"throttle"`
+	Brake    float32 `json:"brake"`
+}