diff --git a/pkg/steering/corrector.go b/pkg/steering/corrector.go index 22f2436..5617b88 100644 --- a/pkg/steering/corrector.go +++ b/pkg/steering/corrector.go @@ -1,9 +1,11 @@ package steering import ( + "encoding/json" "fmt" "github.com/cyrilix/robocar-protobuf/go/events" "go.uber.org/zap" + "os" ) type Corrector struct { @@ -26,7 +28,7 @@ FixFromObjectPosition modify steering value according object positions 60% |-----|-----|-----|-----|-----|-----| : | 0 | 0.25| 0.5 |-0.5 |-0.25| 0 | 80% |-----|-----|-----|-----|-----|-----| - : |-0.25|-0.5 | 1 | -1 |-0.5 | 0.25| + : | 0.25| 0.5 | 1 | -1 |-0.5 |-0.25| 100%|-----|-----|-----|-----|-----|-----| 2. For straight (current steering near of 0), search nearest object and if: @@ -59,12 +61,12 @@ func (c *Corrector) FixFromObjectPosition(currentSteering float64, objects []*ev } if nearest.Left < 0 && nearest.Right < 0 { - return currentSteering + c.fixValues.ValueOf(currentSteering, nearest.Right) + return currentSteering + c.fixValues.ValueOf(currentSteering, float64(nearest.Right)) } if nearest.Left > 0 && nearest.Right > 0 { - return currentSteering + c.fixValues.ValueOf(currentSteering, nearest.Left) + return currentSteering + c.fixValues.ValueOf(currentSteering, float64(nearest.Left)) } - return currentSteering + c.fixValues.ValueOf(currentSteering, nearest.Left+(nearest.Right-nearest.Left)/2.) + return currentSteering + c.fixValues.ValueOf(currentSteering, float64(nearest.Left)+(float64(nearest.Right)-float64(nearest.Left))/2.) } // Search if current steering is near of Right or Left @@ -90,27 +92,45 @@ func (c *Corrector) nearObject(objects []*events.Object) (*events.Object, error) return result, nil } -type FixesTable struct { - values map[string]map[int]float64 +func NewFixesTableFromJson(fileName string) (*FixesTable, 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 + err = json.Unmarshal(content, &ft) + if err != nil { + return nil, fmt.Errorf("unable to unmarshal json content from %s file: %w", fileName, err) + } + // TODO: check structure is valid + return &ft, nil } -func (f *FixesTable) ValueOf(steering float64, distance float32) float64 { - var key string - if steering < -0.66 { - key = "<-0.66" - } else if steering < -0.33 { - key = "-0.66:-0.33" - } else if steering < 0 { - key = "-0.33:0" - } else if steering < 0.33 { - key = "0:0.33" - } else if steering < 0.66 { - key = "0.33:0.66" - } else { - key = ">= 0.66" +type FixesTable struct { + DistanceSteps []float64 `json:"distance_steps"` + SteeringSteps []float64 `json:"steering_steps"` + Data [][]float64 `json:"data"` +} + +func (f *FixesTable) ValueOf(steering float64, distance float64) float64 { + // search column index + var idxCol int + // Start loop at 1 because first column should be skipped + for i := 1; i < len(f.SteeringSteps); i++ { + if steering < f.SteeringSteps[i] { + idxCol = i - 1 + break + } } - keyDistance := int(distance / 20) - return f.values[key][keyDistance] + var idxRow int + // Start loop at 1 because first column should be skipped + for i := 1; i < len(f.DistanceSteps); i++ { + if distance < f.DistanceSteps[i] { + idxRow = i - 1 + break + } + } + return f.Data[idxRow][idxCol] } diff --git a/pkg/steering/corrector_test.go b/pkg/steering/corrector_test.go index f2a7dee..acbcccb 100644 --- a/pkg/steering/corrector_test.go +++ b/pkg/steering/corrector_test.go @@ -186,3 +186,35 @@ func TestCorrector_nearObject(t *testing.T) { }) } } + +func TestNewFixesTableFromJson(t *testing.T) { + type args struct { + fileName string + } + tests := []struct { + name string + args args + want *FixesTable + wantErr bool + }{ + { + name: "default config", + args: args{ + fileName: "test_data/config.json", + }, + }, + // TODO: Add test cases. + } + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + got, err := NewFixesTableFromJson(tt.args.fileName) + if (err != nil) != tt.wantErr { + t.Errorf("NewFixesTableFromJson() error = %v, wantErr %v", err, tt.wantErr) + return + } + if !reflect.DeepEqual(got, tt.want) { + t.Errorf("NewFixesTableFromJson() got = %v, want %v", got, tt.want) + } + }) + } +} diff --git a/pkg/steering/test_data/config.json b/pkg/steering/test_data/config.json new file mode 100644 index 0000000..ae886a4 --- /dev/null +++ b/pkg/steering/test_data/config.json @@ -0,0 +1,12 @@ +{ + "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], + [0, 0.25, 0.5, -0.5, -0.25, 0], + [0.25, 0.5, 1, -1, -0.5, -0.25] + ] +} \ No newline at end of file