WIP

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
 }
 
+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
+}
+
 type FixesTable struct {
-	values map[string]map[int]float64
+	DistanceSteps []float64   `json:"distance_steps"`
+	SteeringSteps []float64   `json:"steering_steps"`
+	Data          [][]float64 `json:"data"`
 }
 
-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"
+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]
 }

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

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