117 lines
3.2 KiB
Go
117 lines
3.2 KiB
Go
package steering
|
|
|
|
import (
|
|
"fmt"
|
|
"github.com/cyrilix/robocar-protobuf/go/events"
|
|
"go.uber.org/zap"
|
|
)
|
|
|
|
type Corrector struct {
|
|
fixValues FixesTable
|
|
}
|
|
|
|
/*
|
|
FixFromObjectPosition modify steering value according object positions
|
|
|
|
1. To compute steering correction, split in image in zones and define correction value for each zone
|
|
|
|
Steering computed
|
|
: -1 -0.66 -0.33 0 0.33 0.66 1
|
|
0% |-----|-----|-----|-----|-----|-----|
|
|
: | 0 | 0 | 0 | 0 | 0 | 0 |
|
|
20% |-----|-----|-----|-----|-----|-----|
|
|
: | 0 | 0 | 0 | 0 | 0 | 0 |
|
|
40% |-----|-----|-----|-----|-----|-----|
|
|
: | 0 | 0 | 0.25|-0.25| 0 | 0 |
|
|
60% |-----|-----|-----|-----|-----|-----|
|
|
: | 0 | 0.25| 0.5 |-0.5 |-0.25| 0 |
|
|
80% |-----|-----|-----|-----|-----|-----|
|
|
: |-0.25|-0.5 | 1 | -1 |-0.5 | 0.25|
|
|
100%|-----|-----|-----|-----|-----|-----|
|
|
|
|
2. For straight (current steering near of 0), search nearest object and if:
|
|
|
|
* left and right values < 0: use correction from right value according image splitting
|
|
* left and right values > 0: use correction from left value according image splitting
|
|
* left < 0 and right values > 0: use (right + (right - left) / 2) value
|
|
|
|
3. If current steering != 0 (turn on left or right), shift right and left values proportionnaly to current steering and
|
|
apply 2.
|
|
*/
|
|
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
|
|
}
|
|
|
|
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 nearest.Left < 0 && nearest.Right < 0 {
|
|
return currentSteering + c.fixValues.ValueOf(currentSteering, nearest.Right)
|
|
}
|
|
if nearest.Left > 0 && nearest.Right > 0 {
|
|
return currentSteering + c.fixValues.ValueOf(currentSteering, nearest.Left)
|
|
}
|
|
return currentSteering + c.fixValues.ValueOf(currentSteering, nearest.Left+(nearest.Right-nearest.Left)/2.)
|
|
}
|
|
|
|
// 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
|
|
}
|
|
|
|
type FixesTable struct {
|
|
values map[string]map[int]float64
|
|
}
|
|
|
|
func (f *FixesTable) ValueOf(steering float64, distance float32) float64 {
|
|
var key string
|
|
if steering < -0.66 {
|
|
key = "<-0.66"
|
|
} else if steering < -0.33 {
|
|
key = "-0.66:-0.33"
|
|
} else if steering < 0 {
|
|
key = "-0.33:0"
|
|
} else if steering < 0.33 {
|
|
key = "0:0.33"
|
|
} else if steering < 0.66 {
|
|
key = "0.33:0.66"
|
|
} else {
|
|
key = ">= 0.66"
|
|
}
|
|
|
|
keyDistance := int(distance / 20)
|
|
return f.values[key][keyDistance]
|
|
|
|
}
|