robocars/robocar-simulator
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Implements Gateway for car controls

Browse Source
This commit is contained in:
Cyrille Nofficial 2020-11-29 18:42:33 +01:00
parent b2ca7423ea
commit 843ada8357
6 changed files with 511 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
camera/camera.go
View File

@ -17,11 +17,13 @@ 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,
publisher: publisher,
log: l,
}
}
@ -33,12 +35,13 @@ type Gateway struct {
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,7 +155,7 @@ 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 {
@ -165,8 +170,8 @@ type MqttPublisher struct {
topic string
}
func (m *MqttPublisher) Publish(payload *[]byte) {
token := m.client.Publish(m.topic, 0, false, *payload)
func (m *MqttPublisher) Publish(payload []byte) {
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)

112
camera/camera_test.go
View File

@ -12,44 +12,44 @@ import (
"strings"
"sync"
"testing"
"time"
)
type MockPublisher struct {
muPubEvents sync.Mutex
publishedEvents []*[]byte
notifyChan chan []byte
initNotifyChan sync.Once
}
func (p *MockPublisher) Publish(payload *[]byte) {
p.muPubEvents.Lock()
defer p.muPubEvents.Unlock()
p.publishedEvents = append(p.publishedEvents, payload)
func (p *MockPublisher) Close() error {
if p.notifyChan != nil {
close(p.notifyChan)
}
return nil
}
func (p *MockPublisher) Events() []*[]byte {
p.muPubEvents.Lock()
defer p.muPubEvents.Unlock()
eventsMsg := make([]*[]byte, len(p.publishedEvents), len(p.publishedEvents))
copy(eventsMsg, p.publishedEvents)
return eventsMsg
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) {
connMock := ConnMock{}
err := connMock.Listen()
simulatorMock := SimulatorMock{}
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, connMock.Addr())
part := New(&publisher, simulatorMock.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 = connMock.WriteMsg(line)
err = simulatorMock.EmitMsg(line)
if err != nil {
t.Errorf("[line %v/%v] unable to write line: %v", idx+1, len(lines), err)
}
}
time.Sleep(10 * time.Millisecond)
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() {
byteMsg := <-publisher.Notify()
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
muConn sync.Mutex
conn net.Conn
muWriter sync.Mutex
writer *bufio.Writer
newConnection chan net.Conn
logger *log.Entry
}
func (c *ConnMock) WriteMsg(p string) (err error) {
c.muWriter.Lock()
defer c.muWriter.Unlock()
func (c *SimulatorMock) EmitMsg(p string) (err error) {
c.muConn.Lock()
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 {
c.muWriter.Lock()
defer c.muWriter.Unlock()
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 {
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 {
log.Infof("close mock server")
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)

2
camera/testdata/msg.json vendored
View File

File diff suppressed because one or more lines are too long

123
controls/controls.go Normal file
View File

@ -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.,
}
}

281
controls/controls_test.go Normal file
View File

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

10
simulator/simulator.go.go
View File

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