Implement steering/throttle reading from simulator

camera/camera_test.go

@@ -1,171 +0,0 @@
-package camera
-
-import (
-	"bufio"
-	"fmt"
-	"github.com/cyrilix/robocar-protobuf/go/events"
-	"github.com/golang/protobuf/proto"
-	log "github.com/sirupsen/logrus"
-	"io"
-	"io/ioutil"
-	"net"
-	"strings"
-	"sync"
-	"testing"
-)
-
-type MockPublisher struct {
-	notifyChan     chan []byte
-	initNotifyChan sync.Once
-}
-
-func (p *MockPublisher) Close() error {
-	if p.notifyChan != nil {
-		close(p.notifyChan)
-	}
-	return nil
-}
-
-func (p *MockPublisher) Publish(payload []byte) {
-	p.notifyChan <- payload
-}
-
-func (p *MockPublisher) Notify() <-chan []byte {
-	p.initNotifyChan.Do(func() { p.notifyChan = make(chan []byte) })
-	return p.notifyChan
-}
-
-func TestPart_ListenEvents(t *testing.T) {
-	simulatorMock := SimulatorMock{}
-	err := simulatorMock.Start()
-	if err != nil {
-		t.Errorf("unable to start mock server: %v", err)
-	}
-	defer func() {
-		if err := simulatorMock.Close(); err != nil {
-			t.Errorf("unable to close mock server: %v", err)
-		}
-	}()
-
-	publisher := MockPublisher{}
-
-	part := New(&publisher, simulatorMock.Addr())
-	go func() {
-		err := part.Start()
-		if err != nil {
-			t.Fatalf("unable to start camera simulator: %v", err)
-		}
-	}()
-	defer func() {
-		if err := part.Close(); err != nil {
-			t.Errorf("unable to close camera simulator: %v", err)
-		}
-	}()
-
-	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 {
-		err = simulatorMock.EmitMsg(line)
-		if err != nil {
-			t.Errorf("[line %v/%v] unable to write line: %v", idx+1, len(lines), err)
-		}
-
-		byteMsg := <-publisher.Notify()
-		var msg events.FrameMessage
-		err = proto.Unmarshal(byteMsg, &msg)
-		if err != nil {
-			t.Errorf("unable to unmarshal frame msg: %v", err)
-			continue
-		}
-		if msg.GetId() == nil {
-			t.Error("frame msg has not Id")
-		}
-		if len(msg.Frame) < 10 {
-			t.Errorf("[%v] invalid frame image: %v", msg.Id, msg.GetFrame())
-		}
-	}
-}
-
-type SimulatorMock struct {
-	ln            net.Listener
-	muConn        sync.Mutex
-	conn          net.Conn
-	writer        *bufio.Writer
-	newConnection chan net.Conn
-	logger 		  *log.Entry
-}
-
-func (c *SimulatorMock) EmitMsg(p string) (err error) {
-	c.muConn.Lock()
-	defer c.muConn.Unlock()
-	_, err = c.writer.WriteString(p + "\n")
-	if err != nil {
-		c.logger.Errorf("unable to write response: %v", err)
-	}
-	if err == io.EOF {
-		c.logger.Info("Connection closed")
-		return err
-	}
-	err = c.writer.Flush()
-	return err
-}
-
-func (c *SimulatorMock) WaitConnection() {
-	c.muConn.Lock()
-	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 {
-			conn, err := c.ln.Accept()
-			if err != nil && err == io.EOF {
-				c.logger.Errorf("connection close: %v", err)
-				break
-			}
-			if c.newConnection == nil {
-				break
-			}
-			c.newConnection <- conn
-		}
-	}()
-	return nil
-}
-
-func (c *SimulatorMock) Addr() string {
-	return c.ln.Addr().String()
-}
-
-func (c *SimulatorMock) Close() error {
-	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)
-	}
-	return nil
-}

cmd/rc-simulator/rc-simultator.go

@@ -3,30 +3,35 @@ package main
 import (
 	"flag"
 	"github.com/cyrilix/robocar-base/cli"
-	"github.com/cyrilix/robocar-simulator/camera"
+	"github.com/cyrilix/robocar-simulator/pkg/gateway"
-	"log"
+	log"github.com/sirupsen/logrus"
 	"os"
 )
 
-const DefaultClientId = "robocar-camera"
+const DefaultClientId = "robocar-simulator"
 
 func main() {
-	var mqttBroker, username, password, clientId, topicBase string
+	var mqttBroker, username, password, clientId, topicFrame string
 	var address string
+	var debug bool
 
 	mqttQos := cli.InitIntFlag("MQTT_QOS", 0)
 	_, mqttRetain := os.LookupEnv("MQTT_RETAIN")
 
 	cli.InitMqttFlags(DefaultClientId, &mqttBroker, &username, &password, &clientId, &mqttQos, &mqttRetain)
 
-	flag.StringVar(&topicBase, "mqtt-topic", os.Getenv("MQTT_TOPIC"), "Mqtt topic to publish camera frames, use MQTT_TOPIC if args not set")
+	flag.StringVar(&topicFrame, "mqtt-topic-frame", os.Getenv("MQTT_TOPIC"), "Mqtt topic to publish gateway frames, use MQTT_TOPIC_FRAME if args not set")
 	flag.StringVar(&address, "simulator-address", "127.0.0.1:9091", "Simulator address")
+	flag.BoolVar(&debug, "debug", false, "Debug logs")
 
 	flag.Parse()
 	if len(os.Args) <= 1 {
 		flag.PrintDefaults()
 		os.Exit(1)
 	}
+	if debug {
+		log.SetLevel(log.DebugLevel)
+	}
 
 	client, err := cli.Connect(mqttBroker, username, password, clientId)
 	if err != nil {
@@ -35,7 +40,7 @@ func main() {
 	defer client.Disconnect(10)
 
 
-	c := camera.New(camera.NewMqttPublisher(client, topicBase), address)
+	c := gateway.New(gateway.NewMqttPublisher(client, topicFrame, "", ""), address)
 	defer c.Stop()
 
 	cli.HandleExit(c)

pkg/controls/controls.go

@@ -5,7 +5,7 @@ import (
 	"encoding/json"
 	"fmt"
 	"github.com/cyrilix/robocar-protobuf/go/events"
-	"github.com/cyrilix/robocar-simulator/simulator"
+	"github.com/cyrilix/robocar-simulator/pkg/simulator"
 	log "github.com/sirupsen/logrus"
 	"io"
 	"net"

pkg/controls/controls_test.go

@@ -5,7 +5,7 @@ import (
 	"encoding/json"
 	"fmt"
 	"github.com/cyrilix/robocar-protobuf/go/events"
-	"github.com/cyrilix/robocar-simulator/simulator"
+	"github.com/cyrilix/robocar-simulator/pkg/simulator"
 	log "github.com/sirupsen/logrus"
 	"io"
 	"net"

pkg/gateway/gateway.go

@@ -1,4 +1,4 @@
-package camera
+package gateway
 
 import (
 	"bufio"
@@ -6,7 +6,7 @@ import (
 	"fmt"
 	"github.com/avast/retry-go"
 	"github.com/cyrilix/robocar-protobuf/go/events"
-	"github.com/cyrilix/robocar-simulator/simulator"
+	"github.com/cyrilix/robocar-simulator/pkg/simulator"
 	mqtt "github.com/eclipse/paho.mqtt.golang"
 	"github.com/golang/protobuf/proto"
 	"github.com/golang/protobuf/ptypes/timestamp"
@@ -18,7 +18,7 @@ import (
 
 func New(publisher Publisher, addressSimulator string) *Gateway {
 	l := log.WithField("simulator", addressSimulator)
-	l.Info("run camera from simulator")
+	l.Info("run gateway from simulator")
 
 	return &Gateway{
 		address:   addressSimulator,
@@ -27,7 +27,7 @@ func New(publisher Publisher, addressSimulator string) *Gateway {
 	}
 }
 
-/* Simulator interface to publish camera frames into mqtt topic */
+/* Simulator interface to publish gateway frames into mqtt topicFrame */
 type Gateway struct {
 	cancel chan interface{}
 
@@ -49,6 +49,8 @@ func (p *Gateway) Start() error {
 		select {
 		case msg := <-msgChan:
 			go p.publishFrame(msg)
+			go p.publishInputSteering(msg)
+			go p.publishInputThrottle(msg)
 		case <-p.cancel:
 			return nil
 		}
@@ -116,7 +118,7 @@ func (p *Gateway) listen(msgChan chan<- *simulator.TelemetryMsg, reader *bufio.R
 		var msg simulator.TelemetryMsg
 		err = json.Unmarshal(rawLine, &msg)
 		if err != nil {
-			p.log.Errorf("unable to unmarshal simulator msg: %v", err)
+			p.log.Errorf("unable to unmarshal simulator msg '%v': %v", string(rawLine), err)
 		}
 		if "telemetry" != msg.MsgType {
 			continue
@@ -129,7 +131,7 @@ func (p *Gateway) publishFrame(msgSim *simulator.TelemetryMsg) {
 	now := time.Now()
 	msg := &events.FrameMessage{
 		Id: &events.FrameRef{
-			Name: "camera",
+			Name: "gateway",
 			Id:   fmt.Sprintf("%d%03d", now.Unix(), now.Nanosecond()/1000/1000),
 			CreatedAt: &timestamp.Timestamp{
 				Seconds: now.Unix(),
@@ -139,11 +141,40 @@ func (p *Gateway) publishFrame(msgSim *simulator.TelemetryMsg) {
 		Frame: msgSim.Image,
 	}
 
+	log.Debugf("publish frame '%v/%v'", msg.Id.Name, msg.Id.Id)
 	payload, err := proto.Marshal(msg)
 	if err != nil {
 		p.log.Errorf("unable to marshal protobuf message: %v", err)
 	}
-	p.publisher.Publish(payload)
+	p.publisher.PublishFrame(payload)
+}
+
+func (p *Gateway) publishInputSteering(msgSim *simulator.TelemetryMsg) {
+	steering := &events.SteeringMessage{
+		Steering:  float32(msgSim.SteeringAngle),
+		Confidence: 1.0,
+	}
+
+	log.Debugf("publish steering '%v'", steering.Steering)
+	payload, err := proto.Marshal(steering)
+	if err != nil {
+		p.log.Errorf("unable to marshal protobuf message: %v", err)
+	}
+	p.publisher.PublishSteering(payload)
+}
+
+func (p *Gateway) publishInputThrottle(msgSim *simulator.TelemetryMsg) {
+	steering := &events.ThrottleMessage{
+		Throttle:  float32(msgSim.Throttle),
+		Confidence: 1.0,
+	}
+
+	log.Debugf("publish throttle '%v'", steering.Throttle)
+	payload, err := proto.Marshal(steering)
+	if err != nil {
+		p.log.Errorf("unable to marshal protobuf message: %v", err)
+	}
+	p.publisher.PublishThrottle(payload)
 }
 
 var connect = func(address string) (io.ReadWriteCloser, error) {
@@ -155,25 +186,62 @@ var connect = func(address string) (io.ReadWriteCloser, error) {
 }
 
 type Publisher interface {
-	Publish(payload []byte)
+	PublishFrame(payload []byte)
+	PublishThrottle(payload []byte)
+	PublishSteering(payload []byte)
 }
 
-func NewMqttPublisher(client mqtt.Client, topic string) Publisher {
+func NewMqttPublisher(client mqtt.Client, topicFrame, topicThrottle, topicSteering string) Publisher {
 	return &MqttPublisher{
-		client: client,
+		client:     client,
-		topic:  topic,
+		topicFrame: topicFrame,
+		topicSteering: topicSteering,
+		topicThrottle: topicThrottle,
 	}
 }
 
 type MqttPublisher struct {
-	client mqtt.Client
+	client     mqtt.Client
-	topic  string
+	topicFrame string
+	topicSteering string
+	topicThrottle string
 }
 
-func (m *MqttPublisher) Publish(payload []byte) {
+func (m *MqttPublisher) PublishThrottle(payload []byte) {
-	token := m.client.Publish(m.topic, 0, false, payload)
+	if m.topicThrottle == "" {
-	token.WaitTimeout(10 * time.Millisecond)
+		return
-	if err := token.Error(); err != nil {
+	}
+	err := m.publish(m.topicThrottle, payload)
+	if err != nil {
+		log.Errorf("unable to publish throttle: %v", err)
+	}
+}
+
+func (m *MqttPublisher) PublishSteering(payload []byte) {
+	if m.topicSteering == "" {
+		return
+	}
+	err := m.publish(m.topicSteering, payload)
+	if err != nil {
+		log.Errorf("unable to publish steering: %v", err)
+	}
+}
+
+func (m *MqttPublisher) PublishFrame(payload []byte) {
+	if m.topicFrame == "" {
+		return
+	}
+	err := m.publish(m.topicFrame, payload)
+	if err != nil {
 		log.Errorf("unable to publish frame: %v", err)
 	}
 }
+
+func (m *MqttPublisher) publish(topic string, payload []byte) error {
+	token := m.client.Publish(topic, 0, false, payload)
+	token.WaitTimeout(10 * time.Millisecond)
+	if err := token.Error(); err != nil {
+		return fmt.Errorf("unable to publish to topic: %v", err)
+	}
+	return nil
+}

pkg/gateway/gateway_test.go

@@ -0,0 +1,289 @@
+package gateway
+
+import (
+	"bufio"
+	"encoding/json"
+	"fmt"
+	"github.com/cyrilix/robocar-protobuf/go/events"
+	"github.com/golang/protobuf/proto"
+	log "github.com/sirupsen/logrus"
+	"io"
+	"io/ioutil"
+	"net"
+	"strings"
+	"sync"
+	"testing"
+	"time"
+)
+
+type MockPublisher struct {
+	notifyFrameChan        chan []byte
+	initNotifyFrameChan    sync.Once
+	notifySteeringChan     chan []byte
+	initNotifySteeringChan sync.Once
+	notifyThrottleChan     chan []byte
+	initNotifyThrottleChan sync.Once
+}
+
+func (p *MockPublisher) Close() error {
+	if p.notifyFrameChan != nil {
+		close(p.notifyFrameChan)
+	}
+	if p.notifyThrottleChan != nil {
+		close(p.notifyThrottleChan)
+	}
+	if p.notifySteeringChan != nil {
+		close(p.notifySteeringChan)
+	}
+	return nil
+}
+
+func (p *MockPublisher) PublishFrame(payload []byte) {
+	p.notifyFrameChan <- payload
+}
+func (p *MockPublisher) PublishSteering(payload []byte) {
+	p.notifySteeringChan <- payload
+}
+func (p *MockPublisher) PublishThrottle(payload []byte) {
+	p.notifyThrottleChan <- payload
+}
+
+func (p *MockPublisher) NotifyFrame() <-chan []byte {
+	p.initNotifyFrameChan.Do(func() { p.notifyFrameChan = make(chan []byte) })
+	return p.notifyFrameChan
+}
+func (p *MockPublisher) NotifySteering() <-chan []byte {
+	p.initNotifySteeringChan.Do(func() { p.notifySteeringChan = make(chan []byte) })
+	return p.notifySteeringChan
+}
+func (p *MockPublisher) NotifyThrottle() <-chan []byte {
+	p.initNotifyThrottleChan.Do(func() { p.notifyThrottleChan = make(chan []byte) })
+	return p.notifyThrottleChan
+}
+
+func TestPart_ListenEvents(t *testing.T) {
+	simulatorMock := SimulatorMock{}
+	err := simulatorMock.Start()
+	if err != nil {
+		t.Errorf("unable to start mock server: %v", err)
+	}
+	defer func() {
+		if err := simulatorMock.Close(); err != nil {
+			t.Errorf("unable to close mock server: %v", err)
+		}
+	}()
+
+	publisher := MockPublisher{}
+
+	part := New(&publisher, simulatorMock.Addr())
+	go func() {
+		err := part.Start()
+		if err != nil {
+			t.Fatalf("unable to start gateway simulator: %v", err)
+		}
+	}()
+	defer func() {
+		if err := part.Close(); err != nil {
+			t.Errorf("unable to close gateway simulator: %v", err)
+		}
+	}()
+
+	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 {
+		err = simulatorMock.EmitMsg(line)
+		if err != nil {
+			t.Errorf("[line %v/%v] unable to write line: %v", idx+1, len(lines), err)
+		}
+
+		eventsType := map[string]bool{"frame": false, "steering": false, "throttle": false}
+		nbEventsExpected := len(eventsType)
+		wg := sync.WaitGroup{}
+		// Expect number mqtt event
+
+		wg.Add(nbEventsExpected)
+		finished := make(chan struct{})
+		go func() {
+			wg.Wait()
+			finished <- struct{}{}
+		}()
+
+		timeout := time.Tick(100 * time.Millisecond)
+
+		endLoop := false
+		for {
+			select {
+			case byteMsg := <-publisher.NotifyFrame():
+				checkFrame(t, byteMsg)
+				eventsType["frame"] = true
+				wg.Done()
+			case byteMsg := <-publisher.NotifySteering():
+				checkSteering(t, byteMsg, line)
+				eventsType["steering"] = true
+				wg.Done()
+			case byteMsg := <-publisher.NotifyThrottle():
+				checkThrottle(t, byteMsg, line)
+				eventsType["throttle"] = true
+				wg.Done()
+			case <-finished:
+				log.Trace("loop ended")
+				endLoop = true
+			case <-timeout:
+				t.Errorf("not all event are published")
+				t.FailNow()
+			}
+			if endLoop {
+				break
+			}
+		}
+		for k, v := range eventsType {
+			if !v {
+				t.Errorf("no %v event published for line %v", k, line)
+			}
+		}
+	}
+}
+
+
+func checkFrame(t *testing.T, byteMsg []byte) {
+	var msg events.FrameMessage
+	err := proto.Unmarshal(byteMsg, &msg)
+	if err != nil {
+		t.Errorf("unable to unmarshal frame msg: %v", err)
+	}
+	if msg.GetId() == nil {
+		t.Error("frame msg has not Id")
+	}
+	if len(msg.Frame) < 10 {
+		t.Errorf("[%v] invalid frame image: %v", msg.Id, msg.GetFrame())
+	}
+}
+func checkSteering(t *testing.T, byteMsg []byte, rawLine string) {
+	var input map[string]interface{}
+	err := json.Unmarshal([]byte(rawLine), &input)
+	if err != nil {
+		t.Fatalf("unable to parse input data '%v': %v", rawLine, err)
+	}
+	steering := input["steering_angle"].(float64)
+	expectedSteering := float32(steering)
+
+	var msg events.SteeringMessage
+	err = proto.Unmarshal(byteMsg, &msg)
+	if err != nil {
+		t.Errorf("unable to unmarshal steering msg: %v", err)
+	}
+
+	if msg.GetSteering() != expectedSteering{
+		t.Errorf("invalid steering value: %f, wants %f", msg.GetSteering(), expectedSteering)
+	}
+	if msg.Confidence != 1.0 {
+		t.Errorf("invalid steering confidence: %f, wants %f", msg.Confidence, 1.0)
+	}
+}
+
+func checkThrottle(t *testing.T, byteMsg []byte, rawLine string) {
+	var input map[string]interface{}
+	err := json.Unmarshal([]byte(rawLine), &input)
+	if err != nil {
+		t.Fatalf("unable to parse input data '%v': %v", rawLine, err)
+	}
+	throttle := input["throttle"].(float64)
+	expectedThrottle := float32(throttle)
+
+	var msg events.SteeringMessage
+	err = proto.Unmarshal(byteMsg, &msg)
+	if err != nil {
+		t.Errorf("unable to unmarshal throttle msg: %v", err)
+	}
+
+	if msg.GetSteering() != expectedThrottle {
+		t.Errorf("invalid throttle value: %f, wants %f", msg.GetSteering(), expectedThrottle)
+	}
+	if msg.Confidence != 1.0 {
+		t.Errorf("invalid throttle confidence: %f, wants %f", msg.Confidence, 1.0)
+	}
+}
+
+type SimulatorMock struct {
+	ln            net.Listener
+	muConn        sync.Mutex
+	conn          net.Conn
+	writer        *bufio.Writer
+	newConnection chan net.Conn
+	logger        *log.Entry
+}
+
+func (c *SimulatorMock) EmitMsg(p string) (err error) {
+	c.muConn.Lock()
+	defer c.muConn.Unlock()
+	_, err = c.writer.WriteString(p + "\n")
+	if err != nil {
+		c.logger.Errorf("unable to write response: %v", err)
+	}
+	if err == io.EOF {
+		c.logger.Info("Connection closed")
+		return err
+	}
+	err = c.writer.Flush()
+	return err
+}
+
+func (c *SimulatorMock) WaitConnection() {
+	c.muConn.Lock()
+	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 {
+			conn, err := c.ln.Accept()
+			if err != nil && err == io.EOF {
+				c.logger.Errorf("connection close: %v", err)
+				break
+			}
+			if c.newConnection == nil {
+				break
+			}
+			c.newConnection <- conn
+		}
+	}()
+	return nil
+}
+
+func (c *SimulatorMock) Addr() string {
+	return c.ln.Addr().String()
+}
+
+func (c *SimulatorMock) Close() error {
+	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)
+	}
+	return nil
+}