robocars/robocar-throttle
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

feat(processor): implement CustomSteeringProcessor

Browse Source
This commit is contained in:
Cyrille Nofficial
2023-06-14 20:07:43 +02:00
parent a7b08ff7b4
commit c2b12f297c
4 changed files with 310 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
pkg/throttle/controller_test.go
View File

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

83
pkg/throttle/processor.go
View File

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

190
pkg/throttle/processor_test.go
View File

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