feat(processor): implement CustomSteeringProcessor

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)

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),
 			)
 

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]
+}

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)
+			}
+		})
+	}
+}