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

refactor: rename part to controller

Browse Source
This commit is contained in:
Cyrille Nofficial
2022-08-20 21:33:45 +02:00
parent ee02b09657
commit 5a386de4ab
3 changed files with 15 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

123
pkg/steering/controller.go Normal file
View File

@@ -0,0 +1,123 @@
package steering
import (
"github.com/cyrilix/robocar-base/service"
"github.com/cyrilix/robocar-protobuf/go/events"
mqtt "github.com/eclipse/paho.mqtt.golang"
"go.uber.org/zap"
"google.golang.org/protobuf/proto"
"sync"
)
func NewController(client mqtt.Client, steeringTopic, driveModeTopic, rcSteeringTopic, tfSteeringTopic string, debug bool) *SteeringController {
return &SteeringController{
client: client,
steeringTopic: steeringTopic,
driveModeTopic: driveModeTopic,
rcSteeringTopic: rcSteeringTopic,
tfSteeringTopic: tfSteeringTopic,
driveMode: events.DriveMode_USER,
}
}
type SteeringController struct {
client mqtt.Client
steeringTopic string
muDriveMode sync.RWMutex
driveMode events.DriveMode
cancel chan interface{}
driveModeTopic, rcSteeringTopic, tfSteeringTopic string
debug bool
}
func (p *SteeringController) Start() error {
if err := registerCallbacks(p); err != nil {
zap.S().Errorf("unable to rgeister callbacks: %v", err)
return err
}
p.cancel = make(chan interface{})
<-p.cancel
return nil
}
func (p *SteeringController) Stop() {
close(p.cancel)
service.StopService("throttle", p.client, p.driveModeTopic, p.rcSteeringTopic, p.tfSteeringTopic)
}
func (p *SteeringController) onDriveMode(_ mqtt.Client, message mqtt.Message) {
var msg events.DriveModeMessage
err := proto.Unmarshal(message.Payload(), &msg)
if err != nil {
zap.S().Errorf("unable to unmarshal protobuf %T message: %v", msg, err)
return
}
p.muDriveMode.Lock()
defer p.muDriveMode.Unlock()
p.driveMode = msg.GetDriveMode()
}
func (p *SteeringController) onRCSteering(_ mqtt.Client, message mqtt.Message) {
p.muDriveMode.RLock()
defer p.muDriveMode.RUnlock()
if p.debug {
var evt events.SteeringMessage
err := proto.Unmarshal(message.Payload(), &evt)
if err != nil {
zap.S().Debugf("unable to unmarshal rc event: %v", err)
} else {
zap.S().Debugf("receive steering message from radio command: %0.00f", evt.GetSteering())
}
}
if p.driveMode == events.DriveMode_USER {
// Republish same content
payload := message.Payload()
publish(p.client, p.steeringTopic, &payload)
}
}
func (p *SteeringController) onTFSteering(_ mqtt.Client, message mqtt.Message) {
p.muDriveMode.RLock()
defer p.muDriveMode.RUnlock()
if p.debug {
var evt events.SteeringMessage
err := proto.Unmarshal(message.Payload(), &evt)
if err != nil {
zap.S().Debugf("unable to unmarshal tensorflow event: %v", err)
} else {
zap.S().Debugf("receive steering message from tensorflow: %0.00f", evt.GetSteering())
}
}
if p.driveMode == events.DriveMode_PILOT {
// Republish same content
payload := message.Payload()
publish(p.client, p.steeringTopic, &payload)
}
}
var registerCallbacks = func(p *SteeringController) error {
err := service.RegisterCallback(p.client, p.driveModeTopic, p.onDriveMode)
if err != nil {
return err
}
err = service.RegisterCallback(p.client, p.rcSteeringTopic, p.onRCSteering)
if err != nil {
return err
}
err = service.RegisterCallback(p.client, p.tfSteeringTopic, p.onTFSteering)
if err != nil {
return err
}
return nil
}
var publish = func(client mqtt.Client, topic string, payload *[]byte) {
client.Publish(topic, 0, false, *payload)
}

116
pkg/steering/controller_test.go Normal file
View File

@@ -0,0 +1,116 @@
package steering
import (
"github.com/cyrilix/robocar-base/testtools"
"github.com/cyrilix/robocar-protobuf/go/events"
mqtt "github.com/eclipse/paho.mqtt.golang"
"google.golang.org/protobuf/proto"
"sync"
"testing"
"time"
)
func TestDefaultSteering(t *testing.T) {
oldRegister := registerCallbacks
oldPublish := publish
defer func() {
registerCallbacks = oldRegister
publish = oldPublish
}()
registerCallbacks = func(p *SteeringController) error {
return nil
}
var muEventsPublished sync.Mutex
eventsPublished := make(map[string][]byte)
publish = func(client mqtt.Client, topic string, payload *[]byte) {
muEventsPublished.Lock()
defer muEventsPublished.Unlock()
eventsPublished[topic] = *payload
}
steeringTopic := "topic/steering"
driveModeTopic := "topic/driveMode"
rcSteeringTopic := "topic/rcSteering"
tfSteeringTopic := "topic/tfSteering"
p := NewController(nil, steeringTopic, driveModeTopic, rcSteeringTopic, tfSteeringTopic, true)
cases := []struct {
driveMode events.DriveModeMessage
rcSteering events.SteeringMessage
tfSteering events.SteeringMessage
expectedSteering events.SteeringMessage
}{
{
events.DriveModeMessage{DriveMode: events.DriveMode_USER},
events.SteeringMessage{Steering: 0.3, Confidence: 1.0},
events.SteeringMessage{Steering: 0.4, Confidence: 1.0},
events.SteeringMessage{Steering: 0.3, Confidence: 1.0},
},
{
events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
events.SteeringMessage{Steering: 0.5, Confidence: 1.0},
events.SteeringMessage{Steering: 0.6, Confidence: 1.0},
events.SteeringMessage{Steering: 0.6, Confidence: 1.0},
},
{
events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
events.SteeringMessage{Steering: 0.4, Confidence: 1.0},
events.SteeringMessage{Steering: 0.7, Confidence: 1.0},
events.SteeringMessage{Steering: 0.7, Confidence: 1.0},
},
{
events.DriveModeMessage{DriveMode: events.DriveMode_USER},
events.SteeringMessage{Steering: 0.5, Confidence: 1.0},
events.SteeringMessage{Steering: 0.8, Confidence: 1.0},
events.SteeringMessage{Steering: 0.5, Confidence: 1.0},
},
{
events.DriveModeMessage{DriveMode: events.DriveMode_USER},
events.SteeringMessage{Steering: 0.4, Confidence: 1.0},
events.SteeringMessage{Steering: 0.9, Confidence: 1.0},
events.SteeringMessage{Steering: 0.4, Confidence: 1.0},
},
{
events.DriveModeMessage{DriveMode: events.DriveMode_USER},
events.SteeringMessage{Steering: 0.6, Confidence: 1.0},
events.SteeringMessage{Steering: -0.3, Confidence: 1.0},
events.SteeringMessage{Steering: 0.6, Confidence: 1.0},
},
}
go p.Start()
defer func() { close(p.cancel) }()
for _, c := range cases {
p.onDriveMode(nil, testtools.NewFakeMessageFromProtobuf(driveModeTopic, &c.driveMode))
p.onRCSteering(nil, testtools.NewFakeMessageFromProtobuf(rcSteeringTopic, &c.rcSteering))
p.onTFSteering(nil, testtools.NewFakeMessageFromProtobuf(tfSteeringTopic, &c.tfSteering))
time.Sleep(10 * time.Millisecond)
for i := 3; i >= 0; i-- {
var msg events.SteeringMessage
muEventsPublished.Lock()
err := proto.Unmarshal(eventsPublished[steeringTopic], &msg)
if err != nil {
t.Errorf("unable to unmarshall response: %v", err)
t.Fail()
}
muEventsPublished.Unlock()
if msg.GetSteering() != c.expectedSteering.GetSteering() {
t.Errorf("bad msg value for mode %v: %v, wants %v",
c.driveMode, msg.GetSteering(), c.expectedSteering.GetSteering())
}
if msg.GetConfidence() != 1. {
t.Errorf("bad throtlle confidence: %v, wants %v", msg.GetConfidence(), 1.)
}
time.Sleep(1 * time.Millisecond)
}
}
}