diff --git a/cmd/rc-throttle/rc-throttle.go b/cmd/rc-throttle/rc-throttle.go index 0815d54..cfeb77e 100644 --- a/cmd/rc-throttle/rc-throttle.go +++ b/cmd/rc-throttle/rc-throttle.go @@ -24,6 +24,7 @@ func main() { var publishPilotFrequency int var brakeConfig string var enableBrake bool + var acceleratorFactor float64 var enableSpeedZone bool var enableCustomSteeringProcessor bool var configFileSteeringProcessor string @@ -58,6 +59,7 @@ 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.Float64Var(&acceleratorFactor, "accelerator-factor", 1.0, "Accelerator factor when --enable-bake 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") @@ -100,6 +102,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("Accelerator factor : %v", acceleratorFactor) zap.S().Infof("CustomSteeringProcessor enabled: %v", enableCustomSteeringProcessor) zap.S().Infof("SpeedZone enabled : %v", enableSpeedZone) zap.S().Infof("SpeedZone slow throttle : %v", slowZoneThrottle) @@ -116,7 +119,7 @@ func main() { var brakeCtrl brake.Controller if enableBrake { - brakeCtrl = brake.NewCustomControllerWithJsonConfig(brakeConfig) + brakeCtrl = brake.NewCustomControllerWithJsonConfigAndAcceleratorFactor(brakeConfig, acceleratorFactor) } else { brakeCtrl = &brake.DisabledController{} } diff --git a/pkg/brake/controller.go b/pkg/brake/controller.go index bf2b54c..679cd0e 100644 --- a/pkg/brake/controller.go +++ b/pkg/brake/controller.go @@ -20,13 +20,22 @@ func NewCustomControllerWithJsonConfig(filename string) *CustomController { if err != nil { zap.S().Panicf("unable to init brake controller with json config '%s': %v", filename, err) } - return &CustomController{cfg: config} + return &CustomController{cfg: config, acceleratorFactor: 1.0} +} + +func NewCustomControllerWithJsonConfigAndAcceleratorFactor(filename string, acceleratorFactor float64) *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, acceleratorFactor: acceleratorFactor} } type CustomController struct { - muRealThrottle sync.RWMutex - realThrottle types.Throttle - cfg *Config + muRealThrottle sync.RWMutex + realThrottle types.Throttle + cfg *Config + acceleratorFactor float64 } func (b *CustomController) SetRealThrottle(t types.Throttle) { @@ -43,6 +52,13 @@ func (b *CustomController) GetRealThrottle() types.Throttle { } func (b *CustomController) AdjustThrottle(targetThrottle types.Throttle) types.Throttle { + if targetThrottle > b.GetRealThrottle() { + throttle := b.GetRealThrottle() + (targetThrottle-b.GetRealThrottle())*types.Throttle(b.acceleratorFactor) + if throttle > 1.0 { + throttle = 1.0 + } + return throttle + } return b.cfg.ValueOf(b.GetRealThrottle(), targetThrottle) } diff --git a/pkg/brake/controller_test.go b/pkg/brake/controller_test.go index 134093a..90191bd 100644 --- a/pkg/brake/controller_test.go +++ b/pkg/brake/controller_test.go @@ -7,7 +7,8 @@ import ( func TestController_AdjustThrottle(t *testing.T) { type fields struct { - realThrottle types.Throttle + realThrottle types.Throttle + acceleratorFactor float64 } type args struct { targetThrottle types.Throttle @@ -18,46 +19,89 @@ func TestController_AdjustThrottle(t *testing.T) { args args want types.Throttle }{ + { + name: "target same as current throttle with big acceleration", + fields: fields{realThrottle: 0.2, acceleratorFactor: 2.}, + args: args{targetThrottle: 0.2}, + want: 0.2, + }, + { + name: "target > as current throttle with big acceleration", + fields: fields{realThrottle: 0.2, acceleratorFactor: 2.}, + args: args{targetThrottle: 0.3}, + want: 0.40000004, + }, + { + name: "target >> as current throttle with big acceleration", + fields: fields{realThrottle: 0.2, acceleratorFactor: 2.}, + args: args{targetThrottle: 0.5}, + want: 0.8, + }, + { + name: "target >> as current throttle with big acceleration, result > 1", + fields: fields{realThrottle: 0.2, acceleratorFactor: 3.}, + args: args{targetThrottle: 0.5}, + want: 1.0, + }, + { + name: "target < as current throttle with big acceleration", + fields: fields{realThrottle: 0.8, acceleratorFactor: 2.}, + args: args{targetThrottle: 0.7}, + want: -0.1, + }, + { + name: "target << as current throttle with big acceleration", + fields: fields{realThrottle: 0.8, acceleratorFactor: 2.}, + args: args{targetThrottle: 0.5}, + want: -0.5, + }, + { + name: "target <<< as current throttle with big acceleration", + fields: fields{realThrottle: 0.8, acceleratorFactor: 2.}, + args: args{targetThrottle: 0.2}, + want: -1., + }, + { name: "target same as current throttle", - fields: fields{realThrottle: 0.2}, + fields: fields{realThrottle: 0.2, acceleratorFactor: 1.}, args: args{targetThrottle: 0.2}, want: 0.2, }, { name: "target > as current throttle", - fields: fields{realThrottle: 0.2}, + fields: fields{realThrottle: 0.2, acceleratorFactor: 1.}, args: args{targetThrottle: 0.3}, want: 0.3, }, { name: "target >> as current throttle", - fields: fields{realThrottle: 0.2}, + fields: fields{realThrottle: 0.2, acceleratorFactor: 1.}, args: args{targetThrottle: 0.5}, want: 0.5, }, { name: "target < as current throttle", - fields: fields{realThrottle: 0.8}, + fields: fields{realThrottle: 0.8, acceleratorFactor: 1.}, args: args{targetThrottle: 0.7}, want: -0.1, }, { name: "target << as current throttle", - fields: fields{realThrottle: 0.8}, + fields: fields{realThrottle: 0.8, acceleratorFactor: 1.}, args: args{targetThrottle: 0.5}, want: -0.5, }, { name: "target <<< as current throttle", - fields: fields{realThrottle: 0.8}, + fields: fields{realThrottle: 0.8, acceleratorFactor: 1.}, args: args{targetThrottle: 0.2}, want: -1., }, } for _, tt := range tests { t.Run(tt.name, func(t *testing.T) { - b := &CustomController{cfg: NewConfig()} + b := &CustomController{cfg: NewConfig(), acceleratorFactor: tt.fields.acceleratorFactor} b.SetRealThrottle(tt.fields.realThrottle) if got := b.AdjustThrottle(tt.args.targetThrottle); got != tt.want { t.Errorf("AdjustThrottle() = %v, want %v", got, tt.want)