From e6a5d6f7819d2e76d609ab2c4618ed1fbffdb76e Mon Sep 17 00:00:00 2001 From: Cyrille Nofficial Date: Tue, 23 Aug 2022 22:08:07 +0200 Subject: [PATCH] wip --- pkg/steering/corrector.go | 82 ++++++++++++++++++++++++++-------- pkg/steering/corrector_test.go | 49 ++++++++++++++++++-- 2 files changed, 108 insertions(+), 23 deletions(-) diff --git a/pkg/steering/corrector.go b/pkg/steering/corrector.go index f3362d1..12bd96f 100644 --- a/pkg/steering/corrector.go +++ b/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") diff --git a/pkg/steering/corrector_test.go b/pkg/steering/corrector_test.go index cde7969..4712da7 100644 --- a/pkg/steering/corrector_test.go +++ b/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) {