From c2b12f297c09062b682c7b811d5eb95df62d4ccb Mon Sep 17 00:00:00 2001 From: Cyrille Nofficial Date: Wed, 14 Jun 2023 20:07:43 +0200 Subject: [PATCH] feat(processor): implement CustomSteeringProcessor --- cmd/rc-throttle/rc-throttle.go | 34 +++++- pkg/throttle/controller_test.go | 4 + pkg/throttle/processor.go | 83 ++++++++++++++ pkg/throttle/processor_test.go | 190 ++++++++++++++++++++++++++++++++ 4 files changed, 310 insertions(+), 1 deletion(-) diff --git a/cmd/rc-throttle/rc-throttle.go b/cmd/rc-throttle/rc-throttle.go index 85a0dac..4e758f1 100644 --- a/cmd/rc-throttle/rc-throttle.go +++ b/cmd/rc-throttle/rc-throttle.go @@ -24,6 +24,8 @@ func main() { var brakeConfig string var enableBrake bool var enableSpeedZone bool + var enableCustomSteeringProcessor bool + var configFileSteeringProcessor string var slowZoneThrottle, normalZoneThrottle, fastZoneThrottle float64 var moderateSteering, fullSteering float64 @@ -54,6 +56,10 @@ 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(&enableCustomSteeringProcessor, "enable-custom-steering-processor", false, "Enable custom steering processor to estimate throttle") + flag.StringVar(&configFileSteeringProcessor, "custom-steering-processor-config", "", "Path to json config to parameter custom steering processor") + 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") @@ -92,6 +98,7 @@ func main() { zap.S().Infof("Max throttle : %v", maxThrottle) zap.S().Infof("Publish frequency : %vHz", publishPilotFrequency) zap.S().Infof("Brake enabled : %v", enableBrake) + zap.S().Infof("CustomSteeringProcessor enabled: %v", enableCustomSteeringProcessor) zap.S().Infof("SpeedZone enabled : %v", enableSpeedZone) zap.S().Infof("SpeedZone slow throttle : %v", slowZoneThrottle) zap.S().Infof("SpeedZone normal throttle : %v", normalZoneThrottle) @@ -111,8 +118,33 @@ func main() { } else { brakeCtrl = &brake.DisabledController{} } + + if enableSpeedZone && enableCustomSteeringProcessor { + zap.S().Panicf("invalid flag, speedZone and customSteering processor can't be enabled at the same time") + } + var throttleProcessor throttle.Processor + if enableSpeedZone { + throttleProcessor = throttle.NewSpeedZoneProcessor( + types.Throttle(slowZoneThrottle), + types.Throttle(normalZoneThrottle), + types.Throttle(fastZoneThrottle), + moderateSteering, + fullSteering, + ) + } else if enableCustomSteeringProcessor { + cfg, err := throttle.NewConfigFromJson(configFileSteeringProcessor) + if err != nil { + zap.S().Fatalf("unable to load config '%v': %v", configFileSteeringProcessor, err) + } + throttleProcessor = throttle.NewCustomSteeringProcessor(cfg) + } else { + throttleProcessor = throttle.NewSteeringProcessor(types.Throttle(minThrottle), types.Throttle(maxThrottle)) + } + p := throttle.New(client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, - types.Throttle(minThrottle), types.Throttle(maxThrottle), 2, throttle.WithBrakeController(brakeCtrl)) + speedZoneTopic, types.Throttle(maxThrottle), 2, + throttle.WithThrottleProcessor(throttleProcessor), + throttle.WithBrakeController(brakeCtrl)) defer p.Stop() cli.HandleExit(p) diff --git a/pkg/throttle/controller_test.go b/pkg/throttle/controller_test.go index b5e94e5..65445f0 100644 --- a/pkg/throttle/controller_test.go +++ b/pkg/throttle/controller_test.go @@ -265,6 +265,10 @@ func TestController_Start(t *testing.T) { throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, speedZoneTopic, tt.fields.max, tt.fields.publishPilotFrequency, + WithThrottleProcessor(&SteeringProcessor{ + minThrottle: tt.fields.min, + maxThrottle: tt.fields.max, + }), WithBrakeController(tt.fields.brakeCtl), ) diff --git a/pkg/throttle/processor.go b/pkg/throttle/processor.go index 7980722..2b1adc1 100644 --- a/pkg/throttle/processor.go +++ b/pkg/throttle/processor.go @@ -94,3 +94,86 @@ func (sp *SpeedZoneProcessor) Process(steering types.Steering) types.Throttle { } func NewCustomSteeringProcessor(cfg *Config) *CustomSteeringProcessor { + return &CustomSteeringProcessor{ + cfg: cfg, + } +} + +type CustomSteeringProcessor struct { + cfg *Config +} + +func (cp *CustomSteeringProcessor) Process(steering types.Steering) types.Throttle { + return cp.cfg.ValueOf(steering) +} + +func (cp *CustomSteeringProcessor) SetSpeedZone(_ events.SpeedZone) { + return +} + +var emptyConfig = Config{ + SteeringValues: []types.Steering{}, + ThrottleSteps: []types.Throttle{}, +} + +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 &emptyConfig, fmt.Errorf("unable to unmarshal json content from %s file: %w", fileName, err) + } + if len(ft.SteeringValues) == 0 { + return &emptyConfig, fmt.Errorf("invalid configuration, none steering value'") + } + if len(ft.SteeringValues) != len(ft.ThrottleSteps) { + return &emptyConfig, fmt.Errorf("invalid config, steering value number must be equals "+ + "to throttle value number: %v/%v", len(ft.SteeringValues), len(ft.ThrottleSteps)) + } + lastT := types.Throttle(1.) + for _, t := range ft.ThrottleSteps { + if t < 0. || t > 1. { + return &emptyConfig, fmt.Errorf("invalid throttle value: 0.0 < %v <= 1.0", t) + } + if t >= lastT { + return &emptyConfig, fmt.Errorf("invalid throttle value, all values must be decreasing: %v <= %v", lastT, t) + } + lastT = t + } + lastS := types.Steering(-0.001) + for _, s := range ft.SteeringValues { + if s < 0. || s > 1. { + return &emptyConfig, fmt.Errorf("invalid steering value: 0.0 < %v <= 1.0", s) + } + if s <= lastS { + return &emptyConfig, fmt.Errorf("invalid steering value, all values must be increasing: %v <= %v", lastS, s) + } + lastS = s + } + return &ft, nil +} + +type Config struct { + SteeringValues []types.Steering `json:"steering_values"` + ThrottleSteps []types.Throttle `json:"throttle_steps"` +} + +func (tc *Config) ValueOf(s types.Steering) types.Throttle { + st := s + if s < 0. { + st = s * -1 + } + if st < tc.SteeringValues[0] { + return tc.ThrottleSteps[0] + } + + for i, steeringStep := range tc.SteeringValues { + if st < steeringStep { + return tc.ThrottleSteps[i-1] + } + } + return tc.ThrottleSteps[len(tc.ThrottleSteps)-1] +} diff --git a/pkg/throttle/processor_test.go b/pkg/throttle/processor_test.go index 4414064..12dc91c 100644 --- a/pkg/throttle/processor_test.go +++ b/pkg/throttle/processor_test.go @@ -222,3 +222,193 @@ func TestSpeedZoneProcessor_Process(t *testing.T) { } } +func TestConfig_ValueOf(t *testing.T) { + type fields struct { + SteeringValue []types.Steering + Data []types.Throttle + } + type args struct { + s types.Steering + } + tests := []struct { + name string + fields fields + args args + want types.Throttle + }{ + { + name: "Nil steering", + fields: fields{[]types.Steering{0.0, 0.5, 1.0}, []types.Throttle{0.9, 0.6, 0.1}}, + args: args{0.0}, + want: 0.9, + }, + { + name: "Nil steering < min config", + fields: fields{[]types.Steering{0.2, 0.5, 1.0}, []types.Throttle{0.9, 0.6, 0.3}}, + args: args{0.1}, + want: 0.9, + }, + { + name: "No nil steering", + fields: fields{[]types.Steering{0.0, 0.5, 1.0}, []types.Throttle{0.9, 0.6, 0.1}}, + args: args{0.2}, + want: 0.9, + }, + { + name: "Intermediate steering", + fields: fields{[]types.Steering{0.0, 0.5, 1.0}, []types.Throttle{0.9, 0.6, 0.1}}, + args: args{0.5}, + want: 0.6, + }, + { + name: "Max steering", + fields: fields{[]types.Steering{0.0, 0.5, 1.0}, []types.Throttle{0.9, 0.6, 0.1}}, + args: args{1.0}, + want: 0.1, + }, + { + name: "Over steering", + fields: fields{[]types.Steering{0.0, 0.5, 1.0}, []types.Throttle{0.9, 0.6, 0.1}}, + args: args{1.1}, + want: 0.1, + }, + { + name: "Negative steering < min config", + fields: fields{[]types.Steering{0.2, 0.5, 1.0}, []types.Throttle{0.9, 0.6, 0.3}}, + args: args{-0.1}, + want: 0.9, + }, + { + name: "Negative steering", + fields: fields{[]types.Steering{0.0, 0.5, 1.0}, []types.Throttle{0.9, 0.6, 0.1}}, + args: args{-0.2}, + want: 0.9, + }, + { + name: "Negative Intermediate steering", + fields: fields{[]types.Steering{0.0, 0.5, 1.0}, []types.Throttle{0.9, 0.6, 0.1}}, + args: args{-0.5}, + want: 0.6, + }, + { + name: "Minimum steering", + fields: fields{[]types.Steering{0.0, 0.5, 1.0}, []types.Throttle{0.9, 0.6, 0.1}}, + args: args{-1.0}, + want: 0.1, + }, + { + name: "Negative Over steering", + fields: fields{[]types.Steering{0.0, 0.5, 1.0}, []types.Throttle{0.9, 0.6, 0.1}}, + args: args{-1.1}, + want: 0.1, + }, + } + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + tc := &Config{ + SteeringValues: tt.fields.SteeringValue, + ThrottleSteps: tt.fields.Data, + } + if got := tc.ValueOf(tt.args.s); got != tt.want { + t.Errorf("ValueOf() = %v, want %v", got, tt.want) + } + }) + } +} + +func TestNewConfigFromJson(t *testing.T) { + type args struct { + configContent string + } + + tests := []struct { + name string + args args + want *Config + wantErr bool + }{ + { + name: "default", + args: args{ + configContent: `{ + "steering_values": [0.0, 0.5, 1.0], + "throttle_steps": [0.9, 0.6, 0.1] +} +`, + }, + want: &Config{ + SteeringValues: []types.Steering{0., 0.5, 1.}, + ThrottleSteps: []types.Throttle{0.9, 0.6, 0.1}, + }, + }, + { + name: "invalid config", + args: args{ + configContent: `{ "steering_values" }`, + }, + want: &emptyConfig, + wantErr: true, + }, + { + name: "empty config", + args: args{ + configContent: `{ + "steering_values": [], + "throttle_steps": [] +}`, + }, + want: &emptyConfig, + wantErr: true, + }, + { + name: "incoherent config", + args: args{ + configContent: `{ + "steering_values": [0.0, 0.5, 1.0], + "throttle_steps": [0.9, 0.1] +}`, + }, + want: &emptyConfig, + wantErr: true, + }, + { + name: "steering in bad order", + args: args{ + configContent: `{ + "steering_values": [0.0, 0.6, 0.5], + "throttle_steps": [0.9, 0.5, 0.1] +}`, + }, + want: &emptyConfig, + wantErr: true, + }, + { + name: "throttle in bad order", + args: args{ + configContent: `{ + "steering_values": [0.0, 0.5, 0.9], + "throttle_steps": [0.4, 0.5, 0.1] +}`, + }, + want: &emptyConfig, + wantErr: true, + }, + } + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + configName := path.Join(t.TempDir(), "config.json") + err := os.WriteFile(configName, []byte(tt.args.configContent), 0644) + if err != nil { + t.Errorf("unable to create test config: %v", err) + } + got, err := NewConfigFromJson(configName) + 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) + } + }) + } +}