refactor: move part to pkg
This commit is contained in:
105
pkg/part/part.go
Normal file
105
pkg/part/part.go
Normal file
@ -0,0 +1,105 @@
|
||||
package part
|
||||
|
||||
import (
|
||||
"github.com/cyrilix/robocar-base/service"
|
||||
"github.com/cyrilix/robocar-protobuf/go/events"
|
||||
mqtt "github.com/eclipse/paho.mqtt.golang"
|
||||
"github.com/golang/protobuf/proto"
|
||||
"go.uber.org/zap"
|
||||
"sync"
|
||||
)
|
||||
|
||||
func NewPart(client mqtt.Client, steeringTopic, driveModeTopic, rcSteeringTopic, tfSteeringTopic string) *SteeringPart {
|
||||
return &SteeringPart{
|
||||
client: client,
|
||||
steeringTopic: steeringTopic,
|
||||
driveModeTopic: driveModeTopic,
|
||||
rcSteeringTopic: rcSteeringTopic,
|
||||
tfSteeringTopic: tfSteeringTopic,
|
||||
driveMode: events.DriveMode_USER,
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
type SteeringPart struct {
|
||||
client mqtt.Client
|
||||
steeringTopic string
|
||||
|
||||
muDriveMode sync.RWMutex
|
||||
driveMode events.DriveMode
|
||||
|
||||
cancel chan interface{}
|
||||
driveModeTopic, rcSteeringTopic, tfSteeringTopic string
|
||||
}
|
||||
|
||||
func (p *SteeringPart) 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 *SteeringPart) Stop() {
|
||||
close(p.cancel)
|
||||
service.StopService("throttle", p.client, p.driveModeTopic, p.rcSteeringTopic, p.tfSteeringTopic)
|
||||
}
|
||||
|
||||
func (p *SteeringPart) 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 *SteeringPart) onRCSteering(_ mqtt.Client, message mqtt.Message) {
|
||||
p.muDriveMode.RLock()
|
||||
defer p.muDriveMode.RUnlock()
|
||||
zap.S().Debugf("receive steering message from radio command: %v",message)
|
||||
if p.driveMode == events.DriveMode_USER {
|
||||
// Republish same content
|
||||
payload := message.Payload()
|
||||
publish(p.client, p.steeringTopic, &payload)
|
||||
}
|
||||
}
|
||||
func (p *SteeringPart) onTFSteering(_ mqtt.Client, message mqtt.Message) {
|
||||
p.muDriveMode.RLock()
|
||||
defer p.muDriveMode.RUnlock()
|
||||
zap.S().Debugf("receive steering message from tensorflow: %v",message)
|
||||
if p.driveMode == events.DriveMode_PILOT {
|
||||
// Republish same content
|
||||
payload := message.Payload()
|
||||
publish(p.client, p.steeringTopic, &payload)
|
||||
}
|
||||
}
|
||||
|
||||
var registerCallbacks = func(p *SteeringPart) 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/part/part_test.go
Normal file
116
pkg/part/part_test.go
Normal file
@ -0,0 +1,116 @@
|
||||
package part
|
||||
|
||||
import (
|
||||
"github.com/cyrilix/robocar-base/testtools"
|
||||
"github.com/cyrilix/robocar-protobuf/go/events"
|
||||
mqtt "github.com/eclipse/paho.mqtt.golang"
|
||||
"github.com/golang/protobuf/proto"
|
||||
"sync"
|
||||
"testing"
|
||||
"time"
|
||||
)
|
||||
|
||||
func TestDefaultSteering(t *testing.T) {
|
||||
oldRegister := registerCallbacks
|
||||
oldPublish := publish
|
||||
defer func() {
|
||||
registerCallbacks = oldRegister
|
||||
publish = oldPublish
|
||||
}()
|
||||
registerCallbacks = func(p *SteeringPart) 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 := NewPart(nil, steeringTopic, driveModeTopic, rcSteeringTopic, tfSteeringTopic)
|
||||
|
||||
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)
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user