From 838c9b4ef34ff0e8aadae7b0c3d173aee532f24c Mon Sep 17 00:00:00 2001 From: Cyrille Nofficial Date: Mon, 5 Sep 2022 15:30:26 +0200 Subject: [PATCH] feat(brake): implement brake feature --- cmd/rc-throttle/rc-throttle.go | 15 +++- pkg/brake/config.go | 54 +++++++++++++ pkg/brake/config_test.go | 132 ++++++++++++++++++++++++++++++++ pkg/brake/controller.go | 55 +++++++++++++ pkg/brake/controller_test.go | 92 ++++++++++++++++++++++ pkg/brake/test_data/config.json | 4 + pkg/throttle/controller.go | 47 +++++++----- pkg/throttle/controller_test.go | 35 ++++++++- pkg/throttle/processor.go | 11 ++- pkg/throttle/processor_test.go | 11 ++- pkg/types/types.go | 3 + 11 files changed, 425 insertions(+), 34 deletions(-) create mode 100644 pkg/brake/config.go create mode 100644 pkg/brake/config_test.go create mode 100644 pkg/brake/controller.go create mode 100644 pkg/brake/controller_test.go create mode 100644 pkg/brake/test_data/config.json create mode 100644 pkg/types/types.go diff --git a/cmd/rc-throttle/rc-throttle.go b/cmd/rc-throttle/rc-throttle.go index 96ef209..306279a 100644 --- a/cmd/rc-throttle/rc-throttle.go +++ b/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" @@ -19,6 +21,8 @@ func main() { 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 { @@ -44,6 +48,8 @@ func main() { 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() @@ -71,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) diff --git a/pkg/brake/config.go b/pkg/brake/config.go new file mode 100644 index 0000000..439d308 --- /dev/null +++ b/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] +} diff --git a/pkg/brake/config_test.go b/pkg/brake/config_test.go new file mode 100644 index 0000000..36b21d3 --- /dev/null +++ b/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) + } + }) + } +} diff --git a/pkg/brake/controller.go b/pkg/brake/controller.go new file mode 100644 index 0000000..bf2b54c --- /dev/null +++ b/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 +} diff --git a/pkg/brake/controller_test.go b/pkg/brake/controller_test.go new file mode 100644 index 0000000..134093a --- /dev/null +++ b/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) + } + }) + } +} diff --git a/pkg/brake/test_data/config.json b/pkg/brake/test_data/config.json new file mode 100644 index 0000000..ae0261c --- /dev/null +++ b/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 ] +} diff --git a/pkg/throttle/controller.go b/pkg/throttle/controller.go index 5878b43..75fe94d 100644 --- a/pkg/throttle/controller.go +++ b/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" @@ -11,8 +13,8 @@ import ( ) func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string, - minValue, maxValue float32, publishPilotFrequency int) *Controller { - return &Controller{ + minValue, maxValue types.Throttle, publishPilotFrequency int, opts ...Option) *Controller { + c := &Controller{ client: client, throttleTopic: throttleTopic, driveModeTopic: driveModeTopic, @@ -23,14 +25,26 @@ func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, ste 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 @@ -39,8 +53,7 @@ type Controller struct { muSteering sync.RWMutex steering float32 - muThrottleFeedback sync.RWMutex - throttleFeedback float32 + brakeCtrl brake.Controller cancel chan interface{} publishPilotFrequency int @@ -73,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) @@ -102,20 +117,17 @@ 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) + zap.S().Errorf("unable to unmarshal protobuf %T message: %v", &msg, err) return } - - c.muThrottleFeedback.Lock() - defer c.muThrottleFeedback.Unlock() - c.throttleFeedback = msg.GetThrottle() + 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 } @@ -137,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) @@ -165,13 +177,6 @@ func (c *Controller) onSteering(_ mqtt.Client, message mqtt.Message) { c.steering = steeringMsg.GetSteering() } -func (c *Controller) ThrottleFeedback() float32 { - c.muThrottleFeedback.RLock() - defer c.muThrottleFeedback.RUnlock() - tf := c.throttleFeedback - return tf -} - var registerCallbacks = func(p *Controller) error { err := service.RegisterCallback(p.client, p.driveModeTopic, p.onDriveMode) if err != nil { diff --git a/pkg/throttle/controller_test.go b/pkg/throttle/controller_test.go index ff8db1e..9033da9 100644 --- a/pkg/throttle/controller_test.go +++ b/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" @@ -35,13 +37,13 @@ func TestDefaultThrottle(t *testing.T) { steeringTopic := "topic/rcThrottle" throttleFeedbackTopic := "topic/feedback/throttle" - minValue := float32(0.56) + minValue := types.Throttle(0.56) 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 @@ -118,8 +120,9 @@ func TestController_Start(t *testing.T) { type fields struct { driveMode events.DriveMode - min, max float32 + min, max types.Throttle publishPilotFrequency int + brakeCtl brake.Controller } type msgEvents struct { driveMode *events.DriveModeMessage @@ -142,6 +145,7 @@ 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}, @@ -158,6 +162,7 @@ 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}, @@ -174,6 +179,7 @@ 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}, @@ -190,6 +196,7 @@ 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}, @@ -206,6 +213,7 @@ 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}, @@ -222,6 +230,7 @@ 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}, @@ -231,6 +240,23 @@ func TestController_Start(t *testing.T) { }, 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 { @@ -239,6 +265,7 @@ func TestController_Start(t *testing.T) { throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, tt.fields.min, tt.fields.max, tt.fields.publishPilotFrequency, + WithBrakeController(tt.fields.brakeCtl), ) go c.Start() diff --git a/pkg/throttle/processor.go b/pkg/throttle/processor.go index f90e805..01381d2 100644 --- a/pkg/throttle/processor.go +++ b/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)) } diff --git a/pkg/throttle/processor_test.go b/pkg/throttle/processor_test.go index 6fb80bc..c0157ff 100644 --- a/pkg/throttle/processor_test.go +++ b/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 - maxThrottle float32 + minThrottle types.Throttle + 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", diff --git a/pkg/types/types.go b/pkg/types/types.go new file mode 100644 index 0000000..a9afd66 --- /dev/null +++ b/pkg/types/types.go @@ -0,0 +1,3 @@ +package types + +type Throttle float32