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 3. If current steering != 0 (turn on left or right), shift right and left values proportionnaly to current steering and
apply 2. 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 { func (c *Corrector) AdjustFromObjectPosition(currentSteering float64, objects []*events.Object) float64 {
// TODO, group rectangle // TODO, group rectangle
@ -59,47 +67,31 @@ func (c *Corrector) AdjustFromObjectPosition(currentSteering float64, objects []
if currentSteering > -1*deltaMiddle && currentSteering < deltaMiddle { if currentSteering > -1*deltaMiddle && currentSteering < deltaMiddle {
// Straight // Straight
return currentSteering + c.computeDeviation(currentSteering, nearest) return currentSteering + c.computeDeviation(currentSteering, nearest)
} } else {
// Turn to right or left, so search to avoid collision with objects on the right
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 // 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 { if err != nil {
zap.S().Warnf("unable to compute factor to apply to object: %v", err) zap.S().Warnf("unable to compute factor to apply to object: %v", err)
return currentSteering return currentSteering
} }
objMoved := events.Object{ objMoved := events.Object{
Type: nearest.Type, Type: nearest.Type,
Left: nearest.Left * float32(factor), Left: nearest.Left + float32(currentSteering*factor),
Top: nearest.Top, Top: nearest.Top,
Right: nearest.Right * float32(factor), Right: nearest.Right + float32(currentSteering*factor),
Bottom: nearest.Bottom, Bottom: nearest.Bottom,
Confidence: nearest.Confidence, Confidence: nearest.Confidence,
} }
return currentSteering + c.computeDeviation(currentSteering, &objMoved) result := currentSteering + c.computeDeviation(currentSteering, &objMoved)
if result < -1. {
result = -1.
} }
if result > 1. {
if currentSteering > deltaMiddle { result = 1.
// 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{ return result
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 { func (c *Corrector) computeDeviation(currentSteering float64, nearest *events.Object) float64 {

7
pkg/steering/corrector_test.go
View File

@ -75,9 +75,9 @@ var (
Data: [][]float64{ Data: [][]float64{
{0., 0., 0., 0., 0., 0.}, {0., 0., 0., 0., 0., 0.},
{0., 0., 0., 0., 0., 0.}, {0., 0., 0., 0., 0., 0.},
{0., 0., 0.25, -0.25, 0., 0.}, {0., 0., 0., 0., 0., 0.},
{0., 0.25, 0.5, -0.5, -0.25, 0.}, {0., 0.25, 0, 0, -0.25, 0.},
{0.25, 0.5, 1, -1, -0.5, -0.25}, {0.5, 0.25, 0, 0, -0.5, -0.25},
}, },
} }
) )
@ -186,6 +186,7 @@ func TestCorrector_AdjustFromObjectPosition(t *testing.T) {
t.Run(tt.name, func(t *testing.T) { t.Run(tt.name, func(t *testing.T) {
c := &Corrector{ c := &Corrector{
gridMap: &defaultGridMap, gridMap: &defaultGridMap,
objectMoveFactors: &defaultObjectFactors,
} }
if got := c.AdjustFromObjectPosition(tt.args.currentSteering, tt.args.objects); got != tt.want { if got := c.AdjustFromObjectPosition(tt.args.currentSteering, tt.args.objects); got != tt.want {
t.Errorf("AdjustFromObjectPosition() = %v, want %v", got, tt.want) t.Errorf("AdjustFromObjectPosition() = %v, want %v", got, tt.want)