wip

pkg/steering/corrector.go

@@ -10,6 +10,7 @@ import (
 
 type Corrector struct {
 	gridMap           *GridMap
+	objectMoveFactors *GridMap
 }
 
 /*
@@ -43,6 +44,8 @@ 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
 	}
@@ -53,9 +56,55 @@ func (c *Corrector) AdjustFromObjectPosition(currentSteering float64, objects []
 		return currentSteering
 	}
 
-	if currentSteering > -0.1 && currentSteering < 0.1 {
+	if currentSteering > -1*deltaMiddle && currentSteering < deltaMiddle {
+		// Straight
+		return currentSteering + c.computeDeviation(currentSteering, nearest)
+	}
 
+	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)
+	}
+
+	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))
@@ -72,11 +121,6 @@ func (c *Corrector) AdjustFromObjectPosition(currentSteering float64, objects []
 	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")

pkg/steering/corrector_test.go

@@ -39,6 +39,22 @@ 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 (
@@ -53,6 +69,17 @@ 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) {
@@ -128,7 +155,7 @@ func TestCorrector_AdjustFromObjectPosition(t *testing.T) {
 				currentSteering: -0.9,
 				objects:         []*events.Object{&objectOnMiddleNear},
 			},
-			want: -0.4,
+			want: -0.9,
 		},
 		{
 			name: "run to right with 1 near object",
@@ -136,10 +163,24 @@ func TestCorrector_AdjustFromObjectPosition(t *testing.T) {
 				currentSteering: 0.9,
 				objects:         []*events.Object{&objectOnMiddleNear},
 			},
-			want: 0.4,
+			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,
 		},
-
-		// Todo Object on left/right near/distant
 	}
 	for _, tt := range tests {
 		t.Run(tt.name, func(t *testing.T) {