robocars/robocar-steering
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

wip

Browse Source
This commit is contained in:
Cyrille Nofficial 2022-08-23 22:08:07 +02:00
parent 9100cedb38
commit e6a5d6f781
2 changed files with 108 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

82
pkg/steering/corrector.go
View File

@ -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")

49
pkg/steering/corrector_test.go
View File

@ -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) {