From 23215bcb4af247dc080c072b052c9441bd67b996 Mon Sep 17 00:00:00 2001 From: Cyrille Nofficial Date: Mon, 22 Aug 2022 14:06:27 +0200 Subject: [PATCH] wip --- pkg/steering/corrector.go | 46 ++++++++ pkg/steering/corrector_test.go | 188 +++++++++++++++++++++++++++++++++ 2 files changed, 234 insertions(+) create mode 100644 pkg/steering/corrector.go create mode 100644 pkg/steering/corrector_test.go diff --git a/pkg/steering/corrector.go b/pkg/steering/corrector.go new file mode 100644 index 0000000..734245a --- /dev/null +++ b/pkg/steering/corrector.go @@ -0,0 +1,46 @@ +package steering + +import ( + "fmt" + "github.com/cyrilix/robocar-protobuf/go/events" + "go.uber.org/zap" +) + +type Corrector struct { +} + +func (c *Corrector) FixFromObjectPosition(currentSteering float64, objects []*events.Object) float64 { + // TODO, group rectangle + + if len(objects) == 0 { + return currentSteering + } + // get nearest object + nearest, err := c.nearObject(objects) + if err != nil { + zap.S().Warnf("unexpected error on nearest seach object, ignore objects: %v", err) + return currentSteering + } + + // 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") + } + if len(objects) == 1 { + return objects[0], nil + } + + var result *events.Object + for _, obj := range objects { + if result == nil || obj.Bottom > result.Bottom { + result = obj + continue + } + } + return result, nil +} diff --git a/pkg/steering/corrector_test.go b/pkg/steering/corrector_test.go new file mode 100644 index 0000000..f2a7dee --- /dev/null +++ b/pkg/steering/corrector_test.go @@ -0,0 +1,188 @@ +package steering + +import ( + "github.com/cyrilix/robocar-protobuf/go/events" + "reflect" + "testing" +) + +var ( + objectOnMiddleDistant = events.Object{ + Type: events.TypeObject_ANY, + Left: 0.4, + Top: 0.1, + Right: 0.6, + Bottom: 0.2, + Confidence: 0.9, + } + objectOnLeftDistant = events.Object{ + Type: events.TypeObject_ANY, + Left: 0.1, + Top: 0.09, + Right: 0.3, + Bottom: 0.19, + Confidence: 0.9, + } + objectOnRightDistant = events.Object{ + Type: events.TypeObject_ANY, + Left: 0.7, + Top: 0.21, + Right: 0.9, + Bottom: 0.11, + Confidence: 0.9, + } + objectOnMiddleNear = events.Object{ + Type: events.TypeObject_ANY, + Left: 0.4, + Top: 0.8, + Right: 0.6, + Bottom: 0.9, + Confidence: 0.9, + } +) + +func TestCorrector_FixFromObjectPosition(t *testing.T) { + type args struct { + currentSteering float64 + objects []*events.Object + } + tests := []struct { + name string + args args + want float64 + }{ + { + name: "run straight without objects", + args: args{ + currentSteering: 0., + objects: []*events.Object{}, + }, + want: 0., + }, + { + name: "run to left without objects", + args: args{ + currentSteering: -0.9, + objects: []*events.Object{}, + }, + want: -0.9, + }, + { + name: "run to right without objects", + args: args{ + currentSteering: 0.9, + objects: []*events.Object{}, + }, want: 0.9, + }, + + { + name: "run straight with 1 distant object", + args: args{ + currentSteering: 0., + objects: []*events.Object{&objectOnMiddleDistant}, + }, + want: 0., + }, + { + name: "run to left with 1 distant object", + args: args{ + currentSteering: -0.9, + objects: []*events.Object{&objectOnMiddleDistant}, + }, + want: -0.9, + }, + { + name: "run to right with 1 distant object", + args: args{ + currentSteering: 0.9, + objects: []*events.Object{&objectOnMiddleDistant}, + }, + want: 0.9, + }, + + { + name: "run straight with 1 near object", + args: args{ + currentSteering: 0., + objects: []*events.Object{&objectOnMiddleNear}, + }, + want: 0.5, + }, + { + name: "run to left with 1 near object", + args: args{ + currentSteering: -0.9, + objects: []*events.Object{&objectOnMiddleNear}, + }, + want: -0.4, + }, + { + name: "run to right with 1 near object", + args: args{ + currentSteering: 0.9, + objects: []*events.Object{&objectOnMiddleNear}, + }, + want: 0.4, + }, + + // Todo Object on left/right near/distant + } + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + c := &Corrector{} + if got := c.FixFromObjectPosition(tt.args.currentSteering, tt.args.objects); got != tt.want { + t.Errorf("FixFromObjectPosition() = %v, want %v", got, tt.want) + } + }) + } +} + +func TestCorrector_nearObject(t *testing.T) { + type args struct { + objects []*events.Object + } + tests := []struct { + name string + args args + want *events.Object + wantErr bool + }{ + { + name: "List object is empty", + args: args{ + objects: []*events.Object{}, + }, + want: nil, + wantErr: true, + }, + { + name: "List with only one object", + args: args{ + objects: []*events.Object{&objectOnMiddleNear}, + }, + want: &objectOnMiddleNear, + wantErr: false, + }, + { + name: "List with many objects", + args: args{ + objects: []*events.Object{&objectOnLeftDistant, &objectOnMiddleNear, &objectOnRightDistant, &objectOnMiddleDistant}, + }, + want: &objectOnMiddleNear, + wantErr: false, + }, + } + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + c := &Corrector{} + got, err := c.nearObject(tt.args.objects) + if (err != nil) != tt.wantErr { + t.Errorf("nearObject() error = %v, wantErr %v", err, tt.wantErr) + return + } + if !reflect.DeepEqual(got, tt.want) { + t.Errorf("nearObject() got = %v, want %v", got, tt.want) + } + }) + } +}