feat(on-objects): Consume objects from mqtt topic

pkg/steering/bbox.go

@@ -0,0 +1,16 @@
+package steering
+
+import (
+	"gocv.io/x/gocv"
+	"image"
+)
+
+func GroupBBoxes(bboxes []image.Rectangle) []image.Rectangle {
+	if len(bboxes) == 0 {
+		return []image.Rectangle{}
+	}
+	if len(bboxes) == 1 {
+		return []image.Rectangle{bboxes[0]}
+	}
+	return gocv.GroupRectangles(bboxes, 1, 0.2)
+}

pkg/steering/bbox_test.go

@@ -0,0 +1,230 @@
+package steering
+
+import (
+	"encoding/json"
+	"fmt"
+	"go.uber.org/zap"
+	"gocv.io/x/gocv"
+	"image"
+	"image/color"
+	"os"
+	"reflect"
+	"testing"
+)
+
+type ObjectsList struct {
+	BBoxes []BBox `json:"bboxes"`
+}
+type BBox struct {
+	Left       float32 `json:"left"`
+	Top        float32 `json:"top"`
+	Bottom     float32 `json:"bottom"`
+	Right      float32 `json:"right"`
+	Confidence float32 `json:"confidence"`
+}
+
+var (
+	dataBBoxes map[string][]image.Rectangle
+	dataImages map[string]*gocv.Mat
+)
+
+func init() {
+	// TODO: empty img without bbox
+	dataNames := []string{"01", "02", "03", "04"}
+	dataBBoxes = make(map[string][]image.Rectangle, len(dataNames))
+	dataImages = make(map[string]*gocv.Mat, len(dataNames))
+
+	for _, dataName := range dataNames {
+		img, bb, err := loadData(dataName)
+		if err != nil {
+			zap.S().Panicf("unable to load data test: %v", err)
+		}
+		dataBBoxes[dataName] = bboxesToRectangles(bb, img.Cols(), img.Rows())
+		dataImages[dataName] = img
+	}
+}
+
+func bboxesToRectangles(bboxes []BBox, imgWidth, imgHeiht int) []image.Rectangle {
+	rects := make([]image.Rectangle, 0, len(bboxes))
+	for _, bb := range bboxes {
+		rects = append(rects, bb.toRect(imgWidth, imgHeiht))
+	}
+	return rects
+}
+
+func (bb *BBox) toRect(imgWidth, imgHeight int) image.Rectangle {
+	return image.Rect(
+		int(bb.Left*float32(imgWidth)),
+		int(bb.Top*float32(imgHeight)),
+		int(bb.Right*float32(imgWidth)),
+		int(bb.Bottom*float32(imgHeight)),
+	)
+}
+
+func loadData(dataName string) (*gocv.Mat, []BBox, error) {
+	contentBBoxes, err := os.ReadFile(fmt.Sprintf("test_data/bboxes-%s.json", dataName))
+	if err != nil {
+		return nil, []BBox{}, fmt.Errorf("unable to load json file for bbox of '%v': %w", dataName, err)
+	}
+
+	var obj ObjectsList
+	err = json.Unmarshal(contentBBoxes, &obj)
+	if err != nil {
+		return nil, []BBox{}, fmt.Errorf("unable to unmarsh json file for bbox of '%v': %w", dataName, err)
+	}
+
+	imgContent, err := os.ReadFile(fmt.Sprintf("test_data/img-%s.jpg", dataName))
+	if err != nil {
+		return nil, []BBox{}, fmt.Errorf("unable to load jpg file of '%v': %w", dataName, err)
+	}
+	img, err := gocv.IMDecode(imgContent, gocv.IMReadUnchanged)
+	if err != nil {
+		return nil, []BBox{}, fmt.Errorf("unable to load jpg of '%v': %w", dataName, err)
+	}
+	return &img, obj.BBoxes, nil
+}
+
+func drawImage(img *gocv.Mat, bboxes []BBox) {
+	for _, bb := range bboxes {
+		gocv.Rectangle(img, bb.toRect(img.Cols(), img.Rows()), color.RGBA{R: 0, G: 255, B: 0, A: 0}, 2)
+		gocv.PutText(
+			img,
+			fmt.Sprintf("%.2f", bb.Confidence),
+			image.Point{
+				X: int(bb.Left*float32(img.Cols()) + 10.),
+				Y: int(bb.Top*float32(img.Rows()) + 10.),
+			},
+			gocv.FontHersheyTriplex,
+			0.4,
+			color.RGBA{R: 0, G: 0, B: 0, A: 0},
+			1)
+	}
+}
+
+func drawRectangles(img *gocv.Mat, rects []image.Rectangle, c color.RGBA) {
+	for _, r := range rects {
+		gocv.Rectangle(img, r, c, 2)
+	}
+}
+
+func saveImage(name string, img *gocv.Mat) error {
+	err := os.MkdirAll("test_result", os.ModePerm)
+	if err != nil {
+		return fmt.Errorf("unable to create directory for test result: %w", err)
+	}
+	jpg, err := gocv.IMEncode(gocv.JPEGFileExt, *img)
+	if err != nil {
+		return fmt.Errorf("unable to encode jpg image: %w", err)
+	}
+	defer jpg.Close()
+
+	err = os.WriteFile(fmt.Sprintf("test_result/%s.jpg", name), jpg.GetBytes(), os.ModePerm)
+	if err != nil {
+		return fmt.Errorf("unable to write jpeg file: %w", err)
+	}
+	return nil
+}
+
+func DisplayImageAndBBoxes(dataName string) error {
+	img, bboxes, err := loadData(dataName)
+	if err != nil {
+		return fmt.Errorf("unable to load image and bboxes: %w", err)
+	}
+	drawImage(img, bboxes)
+	err = saveImage(dataName, img)
+	if err != nil {
+		return fmt.Errorf("unable to save image: %w", err)
+	}
+	return nil
+}
+
+func TestDisplayBBox(t *testing.T) {
+
+	type args struct {
+		dataName string
+	}
+	tests := []struct {
+		name string
+		args args
+	}{
+		{
+			name: "default",
+			args: args{dataName: "01"},
+		},
+		{
+			name: "02",
+			args: args{dataName: "02"},
+		},
+		{
+			name: "03",
+			args: args{dataName: "03"},
+		},
+		{
+			name: "04",
+			args: args{dataName: "04"},
+		},
+	}
+
+	for _, tt := range tests {
+		t.Run(tt.name, func(t *testing.T) {
+			err := DisplayImageAndBBoxes(tt.args.dataName)
+			if err != nil {
+				t.Errorf("unable to draw image: %v", err)
+			}
+		})
+	}
+
+}
+
+func TestGroupBBoxes(t *testing.T) {
+	type args struct {
+		dataName string
+	}
+	tests := []struct {
+		name string
+		args args
+		want []image.Rectangle
+	}{
+		{
+			name: "groupbbox-01",
+			args: args{
+				dataName: "01",
+			},
+			want: []image.Rectangle{{Min: image.Point{X: 42, Y: 20}, Max: image.Point{X: 84, Y: 57}}},
+		},
+		{
+			name: "groupbbox-02",
+			args: args{
+				dataName: "02",
+			},
+			want: []image.Rectangle{{Min: image.Point{X: 25, Y: 13}, Max: image.Point{X: 110, Y: 80}}},
+		},
+		{
+			name: "groupbbox-03",
+			args: args{
+				dataName: "03",
+			},
+			want: []image.Rectangle{{Min: image.Point{X: 0, Y: 17}, Max: image.Point{X: 35, Y: 77}}},
+		},
+		{
+			name: "groupbbox-04",
+			args: args{
+				dataName: "04",
+			},
+			want: []image.Rectangle{{Min: image.Point{X: 129, Y: 10}, Max: image.Point{X: 159, Y: 64}}},
+		},
+	}
+	for _, tt := range tests {
+		t.Run(tt.name, func(t *testing.T) {
+			got := GroupBBoxes(dataBBoxes[tt.args.dataName])
+			img := gocv.NewMat()
+			defer img.Close()
+			dataImages[tt.args.dataName].CopyTo(&img)
+			drawRectangles(&img, got, color.RGBA{R: 0, G: 0, B: 255, A: 0})
+			saveImage(tt.name, &img)
+			if !reflect.DeepEqual(got, tt.want) {
+				t.Errorf("GroupBBoxes() = %v, want %v", got, tt.want)
+			}
+		})
+	}
+}

pkg/steering/controller.go

@@ -9,13 +9,14 @@ import (
 	"sync"
 )
 
-func NewController(client mqtt.Client, steeringTopic, driveModeTopic, rcSteeringTopic, tfSteeringTopic string, debug bool) *Controller {
+func NewController(client mqtt.Client, steeringTopic, driveModeTopic, rcSteeringTopic, tfSteeringTopic, objectsTopic string) *Controller {
 	return &Controller{
 		client:          client,
 		steeringTopic:   steeringTopic,
 		driveModeTopic:  driveModeTopic,
 		rcSteeringTopic: rcSteeringTopic,
 		tfSteeringTopic: tfSteeringTopic,
+		objectsTopic:    objectsTopic,
 		driveMode:       events.DriveMode_USER,
 	}
 
@@ -28,8 +29,11 @@ type Controller struct {
 	muDriveMode sync.RWMutex
 	driveMode   events.DriveMode
 
-	cancel                                           chan interface{}
-	driveModeTopic, rcSteeringTopic, tfSteeringTopic string
+	cancel                                                         chan interface{}
+	driveModeTopic, rcSteeringTopic, tfSteeringTopic, objectsTopic string
+
+	muObjects sync.RWMutex
+	objects   []*events.Object
 
 	debug bool
 }
@@ -50,6 +54,19 @@ func (p *Controller) Stop() {
 	service.StopService("throttle", p.client, p.driveModeTopic, p.rcSteeringTopic, p.tfSteeringTopic)
 }
 
+func (p *Controller) onObjects(_ mqtt.Client, message mqtt.Message) {
+	var msg events.ObjectsMessage
+	err := proto.Unmarshal(message.Payload(), &msg)
+	if err != nil {
+		zap.S().Errorf("unable to unmarshal protobuf %T message: %v", msg, err)
+		return
+	}
+
+	p.muObjects.Lock()
+	defer p.muObjects.Unlock()
+	p.objects = msg.GetObjects()
+}
+
 func (p *Controller) onDriveMode(_ mqtt.Client, message mqtt.Message) {
 	var msg events.DriveModeMessage
 	err := proto.Unmarshal(message.Payload(), &msg)
@@ -115,6 +132,11 @@ var registerCallbacks = func(p *Controller) error {
 	if err != nil {
 		return err
 	}
+
+	err = service.RegisterCallback(p.client, p.objectsTopic, p.onObjects)
+	if err != nil {
+		return err
+	}
 	return nil
 }
 

pkg/steering/controller_test.go

@@ -33,50 +33,58 @@ func TestDefaultSteering(t *testing.T) {
 	driveModeTopic := "topic/driveMode"
 	rcSteeringTopic := "topic/rcSteering"
 	tfSteeringTopic := "topic/tfSteering"
+	objectsTopic := "topic/objects"
 
-	p := NewController(nil, steeringTopic, driveModeTopic, rcSteeringTopic, tfSteeringTopic, true)
+	p := NewController(nil, steeringTopic, driveModeTopic, rcSteeringTopic, tfSteeringTopic, objectsTopic)
 
 	cases := []struct {
 		driveMode        events.DriveModeMessage
 		rcSteering       events.SteeringMessage
 		tfSteering       events.SteeringMessage
 		expectedSteering events.SteeringMessage
+		objects          events.ObjectsMessage
 	}{
 		{
 			events.DriveModeMessage{DriveMode: events.DriveMode_USER},
 			events.SteeringMessage{Steering: 0.3, Confidence: 1.0},
 			events.SteeringMessage{Steering: 0.4, Confidence: 1.0},
 			events.SteeringMessage{Steering: 0.3, Confidence: 1.0},
+			events.ObjectsMessage{},
 		},
 		{
 			events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
 			events.SteeringMessage{Steering: 0.5, Confidence: 1.0},
 			events.SteeringMessage{Steering: 0.6, Confidence: 1.0},
 			events.SteeringMessage{Steering: 0.6, Confidence: 1.0},
+			events.ObjectsMessage{},
 		},
 		{
 			events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
 			events.SteeringMessage{Steering: 0.4, Confidence: 1.0},
 			events.SteeringMessage{Steering: 0.7, Confidence: 1.0},
 			events.SteeringMessage{Steering: 0.7, Confidence: 1.0},
+			events.ObjectsMessage{},
 		},
 		{
 			events.DriveModeMessage{DriveMode: events.DriveMode_USER},
 			events.SteeringMessage{Steering: 0.5, Confidence: 1.0},
 			events.SteeringMessage{Steering: 0.8, Confidence: 1.0},
 			events.SteeringMessage{Steering: 0.5, Confidence: 1.0},
+			events.ObjectsMessage{},
 		},
 		{
 			events.DriveModeMessage{DriveMode: events.DriveMode_USER},
 			events.SteeringMessage{Steering: 0.4, Confidence: 1.0},
 			events.SteeringMessage{Steering: 0.9, Confidence: 1.0},
 			events.SteeringMessage{Steering: 0.4, Confidence: 1.0},
+			events.ObjectsMessage{},
 		},
 		{
 			events.DriveModeMessage{DriveMode: events.DriveMode_USER},
 			events.SteeringMessage{Steering: 0.6, Confidence: 1.0},
 			events.SteeringMessage{Steering: -0.3, Confidence: 1.0},
 			events.SteeringMessage{Steering: 0.6, Confidence: 1.0},
+			events.ObjectsMessage{},
 		},
 	}
 
@@ -88,6 +96,7 @@ func TestDefaultSteering(t *testing.T) {
 		p.onDriveMode(nil, testtools.NewFakeMessageFromProtobuf(driveModeTopic, &c.driveMode))
 		p.onRCSteering(nil, testtools.NewFakeMessageFromProtobuf(rcSteeringTopic, &c.rcSteering))
 		p.onTFSteering(nil, testtools.NewFakeMessageFromProtobuf(tfSteeringTopic, &c.tfSteering))
+		p.onObjects(nil, testtools.NewFakeMessageFromProtobuf(objectsTopic, &c.objects))
 
 		time.Sleep(10 * time.Millisecond)
 

pkg/steering/corrector.go

@@ -0,0 +1,145 @@
+package steering
+
+import (
+	"encoding/json"
+	"fmt"
+	"github.com/cyrilix/robocar-protobuf/go/events"
+	"go.uber.org/zap"
+	"os"
+)
+
+type Corrector struct {
+	gridMap *GridMap
+}
+
+/*
+AdjustFromObjectPosition modify steering value according object positions
+
+ 1. To compute steering correction, split in image in zones and define correction value for each zone
+
+    Steering computed
+    :  -1   -0.66 -0.33   0    0.33  0.66   1
+    0%  |-----|-----|-----|-----|-----|-----|
+    :   |  0  |  0  |  0  |  0  |  0  |  0  |
+    20% |-----|-----|-----|-----|-----|-----|
+    :   |  0  |  0  |  0  |  0  |  0  |  0  |
+    40% |-----|-----|-----|-----|-----|-----|
+    :   |  0  |  0  | 0.25|-0.25|  0  |  0  |
+    60% |-----|-----|-----|-----|-----|-----|
+    :   |  0  | 0.25| 0.5 |-0.5 |-0.25|  0  |
+    80% |-----|-----|-----|-----|-----|-----|
+    :   | 0.25| 0.5 |  1  |  -1 |-0.5 |-0.25|
+    100%|-----|-----|-----|-----|-----|-----|
+
+ 2. For straight (current steering near of 0), search nearest object and if:
+
+    * left and right values < 0: use correction from right value according image splitting
+    * left and right values > 0: use correction from left value according image splitting
+    * left < 0 and right values > 0: use (right + (right - left) / 2) value
+
+ 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) AdjustFromObjectPosition(currentSteering float64, objects []*events.Object) float64 {
+	// TODO, group rectangle
+
+	if len(objects) == 0 {
+		return currentSteering
+	}
+	// get nearest object
+	nearest, err := c.nearObject(objects)
+	if err != nil {
+		zap.S().Warnf("unexpected error on nearest seach object, ignore objects: %v", err)
+		return currentSteering
+	}
+
+	if currentSteering > -0.1 && currentSteering < 0.1 {
+
+		var delta float64
+
+		if nearest.Left < 0 && nearest.Right < 0 {
+			delta, err = c.gridMap.ValueOf(float64(nearest.Right)*2-1., float64(nearest.Bottom))
+		}
+		if nearest.Left > 0 && nearest.Right > 0 {
+			delta, err = c.gridMap.ValueOf(float64(nearest.Left)*2-1., float64(nearest.Bottom))
+		} else {
+			delta, err = c.gridMap.ValueOf(float64(float64(nearest.Left)+(float64(nearest.Right)-float64(nearest.Left))/2.)*2.-1., float64(nearest.Bottom))
+		}
+		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
+
+	return currentSteering
+}
+
+func (c *Corrector) nearObject(objects []*events.Object) (*events.Object, error) {
+	if len(objects) == 0 {
+		return nil, fmt.Errorf("list objects must contain at least one object")
+	}
+	if len(objects) == 1 {
+		return objects[0], nil
+	}
+
+	var result *events.Object
+	for _, obj := range objects {
+		if result == nil || obj.Bottom > result.Bottom {
+			result = obj
+			continue
+		}
+	}
+	return result, nil
+}
+
+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 GridMap
+	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 GridMap struct {
+	DistanceSteps []float64   `json:"distance_steps"`
+	SteeringSteps []float64   `json:"steering_steps"`
+	Data          [][]float64 `json:"data"`
+}
+
+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
+	for i := 1; i < len(f.SteeringSteps); i++ {
+		if steering < f.SteeringSteps[i] {
+			idxCol = i - 1
+			break
+		}
+	}
+
+	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], nil
+}

pkg/steering/corrector_test.go

@@ -0,0 +1,401 @@
+package steering
+
+import (
+	"github.com/cyrilix/robocar-protobuf/go/events"
+	"reflect"
+	"testing"
+)
+
+var (
+	objectOnMiddleDistant = events.Object{
+		Type:       events.TypeObject_ANY,
+		Left:       0.4,
+		Top:        0.1,
+		Right:      0.6,
+		Bottom:     0.2,
+		Confidence: 0.9,
+	}
+	objectOnLeftDistant = events.Object{
+		Type:       events.TypeObject_ANY,
+		Left:       0.1,
+		Top:        0.09,
+		Right:      0.3,
+		Bottom:     0.19,
+		Confidence: 0.9,
+	}
+	objectOnRightDistant = events.Object{
+		Type:       events.TypeObject_ANY,
+		Left:       0.7,
+		Top:        0.21,
+		Right:      0.9,
+		Bottom:     0.11,
+		Confidence: 0.9,
+	}
+	objectOnMiddleNear = events.Object{
+		Type:       events.TypeObject_ANY,
+		Left:       0.4,
+		Top:        0.8,
+		Right:      0.6,
+		Bottom:     0.9,
+		Confidence: 0.9,
+	}
+)
+
+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_AdjustFromObjectPosition(t *testing.T) {
+	type args struct {
+		currentSteering float64
+		objects         []*events.Object
+	}
+	tests := []struct {
+		name string
+		args args
+		want float64
+	}{
+		{
+			name: "run straight without objects",
+			args: args{
+				currentSteering: 0.,
+				objects:         []*events.Object{},
+			},
+			want: 0.,
+		},
+		{
+			name: "run to left without objects",
+			args: args{
+				currentSteering: -0.9,
+				objects:         []*events.Object{},
+			},
+			want: -0.9,
+		},
+		{
+			name: "run to right without objects",
+			args: args{
+				currentSteering: 0.9,
+				objects:         []*events.Object{},
+			}, want: 0.9,
+		},
+
+		{
+			name: "run straight with 1 distant object",
+			args: args{
+				currentSteering: 0.,
+				objects:         []*events.Object{&objectOnMiddleDistant},
+			},
+			want: 0.,
+		},
+		{
+			name: "run to left with 1 distant object",
+			args: args{
+				currentSteering: -0.9,
+				objects:         []*events.Object{&objectOnMiddleDistant},
+			},
+			want: -0.9,
+		},
+		{
+			name: "run to right with 1 distant object",
+			args: args{
+				currentSteering: 0.9,
+				objects:         []*events.Object{&objectOnMiddleDistant},
+			},
+			want: 0.9,
+		},
+
+		{
+			name: "run straight with 1 near object",
+			args: args{
+				currentSteering: 0.,
+				objects:         []*events.Object{&objectOnMiddleNear},
+			},
+			want: 1,
+		},
+		{
+			name: "run to left with 1 near object",
+			args: args{
+				currentSteering: -0.9,
+				objects:         []*events.Object{&objectOnMiddleNear},
+			},
+			want: -0.4,
+		},
+		{
+			name: "run to right with 1 near object",
+			args: args{
+				currentSteering: 0.9,
+				objects:         []*events.Object{&objectOnMiddleNear},
+			},
+			want: 0.4,
+		},
+
+		// Todo Object on left/right near/distant
+	}
+	for _, tt := range tests {
+		t.Run(tt.name, func(t *testing.T) {
+			c := &Corrector{
+				gridMap: &defaultGridMap,
+			}
+			if got := c.AdjustFromObjectPosition(tt.args.currentSteering, tt.args.objects); got != tt.want {
+				t.Errorf("AdjustFromObjectPosition() = %v, want %v", got, tt.want)
+			}
+		})
+	}
+}
+
+func TestCorrector_nearObject(t *testing.T) {
+	type args struct {
+		objects []*events.Object
+	}
+	tests := []struct {
+		name    string
+		args    args
+		want    *events.Object
+		wantErr bool
+	}{
+		{
+			name: "List object is empty",
+			args: args{
+				objects: []*events.Object{},
+			},
+			want:    nil,
+			wantErr: true,
+		},
+		{
+			name: "List with only one object",
+			args: args{
+				objects: []*events.Object{&objectOnMiddleNear},
+			},
+			want:    &objectOnMiddleNear,
+			wantErr: false,
+		},
+		{
+			name: "List with many objects",
+			args: args{
+				objects: []*events.Object{&objectOnLeftDistant, &objectOnMiddleNear, &objectOnRightDistant, &objectOnMiddleDistant},
+			},
+			want:    &objectOnMiddleNear,
+			wantErr: false,
+		},
+	}
+	for _, tt := range tests {
+		t.Run(tt.name, func(t *testing.T) {
+			c := &Corrector{}
+			got, err := c.nearObject(tt.args.objects)
+			if (err != nil) != tt.wantErr {
+				t.Errorf("nearObject() error = %v, wantErr %v", err, tt.wantErr)
+				return
+			}
+			if !reflect.DeepEqual(got, tt.want) {
+				t.Errorf("nearObject() got = %v, want %v", got, tt.want)
+			}
+		})
+	}
+}
+
+func TestNewGridMapFromJson(t *testing.T) {
+	type args struct {
+		fileName string
+	}
+	tests := []struct {
+		name    string
+		args    args
+		want    *GridMap
+		wantErr bool
+	}{
+		{
+			name: "default config",
+			args: args{
+				fileName: "test_data/config.json",
+			},
+			want: &defaultGridMap,
+		},
+	}
+	for _, tt := range tests {
+		t.Run(tt.name, func(t *testing.T) {
+			got, err := NewGridMapFromJson(tt.args.fileName)
+			if (err != nil) != tt.wantErr {
+				t.Errorf("NewGridMapFromJson() error = %v, wantErr %v", err, tt.wantErr)
+				return
+			}
+			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)
+			}
+		})
+	}
+}

pkg/steering/test_data/bboxes-01.json

@@ -0,0 +1,12 @@
+{
+  "bboxes": [
+    {
+      "right": 0.5258789,
+      "top": 0.1706543,
+      "left": 0.26660156,
+      "bottom": 0.47583008,
+      "confidence": 0.4482422
+    }
+  ]
+}
+

pkg/steering/test_data/bboxes-02.json

@@ -0,0 +1,25 @@
+{
+  "bboxes": [
+    {
+      "right": 0.6879883,
+      "top": 0.115234375,
+      "left": 0.1586914,
+      "bottom": 0.66796875,
+      "confidence": 0.82714844
+    },
+    {
+      "right": 0.2794531,
+      "top": 0.301816406,
+      "left": 0.15698242,
+      "bottom": 0.65748047,
+      "confidence": 0.83447266
+    },
+    {
+      "right": 0.6875,
+      "top": 0.11328125,
+      "left": 0.15673828,
+      "bottom": 0.66748047,
+      "confidence": 0.85253906
+    }
+  ]
+}

pkg/steering/test_data/bboxes-03.json

@@ -0,0 +1,27 @@
+{
+  "bboxes": [
+    {
+      "right": 0.2211914,
+      "top": 0.14953613,
+      "left": 0.0015258789,
+      "bottom": 0.64941406,
+      "confidence": 0.5595703
+    },
+    {
+      "right": 0.22192383,
+      "top": 0.14819336,
+      "left": 0.0014038086,
+      "bottom": 0.64941406,
+      "confidence": 0.5493164
+    },
+    {
+      "right": 0.21948242,
+      "top": 0.1459961,
+      "left": 0.0015258789,
+      "bottom": 0.65185547,
+      "confidence": 0.5595703
+    }
+  ]
+}
+
+

pkg/steering/test_data/bboxes-04.json

@@ -0,0 +1,25 @@
+{
+  "bboxes": [
+    {
+      "right": 0.99902344,
+      "top": 0.08947754,
+      "left": 0.8095703,
+      "bottom": 0.54296875,
+      "confidence": 0.4741211
+    },
+    {
+      "right": 0.99902344,
+      "top": 0.08666992,
+      "left": 0.80859375,
+      "bottom": 0.54003906,
+      "confidence": 0.453125
+    },
+    {
+      "right": 0.99902344,
+      "top": 0.09423828,
+      "left": 0.8095703,
+      "bottom": 0.54345703,
+      "confidence": 0.44995117
+    }
+  ]
+}

pkg/steering/test_data/config.json

@@ -0,0 +1,11 @@
+{
+  "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.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]
+  ]
+}