wip

pkg/steering/corrector.go

@@ -9,7 +9,8 @@ import (
 )
 
 type Corrector struct {
-	gridMap *GridMap
+	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,30 +56,71 @@ func (c *Corrector) AdjustFromObjectPosition(currentSteering float64, objects []
 		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
+	if currentSteering > -1*deltaMiddle && currentSteering < deltaMiddle {
+		// Straight
+		return currentSteering + c.computeDeviation(currentSteering, nearest)
 	}
 
-	// Search if current steering is near of Right or Left
+	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))
+	}
+	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,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) {