diff --git a/pkg/steering/corrector.go b/pkg/steering/corrector.go index 6727256..f3362d1 100644 --- a/pkg/steering/corrector.go +++ b/pkg/steering/corrector.go @@ -53,22 +53,17 @@ func (c *Corrector) AdjustFromObjectPosition(currentSteering float64, objects [] return currentSteering } - if currentSteering > 0.1 && currentSteering < 0.1 { - - if nearest.Bottom < 0.4 { - // object is far, so no need to fix steering now - return currentSteering - } + if currentSteering > -0.1 && currentSteering < 0.1 { var delta float64 if nearest.Left < 0 && nearest.Right < 0 { - delta, err = c.gridMap.ValueOf(currentSteering, float64(nearest.Right)) + 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(currentSteering, float64(nearest.Left)) + delta, err = c.gridMap.ValueOf(float64(nearest.Left)*2-1., float64(nearest.Bottom)) } else { - delta, err = c.gridMap.ValueOf(currentSteering, float64(nearest.Left)+(float64(nearest.Right)-float64(nearest.Left))/2.) + 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) diff --git a/pkg/steering/corrector_test.go b/pkg/steering/corrector_test.go index 79d94cd..cde7969 100644 --- a/pkg/steering/corrector_test.go +++ b/pkg/steering/corrector_test.go @@ -55,7 +55,7 @@ var ( } ) -func TestCorrector_FixFromObjectPosition(t *testing.T) { +func TestCorrector_AdjustFromObjectPosition(t *testing.T) { type args struct { currentSteering float64 objects []*events.Object @@ -120,7 +120,7 @@ func TestCorrector_FixFromObjectPosition(t *testing.T) { currentSteering: 0., objects: []*events.Object{&objectOnMiddleNear}, }, - want: 0.5, + want: 1, }, { name: "run to left with 1 near object", @@ -143,7 +143,9 @@ func TestCorrector_FixFromObjectPosition(t *testing.T) { } for _, tt := range tests { t.Run(tt.name, func(t *testing.T) { - c := &Corrector{} + 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) }