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-24 13:54:36 +02:00
parent e6a5d6f781
commit bd4c5d45a4
2 changed files with 24 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
pkg/steering/corrector.go
View File

@ -40,6 +40,14 @@ AdjustFromObjectPosition modify steering value according object positions
3. If current steering != 0 (turn on left or right), shift right and left values proportionnaly to current steering and
apply 2.
: -1 -0.66 -0.33 0 0.33 0.66 1
0% |-----|-----|-----|-----|-----|-----|
: | 0 | 0 | 0 | 0 | 0 | 0 |
20% |-----|-----|-----|-----|-----|-----|
: | 0.2 | 0.1 | 0 | 0 |-0.1 |-0.2 |
40% |-----|-----|-----|-----|-----|-----|
: | ... | ... | ... | ... | ... | ... |
*/
func (c *Corrector) AdjustFromObjectPosition(currentSteering float64, objects []*events.Object) float64 {
// TODO, group rectangle
@ -59,47 +67,31 @@ func (c *Corrector) AdjustFromObjectPosition(currentSteering float64, objects []
if currentSteering > -1*deltaMiddle && currentSteering < deltaMiddle {
// Straight
return currentSteering + c.computeDeviation(currentSteering, nearest)
}
if currentSteering < -1*deltaMiddle {
// Turn to left, so search to avoid collision with objects on the left
} else {
// Turn to right or left, 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))
factor, err := c.objectMoveFactors.ValueOf(float64(nearest.Right), 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),
Left: nearest.Left + float32(currentSteering*factor),
Top: nearest.Top,
Right: nearest.Right * float32(factor),
Right: nearest.Right + float32(currentSteering*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
result := currentSteering + c.computeDeviation(currentSteering, &objMoved)
if result < -1. {
result = -1.
}
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,
if result > 1. {
result = 1.
}
return currentSteering + c.computeDeviation(currentSteering, &objMoved)
return result
}
return currentSteering
}
func (c *Corrector) computeDeviation(currentSteering float64, nearest *events.Object) float64 {

9
pkg/steering/corrector_test.go
View File

@ -75,9 +75,9 @@ var (
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},
{0., 0., 0., 0., 0., 0.},
{0., 0.25, 0, 0, -0.25, 0.},
{0.5, 0.25, 0, 0, -0.5, -0.25},
},
}
)
@ -185,7 +185,8 @@ func TestCorrector_AdjustFromObjectPosition(t *testing.T) {
for _, tt := range tests {
t.Run(tt.name, func(t *testing.T) {
c := &Corrector{
gridMap: &defaultGridMap,
gridMap: &defaultGridMap,
objectMoveFactors: &defaultObjectFactors,
}
if got := c.AdjustFromObjectPosition(tt.args.currentSteering, tt.args.objects); got != tt.want {
t.Errorf("AdjustFromObjectPosition() = %v, want %v", got, tt.want)