Implement steering/throttle reading from simulator
This commit is contained in:
123
pkg/controls/controls.go
Normal file
123
pkg/controls/controls.go
Normal file
@ -0,0 +1,123 @@
|
||||
package controls
|
||||
|
||||
import (
|
||||
"bufio"
|
||||
"encoding/json"
|
||||
"fmt"
|
||||
"github.com/cyrilix/robocar-protobuf/go/events"
|
||||
"github.com/cyrilix/robocar-simulator/pkg/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.,
|
||||
}
|
||||
}
|
||||
|
||||
281
pkg/controls/controls_test.go
Normal file
281
pkg/controls/controls_test.go
Normal file
@ -0,0 +1,281 @@
|
||||
package controls
|
||||
|
||||
import (
|
||||
"bufio"
|
||||
"encoding/json"
|
||||
"fmt"
|
||||
"github.com/cyrilix/robocar-protobuf/go/events"
|
||||
"github.com/cyrilix/robocar-simulator/pkg/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
|
||||
}
|
||||
247
pkg/gateway/gateway.go
Normal file
247
pkg/gateway/gateway.go
Normal file
@ -0,0 +1,247 @@
|
||||
package gateway
|
||||
|
||||
import (
|
||||
"bufio"
|
||||
"encoding/json"
|
||||
"fmt"
|
||||
"github.com/avast/retry-go"
|
||||
"github.com/cyrilix/robocar-protobuf/go/events"
|
||||
"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"
|
||||
log "github.com/sirupsen/logrus"
|
||||
"io"
|
||||
"net"
|
||||
"time"
|
||||
)
|
||||
|
||||
func New(publisher Publisher, addressSimulator string) *Gateway {
|
||||
l := log.WithField("simulator", addressSimulator)
|
||||
l.Info("run gateway from simulator")
|
||||
|
||||
return &Gateway{
|
||||
address: addressSimulator,
|
||||
publisher: publisher,
|
||||
log: l,
|
||||
}
|
||||
}
|
||||
|
||||
/* Simulator interface to publish gateway frames into mqtt topicFrame */
|
||||
type Gateway struct {
|
||||
cancel chan interface{}
|
||||
|
||||
address string
|
||||
conn io.ReadCloser
|
||||
|
||||
publisher Publisher
|
||||
log *log.Entry
|
||||
}
|
||||
|
||||
func (p *Gateway) Start() error {
|
||||
p.log.Info("connect to simulator")
|
||||
p.cancel = make(chan interface{})
|
||||
msgChan := make(chan *simulator.TelemetryMsg)
|
||||
|
||||
go p.run(msgChan)
|
||||
|
||||
for {
|
||||
select {
|
||||
case msg := <-msgChan:
|
||||
go p.publishFrame(msg)
|
||||
go p.publishInputSteering(msg)
|
||||
go p.publishInputThrottle(msg)
|
||||
case <-p.cancel:
|
||||
return nil
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func (p *Gateway) Stop() {
|
||||
p.log.Info("close simulator gateway")
|
||||
close(p.cancel)
|
||||
|
||||
if err := p.Close(); err != nil {
|
||||
p.log.Warnf("unexpected error while simulator connection is closed: %v", err)
|
||||
}
|
||||
}
|
||||
|
||||
func (p *Gateway) Close() error {
|
||||
if p.conn == nil {
|
||||
p.log.Warn("no connection to close")
|
||||
return nil
|
||||
}
|
||||
if err := p.conn.Close(); err != nil {
|
||||
return fmt.Errorf("unable to close connection to simulator: %v", err)
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
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 {
|
||||
p.log.Panicf("unable to connect to simulator: %v", err)
|
||||
}
|
||||
|
||||
reader := bufio.NewReader(p.conn)
|
||||
|
||||
err = retry.Do(
|
||||
func() error { return p.listen(msgChan, reader) },
|
||||
)
|
||||
if err != nil {
|
||||
p.log.Errorf("unable to connect to server: %v", err)
|
||||
}
|
||||
}
|
||||
|
||||
func (p *Gateway) listen(msgChan chan<- *simulator.TelemetryMsg, reader *bufio.Reader) error {
|
||||
for {
|
||||
rawLine, err := reader.ReadBytes('\n')
|
||||
if err == io.EOF {
|
||||
p.log.Info("Connection closed")
|
||||
return err
|
||||
}
|
||||
if err != nil {
|
||||
return fmt.Errorf("unable to read response: %v", err)
|
||||
}
|
||||
|
||||
var msg simulator.TelemetryMsg
|
||||
err = json.Unmarshal(rawLine, &msg)
|
||||
if err != nil {
|
||||
p.log.Errorf("unable to unmarshal simulator msg '%v': %v", string(rawLine), err)
|
||||
}
|
||||
if "telemetry" != msg.MsgType {
|
||||
continue
|
||||
}
|
||||
msgChan <- &msg
|
||||
}
|
||||
}
|
||||
|
||||
func (p *Gateway) publishFrame(msgSim *simulator.TelemetryMsg) {
|
||||
now := time.Now()
|
||||
msg := &events.FrameMessage{
|
||||
Id: &events.FrameRef{
|
||||
Name: "gateway",
|
||||
Id: fmt.Sprintf("%d%03d", now.Unix(), now.Nanosecond()/1000/1000),
|
||||
CreatedAt: ×tamp.Timestamp{
|
||||
Seconds: now.Unix(),
|
||||
Nanos: int32(now.Nanosecond()),
|
||||
},
|
||||
},
|
||||
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.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) {
|
||||
conn, err := net.Dial("tcp", address)
|
||||
if err != nil {
|
||||
return nil, fmt.Errorf("unable to connect to %v", address)
|
||||
}
|
||||
return conn, nil
|
||||
}
|
||||
|
||||
type Publisher interface {
|
||||
PublishFrame(payload []byte)
|
||||
PublishThrottle(payload []byte)
|
||||
PublishSteering(payload []byte)
|
||||
}
|
||||
|
||||
func NewMqttPublisher(client mqtt.Client, topicFrame, topicThrottle, topicSteering string) Publisher {
|
||||
return &MqttPublisher{
|
||||
client: client,
|
||||
topicFrame: topicFrame,
|
||||
topicSteering: topicSteering,
|
||||
topicThrottle: topicThrottle,
|
||||
}
|
||||
}
|
||||
|
||||
type MqttPublisher struct {
|
||||
client mqtt.Client
|
||||
topicFrame string
|
||||
topicSteering string
|
||||
topicThrottle string
|
||||
}
|
||||
|
||||
func (m *MqttPublisher) PublishThrottle(payload []byte) {
|
||||
if m.topicThrottle == "" {
|
||||
return
|
||||
}
|
||||
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
|
||||
}
|
||||
289
pkg/gateway/gateway_test.go
Normal file
289
pkg/gateway/gateway_test.go
Normal file
@ -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
|
||||
}
|
||||
16
pkg/gateway/testdata/msg.json
vendored
Normal file
16
pkg/gateway/testdata/msg.json
vendored
Normal file
File diff suppressed because one or more lines are too long
23
pkg/simulator/simulator.go.go
Normal file
23
pkg/simulator/simulator.go.go
Normal file
@ -0,0 +1,23 @@
|
||||
package simulator
|
||||
|
||||
type TelemetryMsg struct {
|
||||
MsgType string `json:"msg_type"`
|
||||
SteeringAngle float64 `json:"steering_angle"`
|
||||
Throttle float64 `json:"throttle"`
|
||||
Speed float64 `json:"speed"`
|
||||
Image []byte `json:"image"`
|
||||
Hit string `json:"hit"`
|
||||
PosX float64 `json:"pos_x"`
|
||||
PosY float64 `json:"posy"`
|
||||
PosZ float64 `json:"pos_z"`
|
||||
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"`
|
||||
}
|
||||
Reference in New Issue
Block a user