diff --git a/pkg/steering/corrector.go b/pkg/steering/corrector.go index 5617b88..6727256 100644 --- a/pkg/steering/corrector.go +++ b/pkg/steering/corrector.go @@ -9,11 +9,11 @@ import ( ) type Corrector struct { - fixValues FixesTable + gridMap *GridMap } /* -FixFromObjectPosition modify steering value according object positions +AdjustFromObjectPosition modify steering value according object positions 1. To compute steering correction, split in image in zones and define correction value for each zone @@ -40,7 +40,7 @@ FixFromObjectPosition modify steering value according object positions 3. If current steering != 0 (turn on left or right), shift right and left values proportionnaly to current steering and apply 2. */ -func (c *Corrector) FixFromObjectPosition(currentSteering float64, objects []*events.Object) float64 { +func (c *Corrector) AdjustFromObjectPosition(currentSteering float64, objects []*events.Object) float64 { // TODO, group rectangle if len(objects) == 0 { @@ -60,13 +60,21 @@ func (c *Corrector) FixFromObjectPosition(currentSteering float64, objects []*ev return currentSteering } + var delta float64 + if nearest.Left < 0 && nearest.Right < 0 { - return currentSteering + c.fixValues.ValueOf(currentSteering, float64(nearest.Right)) + delta, err = c.gridMap.ValueOf(currentSteering, float64(nearest.Right)) } if nearest.Left > 0 && nearest.Right > 0 { - return currentSteering + c.fixValues.ValueOf(currentSteering, float64(nearest.Left)) + delta, err = c.gridMap.ValueOf(currentSteering, float64(nearest.Left)) + } else { + delta, err = c.gridMap.ValueOf(currentSteering, float64(nearest.Left)+(float64(nearest.Right)-float64(nearest.Left))/2.) } - return currentSteering + c.fixValues.ValueOf(currentSteering, float64(nearest.Left)+(float64(nearest.Right)-float64(nearest.Left))/2.) + if err != nil { + zap.S().Warnf("unable to compute delta to apply to steering, skip correction: %v", err) + delta = 0 + } + return currentSteering + delta } // Search if current steering is near of Right or Left @@ -92,12 +100,12 @@ func (c *Corrector) nearObject(objects []*events.Object) (*events.Object, error) return result, nil } -func NewFixesTableFromJson(fileName string) (*FixesTable, error) { +func NewGridMapFromJson(fileName string) (*GridMap, 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 FixesTable + var ft GridMap err = json.Unmarshal(content, &ft) if err != nil { return nil, fmt.Errorf("unable to unmarshal json content from %s file: %w", fileName, err) @@ -106,13 +114,19 @@ func NewFixesTableFromJson(fileName string) (*FixesTable, error) { return &ft, nil } -type FixesTable struct { +type GridMap struct { DistanceSteps []float64 `json:"distance_steps"` SteeringSteps []float64 `json:"steering_steps"` Data [][]float64 `json:"data"` } -func (f *FixesTable) ValueOf(steering float64, distance float64) float64 { +func (f *GridMap) ValueOf(steering float64, distance float64) (float64, error) { + if steering < f.SteeringSteps[0] || steering > f.SteeringSteps[len(f.SteeringSteps)-1] { + return 0., fmt.Errorf("invalid steering value: %v, must be between %v and %v", steering, f.SteeringSteps[0], f.SteeringSteps[len(f.SteeringSteps)-1]) + } + if distance < f.DistanceSteps[0] || distance > f.DistanceSteps[len(f.DistanceSteps)-1] { + return 0., fmt.Errorf("invalid distance value: %v, must be between %v and %v", steering, f.DistanceSteps[0], f.DistanceSteps[len(f.DistanceSteps)-1]) + } // search column index var idxCol int // Start loop at 1 because first column should be skipped @@ -132,5 +146,5 @@ func (f *FixesTable) ValueOf(steering float64, distance float64) float64 { } } - return f.Data[idxRow][idxCol] + return f.Data[idxRow][idxCol], nil } diff --git a/pkg/steering/corrector_test.go b/pkg/steering/corrector_test.go index acbcccb..79d94cd 100644 --- a/pkg/steering/corrector_test.go +++ b/pkg/steering/corrector_test.go @@ -41,6 +41,20 @@ var ( } ) +var ( + defaultGridMap = GridMap{ + DistanceSteps: []float64{0., 0.2, 0.4, 0.6, 0.8, 1.}, + SteeringSteps: []float64{-1., -0.66, -0.33, 0., 0.33, 0.66, 1.}, + Data: [][]float64{ + {0., 0., 0., 0., 0., 0.}, + {0., 0., 0., 0., 0., 0.}, + {0., 0., 0.25, -0.25, 0., 0.}, + {0., 0.25, 0.5, -0.5, -0.25, 0.}, + {0.25, 0.5, 1, -1, -0.5, -0.25}, + }, + } +) + func TestCorrector_FixFromObjectPosition(t *testing.T) { type args struct { currentSteering float64 @@ -130,8 +144,8 @@ func TestCorrector_FixFromObjectPosition(t *testing.T) { for _, tt := range tests { t.Run(tt.name, func(t *testing.T) { c := &Corrector{} - if got := c.FixFromObjectPosition(tt.args.currentSteering, tt.args.objects); got != tt.want { - t.Errorf("FixFromObjectPosition() = %v, want %v", got, tt.want) + if got := c.AdjustFromObjectPosition(tt.args.currentSteering, tt.args.objects); got != tt.want { + t.Errorf("AdjustFromObjectPosition() = %v, want %v", got, tt.want) } }) } @@ -187,14 +201,14 @@ func TestCorrector_nearObject(t *testing.T) { } } -func TestNewFixesTableFromJson(t *testing.T) { +func TestNewGridMapFromJson(t *testing.T) { type args struct { fileName string } tests := []struct { name string args args - want *FixesTable + want *GridMap wantErr bool }{ { @@ -202,18 +216,183 @@ func TestNewFixesTableFromJson(t *testing.T) { args: args{ fileName: "test_data/config.json", }, + want: &defaultGridMap, }, - // TODO: Add test cases. } for _, tt := range tests { t.Run(tt.name, func(t *testing.T) { - got, err := NewFixesTableFromJson(tt.args.fileName) + got, err := NewGridMapFromJson(tt.args.fileName) if (err != nil) != tt.wantErr { - t.Errorf("NewFixesTableFromJson() error = %v, wantErr %v", err, tt.wantErr) + t.Errorf("NewGridMapFromJson() error = %v, wantErr %v", err, tt.wantErr) return } - if !reflect.DeepEqual(got, tt.want) { - t.Errorf("NewFixesTableFromJson() got = %v, want %v", got, tt.want) + if !reflect.DeepEqual(*got, *tt.want) { + t.Errorf("NewGridMapFromJson() got = %v, want %v", got, tt.want) + } + if !reflect.DeepEqual(got.SteeringSteps, tt.want.SteeringSteps) { + t.Errorf("NewGridMapFromJson(), bad steering limits: got = %v, want %v", got.SteeringSteps, tt.want.SteeringSteps) + } + if !reflect.DeepEqual(got.DistanceSteps, tt.want.DistanceSteps) { + t.Errorf("NewGridMapFromJson(), bad distance limits: got = %v, want %v", got.DistanceSteps, tt.want.DistanceSteps) + } + }) + } +} + +func TestGridMap_ValueOf(t *testing.T) { + type fields struct { + DistanceSteps []float64 + SteeringSteps []float64 + Data [][]float64 + } + type args struct { + steering float64 + distance float64 + } + tests := []struct { + name string + fields fields + args args + want float64 + wantErr bool + }{ + { + name: "nominal", + fields: fields{ + DistanceSteps: defaultGridMap.DistanceSteps, + SteeringSteps: defaultGridMap.SteeringSteps, + Data: defaultGridMap.Data, + }, + args: args{ + steering: 0., + distance: 0., + }, + want: 0, + wantErr: false, + }, + { + name: "limit distance <", + fields: fields{ + DistanceSteps: defaultGridMap.DistanceSteps, + SteeringSteps: defaultGridMap.SteeringSteps, + Data: defaultGridMap.Data, + }, + args: args{ + steering: 0, + distance: 0.39999, + }, + want: 0, + wantErr: false, + }, + { + name: "limit distance >", + fields: fields{ + DistanceSteps: defaultGridMap.DistanceSteps, + SteeringSteps: defaultGridMap.SteeringSteps, + Data: defaultGridMap.Data, + }, + args: args{ + steering: 0, + distance: 0.400001, + }, + want: -0.25, + wantErr: false, + }, + { + name: "limit steering <", + fields: fields{ + DistanceSteps: defaultGridMap.DistanceSteps, + SteeringSteps: defaultGridMap.SteeringSteps, + Data: defaultGridMap.Data, + }, + args: args{ + steering: -0.660001, + distance: 0.85, + }, + want: 0.25, + wantErr: false, + }, + { + name: "limit steering >", + fields: fields{ + DistanceSteps: defaultGridMap.DistanceSteps, + SteeringSteps: defaultGridMap.SteeringSteps, + Data: defaultGridMap.Data, + }, + args: args{ + steering: -0.66, + distance: 0.85, + }, + want: 0.5, + wantErr: false, + }, + { + name: "steering < min value", + fields: fields{ + DistanceSteps: defaultGridMap.DistanceSteps, + SteeringSteps: defaultGridMap.SteeringSteps, + Data: defaultGridMap.Data, + }, + args: args{ + steering: defaultGridMap.SteeringSteps[0] - 0.1, + distance: 0.85, + }, + wantErr: true, + }, + { + name: "steering > max value", + fields: fields{ + DistanceSteps: defaultGridMap.DistanceSteps, + SteeringSteps: defaultGridMap.SteeringSteps, + Data: defaultGridMap.Data, + }, + args: args{ + steering: defaultGridMap.SteeringSteps[len(defaultGridMap.SteeringSteps)-1] + 0.1, + distance: 0.85, + }, + wantErr: true, + }, + { + name: "distance < min value", + fields: fields{ + DistanceSteps: defaultGridMap.DistanceSteps, + SteeringSteps: defaultGridMap.SteeringSteps, + Data: defaultGridMap.Data, + }, + args: args{ + steering: -0.65, + distance: defaultGridMap.DistanceSteps[0] - 0.1, + }, + wantErr: true, + }, + { + name: "distance > max value", + fields: fields{ + DistanceSteps: defaultGridMap.DistanceSteps, + SteeringSteps: defaultGridMap.SteeringSteps, + Data: defaultGridMap.Data, + }, + args: args{ + steering: -0.65, + distance: defaultGridMap.DistanceSteps[len(defaultGridMap.DistanceSteps)-1] + 0.1, + }, + wantErr: true, + }, + } + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + f := &GridMap{ + DistanceSteps: tt.fields.DistanceSteps, + SteeringSteps: tt.fields.SteeringSteps, + Data: tt.fields.Data, + } + got, err := f.ValueOf(tt.args.steering, tt.args.distance) + if (err != nil) != tt.wantErr { + t.Errorf("ValueOf() error = %v, wantErr %v", err, tt.wantErr) + return + } + if !reflect.DeepEqual(got, tt.want) { + t.Errorf("ValueOf() = %v, want %v", got, tt.want) } }) } diff --git a/pkg/steering/test_data/config.json b/pkg/steering/test_data/config.json index ae886a4..009313d 100644 --- a/pkg/steering/test_data/config.json +++ b/pkg/steering/test_data/config.json @@ -2,7 +2,6 @@ "steering_steps":[-1, -0.66, -0.33, 0, 0.33, 0.66, 1], "distance_steps": [0, 0.2, 0.4, 0.6, 0.8, 1], "data": [ - [0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0], [0, 0, 0.25, -0.25, 0, 0],