refactor: move package to pkg

pkg/part/part.go

@@ -0,0 +1,122 @@
+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"
+	"time"
+)
+
+func NewPart(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic string, minValue, maxValue float32, publishPilotFrequency int) *ThrottlePart {
+	return &ThrottlePart{
+		client:                client,
+		throttleTopic:         throttleTopic,
+		driveModeTopic:        driveModeTopic,
+		rcThrottleTopic:       rcThrottleTopic,
+		minThrottle:           minValue,
+		maxThrottle:           maxValue,
+		driveMode:             events.DriveMode_USER,
+		publishPilotFrequency: publishPilotFrequency,
+	}
+
+}
+
+type ThrottlePart struct {
+	client                   mqtt.Client
+	throttleTopic            string
+	minThrottle, maxThrottle float32
+
+	muDriveMode sync.RWMutex
+	driveMode   events.DriveMode
+
+	cancel                          chan interface{}
+	publishPilotFrequency           int
+	driveModeTopic, rcThrottleTopic string
+}
+
+func (p *ThrottlePart) Start() error {
+	if err := registerCallbacks(p); err != nil {
+		zap.S().Errorf("unable to register callbacks: %v", err)
+		return err
+	}
+
+	p.cancel = make(chan interface{})
+	ticker := time.NewTicker(1 * time.Second / time.Duration(p.publishPilotFrequency))
+	for {
+		select {
+		case <-ticker.C:
+			p.onPublishPilotValue()
+		case <-p.cancel:
+			break
+		}
+	}
+}
+
+func (p *ThrottlePart) onPublishPilotValue() {
+	p.muDriveMode.RLock()
+	defer p.muDriveMode.RUnlock()
+
+	if p.driveMode != events.DriveMode_PILOT {
+		return
+	}
+
+	throttleMsg := events.ThrottleMessage{
+		Throttle:   p.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(p.client, p.throttleTopic, &payload)
+}
+
+func (p *ThrottlePart) Stop() {
+	close(p.cancel)
+	service.StopService("throttle", p.client, p.driveModeTopic, p.rcThrottleTopic)
+}
+
+func (p *ThrottlePart) 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 *ThrottlePart) onRCThrottle(_ mqtt.Client, message mqtt.Message) {
+	p.muDriveMode.RLock()
+	defer p.muDriveMode.RUnlock()
+	if p.driveMode == events.DriveMode_USER {
+		// Republish same content
+		payload := message.Payload()
+		publish(p.client, p.throttleTopic, &payload)
+	}
+}
+
+var registerCallbacks = func(p *ThrottlePart) 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)
+}

pkg/part/part_test.go

@@ -0,0 +1,84 @@
+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 TestDefaultThrottle(t *testing.T) {
+	oldRegister := registerCallbacks
+	oldPublish := publish
+	defer func() {
+		registerCallbacks = oldRegister
+		publish = oldPublish
+	}()
+	registerCallbacks = func(p *ThrottlePart) 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 := NewPart(nil, throttleTopic, driveModeTopic, rcThrottleTopic, minValue, 1., 200)
+
+	cases := []struct {
+		driveMode        events.DriveModeMessage
+		rcThrottle       events.ThrottleMessage
+		expectedThrottle events.ThrottleMessage
+	}{
+		{events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0}},
+		{events.DriveModeMessage{DriveMode: events.DriveMode_PILOT}, events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0}, events.ThrottleMessage{Throttle: minValue, Confidence: 1.0}},
+		{events.DriveModeMessage{DriveMode: events.DriveMode_PILOT}, events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}, events.ThrottleMessage{Throttle: minValue, Confidence: 1.0}},
+		{events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0}},
+		{events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}},
+		{events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.ThrottleMessage{Throttle: 0.6, Confidence: 1.0}, events.ThrottleMessage{Throttle: 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.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)
+		}
+	}
+}