feat: adapt throttle from steering

cmd/rc-throttle/rc-throttle.go

@@ -16,8 +16,9 @@ const (
 
 func main() {
 	var mqttBroker, username, password, clientId string
-	var throttleTopic, driveModeTopic, rcThrottleTopic string
+	var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic string
 	var minThrottle, maxThrottle float64
+	var publishPilotFrequency int
 
 	err := cli.SetFloat64DefaultValueFromEnv(&minThrottle, "THROTTLE_MIN", DefaultThrottleMin)
 	if err != nil {
@@ -36,8 +37,12 @@ func main() {
 	flag.StringVar(&throttleTopic, "mqtt-topic-throttle", os.Getenv("MQTT_TOPIC_THROTTLE"), "Mqtt topic to publish throttle result, use MQTT_TOPIC_THROTTLE if args not set")
 	flag.StringVar(&driveModeTopic, "mqtt-topic-drive-mode", os.Getenv("MQTT_TOPIC_DRIVE_MODE"), "Mqtt topic that contains DriveMode value, use MQTT_TOPIC_DRIVE_MODE if args not set")
 	flag.StringVar(&rcThrottleTopic, "mqtt-topic-rc-throttle", os.Getenv("MQTT_TOPIC_RC_THROTTLE"), "Mqtt topic that contains RC Throttle value, use MQTT_TOPIC_RC_THROTTLE if args not set")
+	flag.StringVar(&steeringTopic, "mqtt-topic-steering", os.Getenv("MQTT_TOPIC_STEERING"), "Mqtt topic that contains steering value, use MQTT_TOPIC_STEERING if args not set")
+
 	flag.Float64Var(&minThrottle, "throttle-min", minThrottle, "Minimum throttle value, use THROTTLE_MIN if args not set")
 	flag.Float64Var(&maxThrottle, "throttle-max", maxThrottle, "Minimum throttle value, use THROTTLE_MAX if args not set")
+	flag.IntVar(&publishPilotFrequency, "update-pwm-frequency", 2, "Number of throttle event to publish when pilot mode is enabled")
+
 	logLevel := zap.LevelFlag("log", zap.InfoLevel, "log level")
 
 	flag.Parse()
@@ -65,7 +70,7 @@ func main() {
 	}
 	defer client.Disconnect(50)
 
-	p := throttle.New(client, throttleTopic, driveModeTopic, rcThrottleTopic, float32(minThrottle), float32(maxThrottle), 2)
+	p := throttle.New(client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, float32(minThrottle), float32(maxThrottle), 2)
 	defer p.Stop()
 
 	cli.HandleExit(p)

pkg/throttle/controller.go

@@ -6,16 +6,18 @@ import (
 	mqtt "github.com/eclipse/paho.mqtt.golang"
 	"go.uber.org/zap"
 	"google.golang.org/protobuf/proto"
+	"math"
 	"sync"
 	"time"
 )
 
-func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic string, minValue, maxValue float32, publishPilotFrequency int) *Controller {
+func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic string, minValue, maxValue float32, publishPilotFrequency int) *Controller {
 	return &Controller{
 		client:                client,
 		throttleTopic:         throttleTopic,
 		driveModeTopic:        driveModeTopic,
 		rcThrottleTopic:       rcThrottleTopic,
+		steeringTopic:         steeringTopic,
 		minThrottle:           minValue,
 		maxThrottle:           maxValue,
 		driveMode:             events.DriveMode_USER,
@@ -32,9 +34,12 @@ type Controller struct {
 	muDriveMode sync.RWMutex
 	driveMode   events.DriveMode
 
+	muSteering sync.RWMutex
+	steering   float32
+
 	cancel                                         chan interface{}
 	publishPilotFrequency                          int
-	driveModeTopic, rcThrottleTopic string
+	driveModeTopic, rcThrottleTopic, steeringTopic string
 }
 
 func (c *Controller) Start() error {
@@ -50,7 +55,7 @@ func (c *Controller) Start() error {
 		case <-ticker.C:
 			c.onPublishPilotValue()
 		case <-c.cancel:
-			break
+			return nil
 		}
 	}
 }
@@ -64,21 +69,34 @@ func (c *Controller) onPublishPilotValue() {
 	}
 
 	throttleMsg := events.ThrottleMessage{
-		Throttle:   c.minThrottle,
+		Throttle:   c.computeThrottle(),
 		Confidence: 1.0,
 	}
 	payload, err := proto.Marshal(&throttleMsg)
 	if err != nil {
-		zap.S().Errorf("unable to marshal %T protobuf content: %err", throttleMsg, err)
+		zap.S().Errorf("unable to marshal %v protobuf content: %v", throttleMsg.String(), err)
 		return
 	}
 
-	publish(c.client, c.throttleTopic, &payload)
+	publish(c.client, c.throttleTopic, payload)
+
+}
+
+func (c *Controller) computeThrottle() float32 {
+	s := c.readSteering()
+	absSteering := math.Abs(float64(s))
+	return c.minThrottle + float32(float64(c.maxThrottle-c.minThrottle)*(1-absSteering))
+}
+
+func (c *Controller) readSteering() float32 {
+	c.muSteering.RLock()
+	defer c.muSteering.RUnlock()
+	return c.steering
 }
 
 func (c *Controller) Stop() {
 	close(c.cancel)
-	service.StopService("throttle", c.client, c.driveModeTopic, c.rcThrottleTopic)
+	service.StopService("throttle", c.client, c.driveModeTopic, c.rcThrottleTopic, c.steeringTopic)
 }
 
 func (c *Controller) onDriveMode(_ mqtt.Client, message mqtt.Message) {
@@ -115,13 +133,26 @@ func (c *Controller) onRCThrottle(_ mqtt.Client, message mqtt.Message) {
 				zap.S().Errorf("unable to marshall throttle msg: %v", err)
 				return
 			}
-			publish(c.client, c.throttleTopic, &payloadPatched)
+			publish(c.client, c.throttleTopic, payloadPatched)
 			return
 		}
-		publish(c.client, c.throttleTopic, &payload)
+		publish(c.client, c.throttleTopic, payload)
 	}
 }
 
+func (c *Controller) onSteering(_ mqtt.Client, message mqtt.Message) {
+	var steeringMsg events.SteeringMessage
+	payload := message.Payload()
+	err := proto.Unmarshal(payload, &steeringMsg)
+	if err != nil {
+		zap.S().Errorf("unable to unmarshal steering message, skip value: %v", err)
+		return
+	}
+	c.muSteering.Lock()
+	defer c.muSteering.Unlock()
+	c.steering = steeringMsg.GetSteering()
+}
+
 var registerCallbacks = func(p *Controller) error {
 	err := service.RegisterCallback(p.client, p.driveModeTopic, p.onDriveMode)
 	if err != nil {
@@ -132,9 +163,14 @@ var registerCallbacks = func(p *Controller) error {
 	if err != nil {
 		return err
 	}
+
+	err = service.RegisterCallback(p.client, p.steeringTopic, p.onSteering)
+	if err != nil {
+		return err
+	}
 	return nil
 }
 
-var publish = func(client mqtt.Client, topic string, payload *[]byte) {
+var publish = func(client mqtt.Client, topic string, payload []byte) {
-	client.Publish(topic, 0, false, *payload)
+	client.Publish(topic, 0, false, payload)
 }

pkg/throttle/controller_test.go

@@ -0,0 +1,261 @@
+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"
+	steeringTopic := "topic/rcThrottle"
+
+	minValue := float32(0.56)
+
+	p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, 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: 1.0, Confidence: 1.0}},
+		{"test3", 1., events.DriveModeMessage{DriveMode: events.DriveMode_PILOT}, events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}, events.ThrottleMessage{Throttle: 1.0, 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.String(), 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)
+			}
+		})
+	}
+}
+func TestController_Start(t *testing.T) {
+	oldRegister := registerCallbacks
+	oldPublish := publish
+	defer func() {
+		registerCallbacks = oldRegister
+		publish = oldPublish
+	}()
+	registerCallbacks = func(p *Controller) error {
+		return nil
+	}
+	publishPilotFrequency := 10
+	waitPublish := sync.WaitGroup{}
+	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
+		waitPublish.Done()
+	}
+
+	throttleTopic := "topic/throttle"
+	steeringTopic := "topic/steering"
+	driveModeTopic := "topic/driveMode"
+	rcThrottleTopic := "topic/rcThrottle"
+
+	type fields struct {
+		driveMode             events.DriveMode
+		min, max              float32
+		publishPilotFrequency int
+	}
+	type msgEvents struct {
+		driveMode  *events.DriveModeMessage
+		steering   *events.SteeringMessage
+		rcThrottle *events.ThrottleMessage
+	}
+
+	tests := []struct {
+		name      string
+		fields    fields
+		msgEvents msgEvents
+		want      *events.ThrottleMessage
+		wantErr   bool
+	}{
+		{
+			name: "On user drive mode, throttle from rc",
+			fields: fields{
+				driveMode:             events.DriveMode_USER,
+				max:                   0.8,
+				min:                   0.3,
+				publishPilotFrequency: publishPilotFrequency,
+			},
+			msgEvents: msgEvents{
+				driveMode:  &events.DriveModeMessage{DriveMode: events.DriveMode_USER},
+				steering:   &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
+				rcThrottle: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
+			},
+			want: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
+		},
+		{
+			name: "On user drive mode, limit throttle to max allowed value",
+			fields: fields{
+				driveMode:             events.DriveMode_USER,
+				max:                   0.8,
+				min:                   0.3,
+				publishPilotFrequency: publishPilotFrequency,
+			},
+			msgEvents: msgEvents{
+				driveMode:  &events.DriveModeMessage{DriveMode: events.DriveMode_USER},
+				steering:   &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
+				rcThrottle: &events.ThrottleMessage{Throttle: 0.9, Confidence: 1.0},
+			},
+			want: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
+		},
+		{
+			name: "On user drive mode, throttle can be < to  min allowed value",
+			fields: fields{
+				driveMode:             events.DriveMode_USER,
+				max:                   0.8,
+				min:                   0.3,
+				publishPilotFrequency: publishPilotFrequency,
+			},
+			msgEvents: msgEvents{
+				driveMode:  &events.DriveModeMessage{DriveMode: events.DriveMode_USER},
+				steering:   &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
+				rcThrottle: &events.ThrottleMessage{Throttle: 0.1, Confidence: 1.0},
+			},
+			want: &events.ThrottleMessage{Throttle: 0.1, Confidence: 1.0},
+		},
+		{
+			name: "On pilot drive mode and straight steering, use max throttle allowed",
+			fields: fields{
+				driveMode:             events.DriveMode_PILOT,
+				max:                   0.8,
+				min:                   0.3,
+				publishPilotFrequency: publishPilotFrequency,
+			},
+			msgEvents: msgEvents{
+				driveMode:  &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
+				steering:   &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
+				rcThrottle: &events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0},
+			},
+			want: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
+		},
+		{
+			name: "On pilot drive mode and on left steering, use min throttle allowed",
+			fields: fields{
+				driveMode:             events.DriveMode_PILOT,
+				max:                   0.8,
+				min:                   0.3,
+				publishPilotFrequency: publishPilotFrequency,
+			},
+			msgEvents: msgEvents{
+				driveMode:  &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
+				steering:   &events.SteeringMessage{Steering: -1.0, Confidence: 1.0},
+				rcThrottle: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
+			},
+			want: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
+		},
+		{
+			name: "On pilot drive mode and on right steering, use min throttle allowed",
+			fields: fields{
+				driveMode:             events.DriveMode_PILOT,
+				max:                   0.8,
+				min:                   0.3,
+				publishPilotFrequency: publishPilotFrequency,
+			},
+			msgEvents: msgEvents{
+				driveMode:  &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
+				steering:   &events.SteeringMessage{Steering: 1.0, Confidence: 1.0},
+				rcThrottle: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
+			},
+			want: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
+		},
+	}
+
+	for _, tt := range tests {
+		t.Run(tt.name, func(t *testing.T) {
+			c := New(nil,
+				throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic,
+				tt.fields.min, tt.fields.max,
+				tt.fields.publishPilotFrequency,
+			)
+
+			go c.Start()
+			defer func() { close(c.cancel) }()
+			time.Sleep(1 * time.Millisecond)
+
+			// Publish events and wait generation of new steering message
+			waitPublish.Add(1)
+			c.onDriveMode(nil, testtools.NewFakeMessageFromProtobuf(driveModeTopic, tt.msgEvents.driveMode))
+			c.onRCThrottle(nil, testtools.NewFakeMessageFromProtobuf(rcThrottleTopic, tt.msgEvents.rcThrottle))
+			c.onSteering(nil, testtools.NewFakeMessageFromProtobuf(steeringTopic, tt.msgEvents.steering))
+			waitPublish.Wait()
+
+			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() != tt.want.GetThrottle() {
+				t.Errorf("bad msg value for mode %v: %v, wants %v", c.driveMode.String(), msg.GetThrottle(), tt.want.GetThrottle())
+			}
+
+		})
+	}
+}

pkg/throttle/throttle_test.go

@@ -1,89 +0,0 @@
-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)
-			}
-		})
-	}
-}