refactor: rename part package
This commit is contained in:
Cyrille Nofficial
140
pkg/throttle/throttle.go
Normal file
140
pkg/throttle/throttle.go
Normal file
@ -0,0 +1,140 @@
|
||||
package throttle
|
||||
|
||||
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"
|
||||
"time"
|
||||
)
|
||||
|
||||
func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic string, minValue, maxValue float32, publishPilotFrequency int) *Controller {
|
||||
return &Controller{
|
||||
client: client,
|
||||
throttleTopic: throttleTopic,
|
||||
driveModeTopic: driveModeTopic,
|
||||
rcThrottleTopic: rcThrottleTopic,
|
||||
minThrottle: minValue,
|
||||
maxThrottle: maxValue,
|
||||
driveMode: events.DriveMode_USER,
|
||||
publishPilotFrequency: publishPilotFrequency,
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
type Controller struct {
|
||||
client mqtt.Client
|
||||
throttleTopic string
|
||||
minThrottle, maxThrottle float32
|
||||
|
||||
muDriveMode sync.RWMutex
|
||||
driveMode events.DriveMode
|
||||
|
||||
cancel chan interface{}
|
||||
publishPilotFrequency int
|
||||
driveModeTopic, rcThrottleTopic string
|
||||
}
|
||||
|
||||
func (c *Controller) Start() error {
|
||||
if err := registerCallbacks(c); err != nil {
|
||||
zap.S().Errorf("unable to register callbacks: %v", err)
|
||||
return err
|
||||
}
|
||||
|
||||
c.cancel = make(chan interface{})
|
||||
ticker := time.NewTicker(1 * time.Second / time.Duration(c.publishPilotFrequency))
|
||||
for {
|
||||
select {
|
||||
case <-ticker.C:
|
||||
c.onPublishPilotValue()
|
||||
case <-c.cancel:
|
||||
break
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func (c *Controller) onPublishPilotValue() {
|
||||
c.muDriveMode.RLock()
|
||||
defer c.muDriveMode.RUnlock()
|
||||
|
||||
if c.driveMode != events.DriveMode_PILOT {
|
||||
return
|
||||
}
|
||||
|
||||
throttleMsg := events.ThrottleMessage{
|
||||
Throttle: c.minThrottle,
|
||||
Confidence: 1.0,
|
||||
}
|
||||
payload, err := proto.Marshal(&throttleMsg)
|
||||
if err != nil {
|
||||
zap.S().Errorf("unable to marshal %T protobuf content: %err", throttleMsg, err)
|
||||
return
|
||||
}
|
||||
|
||||
publish(c.client, c.throttleTopic, &payload)
|
||||
}
|
||||
|
||||
func (c *Controller) Stop() {
|
||||
close(c.cancel)
|
||||
service.StopService("throttle", c.client, c.driveModeTopic, c.rcThrottleTopic)
|
||||
}
|
||||
|
||||
func (c *Controller) 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
|
||||
}
|
||||
|
||||
c.muDriveMode.Lock()
|
||||
defer c.muDriveMode.Unlock()
|
||||
c.driveMode = msg.GetDriveMode()
|
||||
}
|
||||
|
||||
func (c *Controller) onRCThrottle(_ mqtt.Client, message mqtt.Message) {
|
||||
c.muDriveMode.RLock()
|
||||
defer c.muDriveMode.RUnlock()
|
||||
if c.driveMode == events.DriveMode_USER {
|
||||
// Republish same content
|
||||
payload := message.Payload()
|
||||
var throttleMsg events.ThrottleMessage
|
||||
err := proto.Unmarshal(payload, &throttleMsg)
|
||||
if err != nil {
|
||||
zap.S().Errorf("unable to unmarshall throttle msg to check throttle value: %v", err)
|
||||
return
|
||||
}
|
||||
zap.S().Debugf("publish new throttle value from rc: %v", throttleMsg.GetThrottle())
|
||||
if throttleMsg.GetThrottle() > c.maxThrottle {
|
||||
zap.S().Debugf("throttle upper that max value allowed, patch value from %v to %v", throttleMsg.GetThrottle(), c.maxThrottle)
|
||||
throttleMsg.Throttle = c.maxThrottle
|
||||
payloadPatched, err := proto.Marshal(&throttleMsg)
|
||||
if err != nil {
|
||||
zap.S().Errorf("unable to marshall throttle msg: %v", err)
|
||||
return
|
||||
}
|
||||
publish(c.client, c.throttleTopic, &payloadPatched)
|
||||
return
|
||||
}
|
||||
publish(c.client, c.throttleTopic, &payload)
|
||||
}
|
||||
}
|
||||
|
||||
var registerCallbacks = func(p *Controller) error {
|
||||
err := service.RegisterCallback(p.client, p.driveModeTopic, p.onDriveMode)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
err = service.RegisterCallback(p.client, p.rcThrottleTopic, p.onRCThrottle)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
var publish = func(client mqtt.Client, topic string, payload *[]byte) {
|
||||
client.Publish(topic, 0, false, *payload)
|
||||
}
|
||||
89
pkg/throttle/throttle_test.go
Normal file
89
pkg/throttle/throttle_test.go
Normal file
@ -0,0 +1,89 @@
|
||||
package throttle
|
||||
|
||||
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 TestDefaultThrottle(t *testing.T) {
|
||||
oldRegister := registerCallbacks
|
||||
oldPublish := publish
|
||||
defer func() {
|
||||
registerCallbacks = oldRegister
|
||||
publish = oldPublish
|
||||
}()
|
||||
registerCallbacks = func(p *Controller) 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
|
||||
}
|
||||
|
||||
throttleTopic := "topic/throttle"
|
||||
driveModeTopic := "topic/driveMode"
|
||||
rcThrottleTopic := "topic/rcThrottle"
|
||||
|
||||
minValue := float32(0.56)
|
||||
|
||||
p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, minValue, 1., 200)
|
||||
|
||||
cases := []struct {
|
||||
name string
|
||||
maxThrottle float32
|
||||
driveMode events.DriveModeMessage
|
||||
rcThrottle events.ThrottleMessage
|
||||
expectedThrottle events.ThrottleMessage
|
||||
}{
|
||||
{"test1", 1., events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0}},
|
||||
{"test2", 1., events.DriveModeMessage{DriveMode: events.DriveMode_PILOT}, events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0}, events.ThrottleMessage{Throttle: minValue, Confidence: 1.0}},
|
||||
{"test3", 1., events.DriveModeMessage{DriveMode: events.DriveMode_PILOT}, events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}, events.ThrottleMessage{Throttle: minValue, Confidence: 1.0}},
|
||||
{"test4", 1., events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0}},
|
||||
{"test5", 1., events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}},
|
||||
{"test6", 1., events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.ThrottleMessage{Throttle: 0.6, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.6, Confidence: 1.0}},
|
||||
{"limit max throttle on user mode", 0.4, events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.ThrottleMessage{Throttle: 0.6, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}},
|
||||
}
|
||||
|
||||
go p.Start()
|
||||
defer func() { close(p.cancel) }()
|
||||
|
||||
for _, c := range cases {
|
||||
t.Run(c.name, func(t *testing.T) {
|
||||
p.maxThrottle = c.maxThrottle
|
||||
p.onDriveMode(nil, testtools.NewFakeMessageFromProtobuf(driveModeTopic, &c.driveMode))
|
||||
p.onRCThrottle(nil, testtools.NewFakeMessageFromProtobuf(rcThrottleTopic, &c.rcThrottle))
|
||||
|
||||
time.Sleep(10 * time.Millisecond)
|
||||
|
||||
for i := 3; i >= 0; i-- {
|
||||
|
||||
var msg events.ThrottleMessage
|
||||
muEventsPublished.Lock()
|
||||
err := proto.Unmarshal(eventsPublished[throttleTopic], &msg)
|
||||
if err != nil {
|
||||
t.Errorf("unable to unmarshall response: %v", err)
|
||||
t.Fail()
|
||||
}
|
||||
muEventsPublished.Unlock()
|
||||
|
||||
if msg.GetThrottle() != c.expectedThrottle.GetThrottle() {
|
||||
t.Errorf("bad msg value for mode %v: %v, wants %v", c.driveMode, msg.GetThrottle(), c.expectedThrottle.GetThrottle())
|
||||
}
|
||||
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