feat: add Processor to compute throttle from external values

* SpeedZoneProcessor
 * SteeringProcessor

cmd/rc-throttle/rc-throttle.go

@@ -13,16 +13,19 @@ import (
 
 const (
 	DefaultClientId    = "robocar-throttle"
-	DefaultThrottleMin = 0.3
+	DefaultThrottleMin = 0.1
 )
 
 func main() {
 	var mqttBroker, username, password, clientId string
-	var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string
+	var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, speedZoneTopic string
 	var minThrottle, maxThrottle float64
 	var publishPilotFrequency int
 	var brakeConfig string
 	var enableBrake bool
+	var enableSpeedZone bool
+	var slowZoneThrottle, normalZoneThrottle, fastZoneThrottle float64
+	var moderateSteering, fullSteering float64
 
 	err := cli.SetFloat64DefaultValueFromEnv(&minThrottle, "THROTTLE_MIN", DefaultThrottleMin)
 	if err != nil {
@@ -43,6 +46,7 @@ func main() {
 	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.StringVar(&throttleFeedbackTopic, "mqtt-topic-throttle-feedback", os.Getenv("MQTT_TOPIC_THROTTLE_FEEDBACK"), "Mqtt topic where to publish throttle feedback, use MQTT_TOPIC_THROTTLE_FEEDBACK if args not set")
+	flag.StringVar(&speedZoneTopic, "mqtt-topic-speed-zone", os.Getenv("MQTT_TOPIC_SPEED_ZONE"), "Mqtt topic where to subscribe speed zone events, use MQTT_TOPIC_SPEED_ZONE 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")
@@ -50,6 +54,13 @@ func main() {
 
 	flag.BoolVar(&enableBrake, "enable-brake-feature", false, "Enable brake to slow car on throttle changes")
 	flag.StringVar(&brakeConfig, "brake-configuration", "", "Json file to use to configure brake adaptation when --enable-brake is `true`")
+	flag.BoolVar(&enableSpeedZone, "enable-speed-zone", false, "Enable speed zone information to estimate throttle")
+	flag.Float64Var(&slowZoneThrottle, "slow-zone-throttle", 0.11, "Throttle target for slow speed zone")
+	flag.Float64Var(&normalZoneThrottle, "normal-zone-throttle", 0.12, "Throttle target for normal speed zone")
+	flag.Float64Var(&fastZoneThrottle, "fast-zone-throttle", 0.13, "Throttle target for fast speed zone")
+	flag.Float64Var(&moderateSteering, "moderate-steering", 0.3, "Steering above is considered as moderate")
+	flag.Float64Var(&fullSteering, "full-steering", 0.8, "Steering above is considered as full")
+
 	logLevel := zap.LevelFlag("log", zap.InfoLevel, "log level")
 
 	flag.Parse()
@@ -76,10 +87,17 @@ func main() {
 	zap.S().Infof("Topic throttle feedback        : %s", throttleFeedbackTopic)
 	zap.S().Infof("Topic steering                 : %s", steeringTopic)
 	zap.S().Infof("Topic drive mode               : %s", driveModeTopic)
+	zap.S().Infof("Topic speed zone               : %s", speedZoneTopic)
 	zap.S().Infof("Min throttle                   : %v", minThrottle)
 	zap.S().Infof("Max throttle                   : %v", maxThrottle)
 	zap.S().Infof("Publish frequency              : %vHz", publishPilotFrequency)
 	zap.S().Infof("Brake enabled                  : %v", enableBrake)
+	zap.S().Infof("SpeedZone enabled              : %v", enableSpeedZone)
+	zap.S().Infof("SpeedZone slow throttle        : %v", slowZoneThrottle)
+	zap.S().Infof("SpeedZone normal throttle      : %v", normalZoneThrottle)
+	zap.S().Infof("SpeedZone fast throttle        : %v", fastZoneThrottle)
+	zap.S().Infof("Steering moderate              : %v", moderateSteering)
+	zap.S().Infof("Steering full                  : %v", fullSteering)
 
 	client, err := cli.Connect(mqttBroker, username, password, clientId)
 	if err != nil {

pkg/throttle/controller.go

@@ -12,8 +12,9 @@ import (
 	"time"
 )
 
-func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string,
+func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
-	minValue, maxValue types.Throttle, publishPilotFrequency int, opts ...Option) *Controller {
+	speedZoneTopic string,
+	maxValue types.Throttle, publishPilotFrequency int, opts ...Option) *Controller {
 	c := &Controller{
 		client:                client,
 		throttleTopic:         throttleTopic,
@@ -21,10 +22,11 @@ func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, ste
 		rcThrottleTopic:       rcThrottleTopic,
 		steeringTopic:         steeringTopic,
 		throttleFeedbackTopic: throttleFeedbackTopic,
+		speedZoneTopic:        speedZoneTopic,
 		maxThrottle:           maxValue,
 		driveMode:             events.DriveMode_USER,
 		publishPilotFrequency: publishPilotFrequency,
-		steeringProcessor:     &SteeringProcessor{minThrottle: minValue, maxThrottle: maxValue},
+		processor:             &SteeringProcessor{minThrottle: 0.1, maxThrottle: maxValue},
 		brakeCtrl:             &brake.DisabledController{},
 	}
 	for _, o := range opts {
@@ -41,23 +43,30 @@ func WithBrakeController(bc brake.Controller) Option {
 	}
 }
 
+func WithThrottleProcessor(p Processor) Option {
+	return func(c *Controller) {
+		c.processor = p
+	}
+}
+
 type Controller struct {
 	client        mqtt.Client
 	throttleTopic string
 	maxThrottle   types.Throttle
-	steeringProcessor *SteeringProcessor
+	processor     Processor
 
 	muDriveMode sync.RWMutex
 	driveMode   events.DriveMode
 
 	muSteering sync.RWMutex
-	steering   float32
+	steering   types.Steering
 
 	brakeCtrl brake.Controller
 
 	cancel                                                                chan interface{}
 	publishPilotFrequency                                                 int
 	driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string
+	speedZoneTopic                                                        string
 }
 
 func (c *Controller) Start() error {
@@ -86,7 +95,7 @@ func (c *Controller) onPublishPilotValue() {
 		return
 	}
 
-	throttleFromSteering := c.steeringProcessor.Process(c.readSteering())
+	throttleFromSteering := c.processor.Process(c.readSteering())
 
 	throttleMsg := events.ThrottleMessage{
 		Throttle:   float32(c.brakeCtrl.AdjustThrottle(throttleFromSteering)),
@@ -102,7 +111,7 @@ func (c *Controller) onPublishPilotValue() {
 
 }
 
-func (c *Controller) readSteering() float32 {
+func (c *Controller) readSteering() types.Steering {
 	c.muSteering.RLock()
 	defer c.muSteering.RUnlock()
 	return c.steering
@@ -110,7 +119,8 @@ func (c *Controller) readSteering() float32 {
 
 func (c *Controller) Stop() {
 	close(c.cancel)
-	service.StopService("throttle", c.client, c.driveModeTopic, c.rcThrottleTopic, c.steeringTopic, c.throttleFeedbackTopic)
+	service.StopService("throttle", c.client, c.driveModeTopic, c.rcThrottleTopic, c.steeringTopic,
+		c.throttleFeedbackTopic, c.speedZoneTopic)
 }
 
 func (c *Controller) onThrottleFeedback(_ mqtt.Client, message mqtt.Message) {
@@ -174,7 +184,18 @@ func (c *Controller) onSteering(_ mqtt.Client, message mqtt.Message) {
 	}
 	c.muSteering.Lock()
 	defer c.muSteering.Unlock()
-	c.steering = steeringMsg.GetSteering()
+	c.steering = types.Steering(steeringMsg.GetSteering())
+}
+
+func (c *Controller) onSpeedZone(_ mqtt.Client, message mqtt.Message) {
+	var szMsg events.SpeedZoneMessage
+	payload := message.Payload()
+	err := proto.Unmarshal(payload, &szMsg)
+	if err != nil {
+		zap.S().Errorf("unable to unmarshal speedZone message, skip value: %v", err)
+		return
+	}
+	c.processor.SetSpeedZone(szMsg.GetSpeedZone())
 }
 
 var registerCallbacks = func(p *Controller) error {
@@ -196,6 +217,10 @@ var registerCallbacks = func(p *Controller) error {
 	if err != nil {
 		return err
 	}
+	err = service.RegisterCallback(p.client, p.speedZoneTopic, p.onSpeedZone)
+	if err != nil {
+		return err
+	}
 	return nil
 }
 

pkg/throttle/controller_test.go

@@ -36,10 +36,9 @@ func TestDefaultThrottle(t *testing.T) {
 	rcThrottleTopic := "topic/rcThrottle"
 	steeringTopic := "topic/rcThrottle"
 	throttleFeedbackTopic := "topic/feedback/throttle"
+	speedZoneTopic := "topic/speedZone"
 
-	minValue := types.Throttle(0.56)
+	p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, speedZoneTopic, 1., 200)
-
-	p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, minValue, 1., 200)
 
 	cases := []*struct {
 		name             string
@@ -117,6 +116,7 @@ func TestController_Start(t *testing.T) {
 	driveModeTopic := "topic/driveMode"
 	rcThrottleTopic := "topic/rcThrottle"
 	throttleFeedbackTopic := "topic/feedback/throttle"
+	speedZoneTopic := "topic/speedZone"
 
 	type fields struct {
 		driveMode             events.DriveMode
@@ -263,7 +263,7 @@ func TestController_Start(t *testing.T) {
 		t.Run(tt.name, func(t *testing.T) {
 			c := New(nil,
 				throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
-				tt.fields.min, tt.fields.max,
+				speedZoneTopic, tt.fields.max,
 				tt.fields.publishPilotFrequency,
 				WithBrakeController(tt.fields.brakeCtl),
 			)

pkg/throttle/processor.go

@@ -1,16 +1,96 @@
 package throttle
 
 import (
+	"encoding/json"
+	"fmt"
+	"github.com/cyrilix/robocar-protobuf/go/events"
 	"github.com/cyrilix/robocar-throttle/pkg/types"
 	"math"
+	"os"
+	"sync"
 )
 
+type Processor interface {
+	// Process compute throttle from steering value
+	Process(steering types.Steering) types.Throttle
+
+	SetSpeedZone(sz events.SpeedZone)
+}
+
+func NewSteeringProcessor(minThrottle, maxThrottle types.Throttle) *SteeringProcessor {
+	return &SteeringProcessor{
+		minThrottle: minThrottle,
+		maxThrottle: maxThrottle,
+	}
+}
+
 type SteeringProcessor struct {
 	minThrottle, maxThrottle types.Throttle
 }
 
+func (sp *SteeringProcessor) SetSpeedZone(_ events.SpeedZone) {
+	return
+}
+
 // Process compute throttle from steering value
-func (sp *SteeringProcessor) Process(steering float32) types.Throttle {
+func (sp *SteeringProcessor) Process(steering types.Steering) types.Throttle {
 	absSteering := math.Abs(float64(steering))
 	return sp.minThrottle + types.Throttle(float64(sp.maxThrottle-sp.minThrottle)*(1-absSteering))
 }
+
+func NewSpeedZoneProcessor(slowThrottle, normalThrottle, fastThrottle types.Throttle,
+	moderateSteering, fullSteering float64) *SpeedZoneProcessor {
+	return &SpeedZoneProcessor{
+		muSz:             sync.Mutex{},
+		speedZone:        events.SpeedZone_UNKNOWN,
+		slowThrottle:     slowThrottle,
+		normalThrottle:   normalThrottle,
+		fastThrottle:     fastThrottle,
+		moderateSteering: moderateSteering,
+		fullSteering:     fullSteering,
+	}
+}
+
+type SpeedZoneProcessor struct {
+	muSz                                       sync.Mutex
+	speedZone                                  events.SpeedZone
+	slowThrottle, normalThrottle, fastThrottle types.Throttle
+	moderateSteering, fullSteering             float64
+}
+
+func (sp *SpeedZoneProcessor) SpeedZone() events.SpeedZone {
+	sp.muSz.Lock()
+	defer sp.muSz.Unlock()
+	return sp.speedZone
+}
+
+func (sp *SpeedZoneProcessor) SetSpeedZone(sz events.SpeedZone) {
+	sp.muSz.Lock()
+	defer sp.muSz.Unlock()
+	sp.speedZone = sz
+}
+
+// Process compute throttle from steering value
+func (sp *SpeedZoneProcessor) Process(steering types.Steering) types.Throttle {
+	st := math.Abs(float64(steering))
+
+	switch sp.SpeedZone() {
+	case events.SpeedZone_FAST:
+		if st >= sp.fullSteering {
+			return sp.slowThrottle
+		} else if st >= sp.moderateSteering {
+			return sp.normalThrottle
+		}
+		return sp.fastThrottle
+	case events.SpeedZone_NORMAL:
+		if st > sp.fullSteering {
+			return sp.slowThrottle
+		}
+		return sp.normalThrottle
+	case events.SpeedZone_SLOW:
+		return sp.slowThrottle
+	}
+	return sp.slowThrottle
+}
+
+func NewCustomSteeringProcessor(cfg *Config) *CustomSteeringProcessor {

pkg/throttle/processor_test.go

@@ -1,7 +1,11 @@
 package throttle
 
 import (
+	"github.com/cyrilix/robocar-protobuf/go/events"
 	"github.com/cyrilix/robocar-throttle/pkg/types"
+	"os"
+	"path"
+	"reflect"
 	"testing"
 )
 
@@ -11,7 +15,7 @@ func TestSteeringProcessor_Process(t *testing.T) {
 		maxThrottle types.Throttle
 	}
 	type args struct {
-		steering float32
+		steering types.Steering
 	}
 	tests := []struct {
 		name   string
@@ -87,3 +91,134 @@ func TestSteeringProcessor_Process(t *testing.T) {
 		})
 	}
 }
+
+func TestSpeedZoneProcessor_Process(t *testing.T) {
+	type fields struct {
+		slowThrottle   types.Throttle
+		normalThrottle types.Throttle
+		fastThrottle   types.Throttle
+		speedZone      events.SpeedZone
+	}
+	type args struct {
+		steering types.Steering
+	}
+	tests := []struct {
+		name   string
+		fields fields
+		args   args
+		want   types.Throttle
+	}{
+		{
+			name:   "steering straight, undefined zone",
+			fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_SLOW},
+			args:   args{steering: 0.},
+			want:   0.2,
+		},
+		{
+			name:   "steering straight, slow zone",
+			fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_SLOW},
+			args:   args{steering: 0.},
+			want:   0.2,
+		},
+		{
+			name:   "moderate left, slow speed",
+			fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_SLOW},
+			args:   args{steering: -0.5},
+			want:   0.2,
+		},
+		{
+			name:   "moderate right, slow speed",
+			fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_SLOW},
+			args:   args{steering: 0.5},
+			want:   0.2,
+		},
+		{
+			name:   "full left, slow speed",
+			fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_SLOW},
+			args:   args{steering: -0.95},
+			want:   0.2,
+		},
+		{
+			name:   "full right, slow speed",
+			fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_SLOW},
+			args:   args{steering: 0.95},
+			want:   0.2,
+		},
+		{
+			name:   "steering straight, normal zone",
+			fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_NORMAL},
+			args:   args{steering: 0.},
+			want:   0.5,
+		},
+		{
+			name:   "moderate left, normal speed",
+			fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_NORMAL},
+			args:   args{steering: -0.5},
+			want:   0.5,
+		},
+		{
+			name:   "moderate right, normal speed",
+			fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_NORMAL},
+			args:   args{steering: 0.5},
+			want:   0.5,
+		},
+		{
+			name:   "full left, normal speed",
+			fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_NORMAL},
+			args:   args{steering: -0.95},
+			want:   0.2,
+		},
+		{
+			name:   "full right, normal speed",
+			fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_NORMAL},
+			args:   args{steering: 0.95},
+			want:   0.2,
+		},
+		{
+			name:   "steering straight, fast zone",
+			fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_FAST},
+			args:   args{steering: 0.},
+			want:   0.8,
+		},
+		{
+			name:   "moderate left, fast speed",
+			fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_FAST},
+			args:   args{steering: -0.5},
+			want:   0.5,
+		},
+		{
+			name:   "moderate right, fast speed",
+			fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_FAST},
+			args:   args{steering: 0.5},
+			want:   0.5,
+		},
+		{
+			name:   "full left, fast speed",
+			fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_FAST},
+			args:   args{steering: -0.95},
+			want:   0.2,
+		},
+		{
+			name:   "full right, fast speed",
+			fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_FAST},
+			args:   args{steering: 0.95},
+			want:   0.2,
+		},
+	}
+	for _, tt := range tests {
+		t.Run(tt.name, func(t *testing.T) {
+			sp := &SpeedZoneProcessor{
+				slowThrottle:     tt.fields.slowThrottle,
+				normalThrottle:   tt.fields.normalThrottle,
+				fastThrottle:     tt.fields.fastThrottle,
+				moderateSteering: 0.4,
+				fullSteering:     0.8,
+			}
+			sp.SetSpeedZone(tt.fields.speedZone)
+			if got := sp.Process(tt.args.steering); got != tt.want {
+				t.Errorf("Process() = %v, want %v", got, tt.want)
+			}
+		})
+	}
+}
+

pkg/types/types.go

@@ -1,3 +1,5 @@
 package types
 
 type Throttle float32
+
+type Steering float32