feat(brake): implement brake feature

cmd/rc-throttle/rc-throttle.go

@@ -3,7 +3,9 @@ package main
 import (
 	"flag"
 	"github.com/cyrilix/robocar-base/cli"
+	"github.com/cyrilix/robocar-throttle/pkg/brake"
 	"github.com/cyrilix/robocar-throttle/pkg/throttle"
+	"github.com/cyrilix/robocar-throttle/pkg/types"
 	"go.uber.org/zap"
 	"log"
 	"os"
@@ -16,9 +18,11 @@ const (
 
 func main() {
 	var mqttBroker, username, password, clientId string
-	var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic string
+	var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string
 	var minThrottle, maxThrottle float64
 	var publishPilotFrequency int
+	var brakeConfig string
+	var enableBrake bool
 
 	err := cli.SetFloat64DefaultValueFromEnv(&minThrottle, "THROTTLE_MIN", DefaultThrottleMin)
 	if err != nil {
@@ -38,11 +42,14 @@ func main() {
 	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.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.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")
 
+	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`")
 	logLevel := zap.LevelFlag("log", zap.InfoLevel, "log level")
 
 	flag.Parse()
@@ -70,7 +77,14 @@ func main() {
 	}
 	defer client.Disconnect(50)
 
-	p := throttle.New(client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, float32(minThrottle), float32(maxThrottle), 2)
+	var brakeCtrl brake.Controller
+	if enableBrake {
+		brakeCtrl = brake.NewCustomControllerWithJsonConfig(brakeConfig)
+	} else {
+		brakeCtrl = &brake.DisabledController{}
+	}
+	p := throttle.New(client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
+		types.Throttle(minThrottle), types.Throttle(maxThrottle), 2, throttle.WithBrakeController(brakeCtrl))
 	defer p.Stop()
 
 	cli.HandleExit(p)

pkg/brake/config.go

@@ -0,0 +1,54 @@
+package brake
+
+import (
+	"encoding/json"
+	"fmt"
+	"github.com/cyrilix/robocar-throttle/pkg/types"
+	"os"
+)
+
+var (
+	defaultBrakeConfig = Config{
+		DeltaSteps: []float32{0.05, 0.3, 0.5},
+		Data:       []types.Throttle{-0.1, -0.5, -1.},
+	}
+)
+
+func NewConfig() *Config {
+	return &defaultBrakeConfig
+}
+
+func NewConfigFromJson(fileName string) (*Config, error) {
+	content, err := os.ReadFile(fileName)
+	if err != nil {
+		return nil, fmt.Errorf("unable to read content from %s file: %w", fileName, err)
+	}
+	var ft Config
+	err = json.Unmarshal(content, &ft)
+	if err != nil {
+		return nil, fmt.Errorf("unable to unmarshal json content from %s file: %w", fileName, err)
+	}
+	return &ft, nil
+}
+
+type Config struct {
+	DeltaSteps []float32        `json:"delta_steps"`
+	Data       []types.Throttle `json:"data"`
+}
+
+func (tc *Config) ValueOf(currentThrottle, targetThrottle types.Throttle) types.Throttle {
+	delta := float32(currentThrottle - targetThrottle)
+
+	if delta < tc.DeltaSteps[0] {
+		return targetThrottle
+	}
+	if delta >= tc.DeltaSteps[len(tc.DeltaSteps)-1] {
+		return tc.Data[len(tc.Data)-1]
+	}
+	for idx, step := range tc.DeltaSteps {
+		if delta < step {
+			return tc.Data[idx-1]
+		}
+	}
+	return tc.Data[len(tc.Data)-1]
+}

pkg/brake/config_test.go

@@ -0,0 +1,132 @@
+package brake
+
+import (
+	"github.com/cyrilix/robocar-throttle/pkg/types"
+	"reflect"
+	"testing"
+)
+
+func TestNewConfigFromJson(t *testing.T) {
+	type args struct {
+		fileName string
+	}
+	tests := []struct {
+		name    string
+		args    args
+		want    *Config
+		wantErr bool
+	}{
+		{
+			name: "default config",
+			args: args{
+				fileName: "test_data/config.json",
+			},
+			want: &defaultBrakeConfig,
+		},
+	}
+	for _, tt := range tests {
+		t.Run(tt.name, func(t *testing.T) {
+			got, err := NewConfigFromJson(tt.args.fileName)
+			if (err != nil) != tt.wantErr {
+				t.Errorf("NewConfigFromJson() error = %v, wantErr %v", err, tt.wantErr)
+				return
+			}
+			if !reflect.DeepEqual(*got, *tt.want) {
+				t.Errorf("NewConfigFromJson() got = %v, want %v", got, tt.want)
+			}
+			if !reflect.DeepEqual(got.DeltaSteps, tt.want.DeltaSteps) {
+				t.Errorf("NewConfigFromJson(), bad DeltaSteps: got = %v, want %v", got.DeltaSteps, tt.want.DeltaSteps)
+			}
+		})
+	}
+}
+
+func TestConfig_ValueOf(t *testing.T) {
+	type fields struct {
+		DeltaSteps []float32
+		MinValue   int
+		Data       []types.Throttle
+	}
+	type args struct {
+		currentThrottle, targetThrottle types.Throttle
+	}
+	tests := []struct {
+		name   string
+		fields fields
+		args   args
+		want   types.Throttle
+	}{
+		{
+			name: "delta > 0",
+			fields: fields{
+				DeltaSteps: defaultBrakeConfig.DeltaSteps,
+				Data:       defaultBrakeConfig.Data,
+			},
+			args: args{
+				currentThrottle: 0.5,
+				targetThrottle:  0.8,
+			},
+			want: 0.8,
+		},
+		{
+			name: "no delta",
+			fields: fields{
+				DeltaSteps: defaultBrakeConfig.DeltaSteps,
+				Data:       defaultBrakeConfig.Data,
+			},
+			args: args{
+				currentThrottle: 0.5,
+				targetThrottle:  0.5,
+			},
+			want: 0.5,
+		},
+		{
+			name: "delta very low (< 1st step)",
+			fields: fields{
+				DeltaSteps: defaultBrakeConfig.DeltaSteps,
+				Data:       defaultBrakeConfig.Data,
+			},
+			args: args{
+				currentThrottle: 0.5,
+				targetThrottle:  0.495,
+			},
+			want: 0.495,
+		},
+		{
+			name: "low delta ( 1st step < delta < 2nd step )",
+			fields: fields{
+				DeltaSteps: defaultBrakeConfig.DeltaSteps,
+				Data:       defaultBrakeConfig.Data,
+			},
+			args: args{
+				currentThrottle: 0.5,
+				targetThrottle:  0.38,
+			},
+			want: -0.1,
+		},
+		{
+			name: "high delta",
+			fields: fields{
+				DeltaSteps: defaultBrakeConfig.DeltaSteps,
+				Data:       defaultBrakeConfig.Data,
+			},
+			args: args{
+				currentThrottle: 0.8,
+				targetThrottle:  0.3,
+			},
+			want: -1.,
+		},
+	}
+	for _, tt := range tests {
+		t.Run(tt.name, func(t *testing.T) {
+			f := &Config{
+				DeltaSteps: tt.fields.DeltaSteps,
+				Data:       tt.fields.Data,
+			}
+			got := f.ValueOf(tt.args.currentThrottle, tt.args.targetThrottle)
+			if !reflect.DeepEqual(got, tt.want) {
+				t.Errorf("ValueOf() = %v, want %v", got, tt.want)
+			}
+		})
+	}
+}

pkg/brake/controller.go

@@ -0,0 +1,55 @@
+package brake
+
+import (
+	"github.com/cyrilix/robocar-throttle/pkg/types"
+	"go.uber.org/zap"
+	"sync"
+)
+
+type Controller interface {
+	SetRealThrottle(t types.Throttle)
+	AdjustThrottle(targetThrottle types.Throttle) types.Throttle
+}
+
+func NewCustomController() *CustomController {
+	return &CustomController{cfg: NewConfig()}
+}
+
+func NewCustomControllerWithJsonConfig(filename string) *CustomController {
+	config, err := NewConfigFromJson(filename)
+	if err != nil {
+		zap.S().Panicf("unable to init brake controller with json config '%s': %v", filename, err)
+	}
+	return &CustomController{cfg: config}
+}
+
+type CustomController struct {
+	muRealThrottle sync.RWMutex
+	realThrottle   types.Throttle
+	cfg            *Config
+}
+
+func (b *CustomController) SetRealThrottle(t types.Throttle) {
+	b.muRealThrottle.Lock()
+	defer b.muRealThrottle.Unlock()
+	b.realThrottle = t
+}
+
+func (b *CustomController) GetRealThrottle() types.Throttle {
+	b.muRealThrottle.RLock()
+	defer b.muRealThrottle.RUnlock()
+	res := b.realThrottle
+	return res
+}
+
+func (b *CustomController) AdjustThrottle(targetThrottle types.Throttle) types.Throttle {
+	return b.cfg.ValueOf(b.GetRealThrottle(), targetThrottle)
+}
+
+type DisabledController struct{}
+
+func (d *DisabledController) SetRealThrottle(_ types.Throttle) {}
+
+func (d *DisabledController) AdjustThrottle(targetThrottle types.Throttle) types.Throttle {
+	return targetThrottle
+}

pkg/brake/controller_test.go

@@ -0,0 +1,92 @@
+package brake
+
+import (
+	"github.com/cyrilix/robocar-throttle/pkg/types"
+	"testing"
+)
+
+func TestController_AdjustThrottle(t *testing.T) {
+	type fields struct {
+		realThrottle types.Throttle
+	}
+	type args struct {
+		targetThrottle types.Throttle
+	}
+	tests := []struct {
+		name   string
+		fields fields
+		args   args
+		want   types.Throttle
+	}{
+		{
+			name:   "target same as current throttle",
+			fields: fields{realThrottle: 0.2},
+			args:   args{targetThrottle: 0.2},
+			want:   0.2,
+		},
+		{
+			name:   "target > as current throttle",
+			fields: fields{realThrottle: 0.2},
+			args:   args{targetThrottle: 0.3},
+			want:   0.3,
+		},
+		{
+			name:   "target >> as current throttle",
+			fields: fields{realThrottle: 0.2},
+			args:   args{targetThrottle: 0.5},
+			want:   0.5,
+		},
+		{
+			name:   "target < as current throttle",
+			fields: fields{realThrottle: 0.8},
+			args:   args{targetThrottle: 0.7},
+			want:   -0.1,
+		},
+		{
+			name:   "target << as current throttle",
+			fields: fields{realThrottle: 0.8},
+			args:   args{targetThrottle: 0.5},
+			want:   -0.5,
+		},
+		{
+			name:   "target <<< as current throttle",
+			fields: fields{realThrottle: 0.8},
+			args:   args{targetThrottle: 0.2},
+			want:   -1.,
+		},
+	}
+	for _, tt := range tests {
+		t.Run(tt.name, func(t *testing.T) {
+			b := &CustomController{cfg: NewConfig()}
+			b.SetRealThrottle(tt.fields.realThrottle)
+			if got := b.AdjustThrottle(tt.args.targetThrottle); got != tt.want {
+				t.Errorf("AdjustThrottle() = %v, want %v", got, tt.want)
+			}
+		})
+	}
+}
+
+func TestDisabledController_AdjustThrottle(t *testing.T) {
+	type args struct {
+		targetThrottle types.Throttle
+	}
+	tests := []struct {
+		name string
+		args args
+		want types.Throttle
+	}{
+		{
+			name: "doesn't modify value",
+			args: args{targetThrottle: 0.5},
+			want: 0.5,
+		},
+	}
+	for _, tt := range tests {
+		t.Run(tt.name, func(t *testing.T) {
+			d := &DisabledController{}
+			if got := d.AdjustThrottle(tt.args.targetThrottle); got != tt.want {
+				t.Errorf("AdjustThrottle() = %v, want %v", got, tt.want)
+			}
+		})
+	}
+}

pkg/brake/test_data/config.json

@@ -0,0 +1,4 @@
+{
+  "delta_steps": [ 0.05, 0.3, 0.5 ],
+  "data": [ -0.1, -0.5, -1.0 ]
+}

pkg/throttle/controller.go

@@ -3,6 +3,8 @@ package throttle
 import (
 	"github.com/cyrilix/robocar-base/service"
 	"github.com/cyrilix/robocar-protobuf/go/events"
+	"github.com/cyrilix/robocar-throttle/pkg/brake"
+	"github.com/cyrilix/robocar-throttle/pkg/types"
 	mqtt "github.com/eclipse/paho.mqtt.golang"
 	"go.uber.org/zap"
 	"google.golang.org/protobuf/proto"
@@ -10,25 +12,39 @@ import (
 	"time"
 )
 
-func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic string, minValue, maxValue float32, publishPilotFrequency int) *Controller {
+func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string,
-	return &Controller{
+	minValue, maxValue types.Throttle, publishPilotFrequency int, opts ...Option) *Controller {
+	c := &Controller{
 		client:                client,
 		throttleTopic:         throttleTopic,
 		driveModeTopic:        driveModeTopic,
 		rcThrottleTopic:       rcThrottleTopic,
 		steeringTopic:         steeringTopic,
+		throttleFeedbackTopic: throttleFeedbackTopic,
 		maxThrottle:           maxValue,
 		driveMode:             events.DriveMode_USER,
 		publishPilotFrequency: publishPilotFrequency,
 		steeringProcessor:     &SteeringProcessor{minThrottle: minValue, maxThrottle: maxValue},
+		brakeCtrl:             &brake.DisabledController{},
 	}
+	for _, o := range opts {
+		o(c)
+	}
+	return c
+}
 
+type Option func(c *Controller)
+
+func WithBrakeController(bc brake.Controller) Option {
+	return func(c *Controller) {
+		c.brakeCtrl = bc
+	}
 }
 
 type Controller struct {
 	client            mqtt.Client
 	throttleTopic     string
-	maxThrottle       float32
+	maxThrottle       types.Throttle
 	steeringProcessor *SteeringProcessor
 
 	muDriveMode sync.RWMutex
@@ -37,9 +53,11 @@ type Controller struct {
 	muSteering sync.RWMutex
 	steering   float32
 
-	cancel                                         chan interface{}
+	brakeCtrl brake.Controller
-	publishPilotFrequency                          int
+
-	driveModeTopic, rcThrottleTopic, steeringTopic string
+	cancel                                                                chan interface{}
+	publishPilotFrequency                                                 int
+	driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string
 }
 
 func (c *Controller) Start() error {
@@ -68,8 +86,10 @@ func (c *Controller) onPublishPilotValue() {
 		return
 	}
 
+	throttleFromSteering := c.steeringProcessor.Process(c.readSteering())
+
 	throttleMsg := events.ThrottleMessage{
-		Throttle:   c.steeringProcessor.Process(c.readSteering()),
+		Throttle:   float32(c.brakeCtrl.AdjustThrottle(throttleFromSteering)),
 		Confidence: 1.0,
 	}
 	payload, err := proto.Marshal(&throttleMsg)
@@ -90,14 +110,24 @@ func (c *Controller) readSteering() float32 {
 
 func (c *Controller) Stop() {
 	close(c.cancel)
-	service.StopService("throttle", c.client, c.driveModeTopic, c.rcThrottleTopic, c.steeringTopic)
+	service.StopService("throttle", c.client, c.driveModeTopic, c.rcThrottleTopic, c.steeringTopic, c.throttleFeedbackTopic)
+}
+
+func (c *Controller) onThrottleFeedback(_ mqtt.Client, message mqtt.Message) {
+	var msg events.ThrottleMessage
+	err := proto.Unmarshal(message.Payload(), &msg)
+	if err != nil {
+		zap.S().Errorf("unable to unmarshal protobuf %T message: %v", &msg, err)
+		return
+	}
+	c.brakeCtrl.SetRealThrottle(types.Throttle(msg.GetThrottle()))
 }
 
 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)
+		zap.S().Errorf("unable to unmarshal protobuf %T message: %v", &msg, err)
 		return
 	}
 
@@ -119,9 +149,9 @@ func (c *Controller) onRCThrottle(_ mqtt.Client, message mqtt.Message) {
 			return
 		}
 		zap.S().Debugf("publish new throttle value from rc: %v", throttleMsg.GetThrottle())
-		if throttleMsg.GetThrottle() > c.maxThrottle {
+		if types.Throttle(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
+			throttleMsg.Throttle = float32(c.maxThrottle)
 			payloadPatched, err := proto.Marshal(&throttleMsg)
 			if err != nil {
 				zap.S().Errorf("unable to marshall throttle msg: %v", err)
@@ -162,6 +192,10 @@ var registerCallbacks = func(p *Controller) error {
 	if err != nil {
 		return err
 	}
+	err = service.RegisterCallback(p.client, p.throttleFeedbackTopic, p.onThrottleFeedback)
+	if err != nil {
+		return err
+	}
 	return nil
 }
 

pkg/throttle/controller_test.go

@@ -3,6 +3,8 @@ package throttle
 import (
 	"github.com/cyrilix/robocar-base/testtools"
 	"github.com/cyrilix/robocar-protobuf/go/events"
+	"github.com/cyrilix/robocar-throttle/pkg/brake"
+	"github.com/cyrilix/robocar-throttle/pkg/types"
 	mqtt "github.com/eclipse/paho.mqtt.golang"
 	"google.golang.org/protobuf/proto"
 	"sync"
@@ -33,14 +35,15 @@ func TestDefaultThrottle(t *testing.T) {
 	driveModeTopic := "topic/driveMode"
 	rcThrottleTopic := "topic/rcThrottle"
 	steeringTopic := "topic/rcThrottle"
+	throttleFeedbackTopic := "topic/feedback/throttle"
 
-	minValue := float32(0.56)
+	minValue := types.Throttle(0.56)
 
-	p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, minValue, 1., 200)
+	p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, minValue, 1., 200)
 
-	cases := []struct {
+	cases := []*struct {
 		name             string
-		maxThrottle      float32
+		maxThrottle      types.Throttle
 		driveMode        events.DriveModeMessage
 		rcThrottle       events.ThrottleMessage
 		expectedThrottle events.ThrottleMessage
@@ -113,16 +116,19 @@ func TestController_Start(t *testing.T) {
 	steeringTopic := "topic/steering"
 	driveModeTopic := "topic/driveMode"
 	rcThrottleTopic := "topic/rcThrottle"
+	throttleFeedbackTopic := "topic/feedback/throttle"
 
 	type fields struct {
 		driveMode             events.DriveMode
-		min, max              float32
+		min, max              types.Throttle
 		publishPilotFrequency int
+		brakeCtl              brake.Controller
 	}
 	type msgEvents struct {
-		driveMode  *events.DriveModeMessage
+		driveMode        *events.DriveModeMessage
-		steering   *events.SteeringMessage
+		steering         *events.SteeringMessage
-		rcThrottle *events.ThrottleMessage
+		rcThrottle       *events.ThrottleMessage
+		throttleFeedback *events.ThrottleMessage
 	}
 
 	tests := []struct {
@@ -139,11 +145,13 @@ func TestController_Start(t *testing.T) {
 				max:                   0.8,
 				min:                   0.3,
 				publishPilotFrequency: publishPilotFrequency,
+				brakeCtl:              &brake.DisabledController{},
 			},
 			msgEvents: msgEvents{
-				driveMode:  &events.DriveModeMessage{DriveMode: events.DriveMode_USER},
+				driveMode:        &events.DriveModeMessage{DriveMode: events.DriveMode_USER},
-				steering:   &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
+				steering:         &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
-				rcThrottle: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
+				rcThrottle:       &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
+				throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
 			},
 			want: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
 		},
@@ -154,11 +162,13 @@ func TestController_Start(t *testing.T) {
 				max:                   0.8,
 				min:                   0.3,
 				publishPilotFrequency: publishPilotFrequency,
+				brakeCtl:              &brake.DisabledController{},
 			},
 			msgEvents: msgEvents{
-				driveMode:  &events.DriveModeMessage{DriveMode: events.DriveMode_USER},
+				driveMode:        &events.DriveModeMessage{DriveMode: events.DriveMode_USER},
-				steering:   &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
+				steering:         &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
-				rcThrottle: &events.ThrottleMessage{Throttle: 0.9, Confidence: 1.0},
+				rcThrottle:       &events.ThrottleMessage{Throttle: 0.9, Confidence: 1.0},
+				throttleFeedback: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
 			},
 			want: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
 		},
@@ -169,11 +179,13 @@ func TestController_Start(t *testing.T) {
 				max:                   0.8,
 				min:                   0.3,
 				publishPilotFrequency: publishPilotFrequency,
+				brakeCtl:              &brake.DisabledController{},
 			},
 			msgEvents: msgEvents{
-				driveMode:  &events.DriveModeMessage{DriveMode: events.DriveMode_USER},
+				driveMode:        &events.DriveModeMessage{DriveMode: events.DriveMode_USER},
-				steering:   &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
+				steering:         &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
-				rcThrottle: &events.ThrottleMessage{Throttle: 0.1, Confidence: 1.0},
+				rcThrottle:       &events.ThrottleMessage{Throttle: 0.1, Confidence: 1.0},
+				throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
 			},
 			want: &events.ThrottleMessage{Throttle: 0.1, Confidence: 1.0},
 		},
@@ -184,11 +196,13 @@ func TestController_Start(t *testing.T) {
 				max:                   0.8,
 				min:                   0.3,
 				publishPilotFrequency: publishPilotFrequency,
+				brakeCtl:              &brake.DisabledController{},
 			},
 			msgEvents: msgEvents{
-				driveMode:  &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
+				driveMode:        &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
-				steering:   &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
+				steering:         &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
-				rcThrottle: &events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0},
+				rcThrottle:       &events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0},
+				throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
 			},
 			want: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
 		},
@@ -199,11 +213,13 @@ func TestController_Start(t *testing.T) {
 				max:                   0.8,
 				min:                   0.3,
 				publishPilotFrequency: publishPilotFrequency,
+				brakeCtl:              &brake.DisabledController{},
 			},
 			msgEvents: msgEvents{
-				driveMode:  &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
+				driveMode:        &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
-				steering:   &events.SteeringMessage{Steering: -1.0, Confidence: 1.0},
+				steering:         &events.SteeringMessage{Steering: -1.0, Confidence: 1.0},
-				rcThrottle: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
+				rcThrottle:       &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
+				throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
 			},
 			want: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
 		},
@@ -214,22 +230,42 @@ func TestController_Start(t *testing.T) {
 				max:                   0.8,
 				min:                   0.3,
 				publishPilotFrequency: publishPilotFrequency,
+				brakeCtl:              &brake.DisabledController{},
 			},
 			msgEvents: msgEvents{
-				driveMode:  &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
+				driveMode:        &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
-				steering:   &events.SteeringMessage{Steering: 1.0, Confidence: 1.0},
+				steering:         &events.SteeringMessage{Steering: 1.0, Confidence: 1.0},
-				rcThrottle: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
+				rcThrottle:       &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
+				throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
 			},
 			want: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
 		},
+		{
+			name: "On pilot drive mode, should brake on brutal change",
+			fields: fields{
+				driveMode:             events.DriveMode_PILOT,
+				max:                   1.0,
+				min:                   0.3,
+				publishPilotFrequency: publishPilotFrequency,
+				brakeCtl:              brake.NewCustomController(),
+			},
+			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},
+				throttleFeedback: &events.ThrottleMessage{Throttle: 1.0, Confidence: 1.0},
+			},
+			want: &events.ThrottleMessage{Throttle: -1.0, Confidence: 1.0},
+		},
 	}
 
 	for _, tt := range tests {
 		t.Run(tt.name, func(t *testing.T) {
 			c := New(nil,
-				throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic,
+				throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
 				tt.fields.min, tt.fields.max,
 				tt.fields.publishPilotFrequency,
+				WithBrakeController(tt.fields.brakeCtl),
 			)
 
 			go c.Start()
@@ -241,6 +277,7 @@ func TestController_Start(t *testing.T) {
 			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))
+			c.onThrottleFeedback(nil, testtools.NewFakeMessageFromProtobuf(throttleFeedbackTopic, tt.msgEvents.throttleFeedback))
 			waitPublish.Wait()
 
 			var msg events.ThrottleMessage

pkg/throttle/processor.go

@@ -1,13 +1,16 @@
 package throttle
 
-import "math"
+import (
+	"github.com/cyrilix/robocar-throttle/pkg/types"
+	"math"
+)
 
 type SteeringProcessor struct {
-	minThrottle, maxThrottle float32
+	minThrottle, maxThrottle types.Throttle
 }
 
 // Process compute throttle from steering value
-func (sp *SteeringProcessor) Process(steering float32) float32 {
+func (sp *SteeringProcessor) Process(steering float32) types.Throttle {
 	absSteering := math.Abs(float64(steering))
-	return sp.minThrottle + float32(float64(sp.maxThrottle-sp.minThrottle)*(1-absSteering))
+	return sp.minThrottle + types.Throttle(float64(sp.maxThrottle-sp.minThrottle)*(1-absSteering))
 }

pkg/throttle/processor_test.go

@@ -1,11 +1,14 @@
 package throttle
 
-import "testing"
+import (
+	"github.com/cyrilix/robocar-throttle/pkg/types"
+	"testing"
+)
 
 func TestSteeringProcessor_Process(t *testing.T) {
 	type fields struct {
-		minThrottle float32
+		minThrottle types.Throttle
-		maxThrottle float32
+		maxThrottle types.Throttle
 	}
 	type args struct {
 		steering float32
@@ -14,7 +17,7 @@ func TestSteeringProcessor_Process(t *testing.T) {
 		name   string
 		fields fields
 		args   args
-		want   float32
+		want   types.Throttle
 	}{
 		{
 			name: "steering straight",

pkg/types/types.go

@@ -0,0 +1,3 @@
+package types
+
+type Throttle float32