Dockerfile

@@ -1,4 +1,4 @@
-FROM --platform=$BUILDPLATFORM golang:1.19-alpine AS builder
+FROM --platform=$BUILDPLATFORM golang:1.17-alpine AS builder
 
 ARG TARGETPLATFORM
 ARG BUILDPLATFORM

go.mod

@@ -1,6 +1,6 @@
 module github.com/cyrilix/robocar-steering
 
-go 1.19
+go 1.18
 
 require (
 	github.com/cyrilix/robocar-base v0.1.7

go.sum

@@ -2,6 +2,8 @@ github.com/benbjohnson/clock v1.1.0 h1:Q92kusRqC1XV2MjkWETPvjJVqKetz1OzxZB7mHJLj
 github.com/benbjohnson/clock v1.1.0/go.mod h1:J11/hYXuz8f4ySSvYwY0FKfm+ezbsZBKZxNJlLklBHA=
 github.com/cyrilix/robocar-base v0.1.7 h1:EVzZ0KjigSFpke5f3A/PybEH3WFUEIrYSc3z/dhOZ48=
 github.com/cyrilix/robocar-base v0.1.7/go.mod h1:4E11HQSNy2NT8e7MW188y6ST9C0RzarKyn7sK/3V/Lk=
+github.com/cyrilix/robocar-protobuf/go v1.0.5 h1:PX1At+pf6G7gJwT4LzJLQu3/LPFTTNNlZmZSYtnSELY=
+github.com/cyrilix/robocar-protobuf/go v1.0.5/go.mod h1:Y3AE28K5V7EZxMXp/6A8RhkRz15VOfFy4CjST35FbtQ=
 github.com/cyrilix/robocar-protobuf/go v1.1.0 h1:txIjGnnCF3UzedpsWu+sL7nMA+pNjSnX6HZlAmuReH4=
 github.com/cyrilix/robocar-protobuf/go v1.1.0/go.mod h1:Y3AE28K5V7EZxMXp/6A8RhkRz15VOfFy4CjST35FbtQ=
 github.com/davecgh/go-spew v1.1.0/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSsI+c5H38=

pkg/steering/corrector.go

@@ -9,8 +9,7 @@ import (
 )
 
 type Corrector struct {
-	gridMap           *GridMap
-	objectMoveFactors *GridMap
+	gridMap *GridMap
 }
 
 /*
@@ -44,8 +43,6 @@ AdjustFromObjectPosition modify steering value according object positions
 func (c *Corrector) AdjustFromObjectPosition(currentSteering float64, objects []*events.Object) float64 {
 	// TODO, group rectangle
 
-	var deltaMiddle = 0.1
-
 	if len(objects) == 0 {
 		return currentSteering
 	}
@@ -56,71 +53,30 @@ func (c *Corrector) AdjustFromObjectPosition(currentSteering float64, objects []
 		return currentSteering
 	}
 
-	if currentSteering > -1*deltaMiddle && currentSteering < deltaMiddle {
-		// Straight
-		return currentSteering + c.computeDeviation(currentSteering, nearest)
+	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
 	}
 
-	if currentSteering < -1*deltaMiddle {
-		// Turn to left, so search to avoid collision with objects on the left
-		// Apply factor to object to move it at middle. This factor is function of distance
-		factor, err := c.objectMoveFactors.ValueOf(float64(nearest.Left), float64(nearest.Bottom))
-		if err != nil {
-			zap.S().Warnf("unable to compute factor to apply to object: %v", err)
-			return currentSteering
-		}
-		objMoved := events.Object{
-			Type:       nearest.Type,
-			Left:       nearest.Left * float32(factor),
-			Top:        nearest.Top,
-			Right:      nearest.Right * float32(factor),
-			Bottom:     nearest.Bottom,
-			Confidence: nearest.Confidence,
-		}
-		return currentSteering + c.computeDeviation(currentSteering, &objMoved)
-	}
-
-	if currentSteering > deltaMiddle {
-		// Turn to right, so search to avoid collision with objects on the right
-		// Apply factor to object to move it at middle. This factor is function of distance
-		factor, err := c.objectMoveFactors.ValueOf(float64(nearest.Left), float64(nearest.Bottom))
-		if err != nil {
-			zap.S().Warnf("unable to compute factor to apply to object: %v", err)
-			return currentSteering
-		}
-		objMoved := events.Object{
-			Type:       nearest.Type,
-			Left:       nearest.Left * float32(factor),
-			Top:        nearest.Top,
-			Right:      nearest.Right * float32(factor),
-			Bottom:     nearest.Bottom,
-			Confidence: nearest.Confidence,
-		}
-		return currentSteering + c.computeDeviation(currentSteering, &objMoved)
-	}
+	// Search if current steering is near of Right or Left
 
 	return currentSteering
 }
 
-func (c *Corrector) computeDeviation(currentSteering float64, nearest *events.Object) float64 {
-	var delta float64
-	var err error
-
-	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
-}
-
 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")

pkg/steering/corrector_test.go

@@ -39,22 +39,6 @@ var (
 		Bottom:     0.9,
 		Confidence: 0.9,
 	}
-	objectOnRightNear = events.Object{
-		Type:       events.TypeObject_ANY,
-		Left:       0.7,
-		Top:        0.8,
-		Right:      0.9,
-		Bottom:     0.9,
-		Confidence: 0.9,
-	}
-	objectOnLeftNear = events.Object{
-		Type:       events.TypeObject_ANY,
-		Left:       0.1,
-		Top:        0.8,
-		Right:      0.3,
-		Bottom:     0.9,
-		Confidence: 0.9,
-	}
 )
 
 var (
@@ -69,17 +53,6 @@ var (
 			{0.25, 0.5, 1, -1, -0.5, -0.25},
 		},
 	}
-	defaultObjectFactors = 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) {
@@ -155,7 +128,7 @@ func TestCorrector_AdjustFromObjectPosition(t *testing.T) {
 				currentSteering: -0.9,
 				objects:         []*events.Object{&objectOnMiddleNear},
 			},
-			want: -0.9,
+			want: -0.4,
 		},
 		{
 			name: "run to right with 1 near object",
@@ -163,24 +136,10 @@ func TestCorrector_AdjustFromObjectPosition(t *testing.T) {
 				currentSteering: 0.9,
 				objects:         []*events.Object{&objectOnMiddleNear},
 			},
-			want: 0.9,
-		},
-		{
-			name: "run to right with 1 near object on the right",
-			args: args{
-				currentSteering: 0.9,
-				objects:         []*events.Object{&objectOnRightNear},
-			},
-			want: 0.1,
-		},
-		{
-			name: "run to left with 1 near object on the left",
-			args: args{
-				currentSteering: -0.9,
-				objects:         []*events.Object{&objectOnLeftNear},
-			},
-			want: -0.1,
+			want: 0.4,
 		},
+
+		// Todo Object on left/right near/distant
 	}
 	for _, tt := range tests {
 		t.Run(tt.name, func(t *testing.T) {