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 13:24:31 +02:00
parent 7d27ea866f
commit 86f91d9f88
3 changed files with 213 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
pkg/steering/corrector.go
View File

@@ -9,11 +9,11 @@ import (
)
type Corrector struct {
fixValues FixesTable
gridMap *GridMap
}
/*
FixFromObjectPosition modify steering value according object positions
AdjustFromObjectPosition modify steering value according object positions
1. To compute steering correction, split in image in zones and define correction value for each zone
@@ -40,7 +40,7 @@ FixFromObjectPosition 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.
*/
func (c *Corrector) FixFromObjectPosition(currentSteering float64, objects []*events.Object) float64 {
func (c *Corrector) AdjustFromObjectPosition(currentSteering float64, objects []*events.Object) float64 {
// TODO, group rectangle
if len(objects) == 0 {
@@ -60,13 +60,21 @@ func (c *Corrector) FixFromObjectPosition(currentSteering float64, objects []*ev
return currentSteering
}
var delta float64
if nearest.Left < 0 && nearest.Right < 0 {
return currentSteering + c.fixValues.ValueOf(currentSteering, float64(nearest.Right))
delta, err = c.gridMap.ValueOf(currentSteering, float64(nearest.Right))
}
if nearest.Left > 0 && nearest.Right > 0 {
return currentSteering + c.fixValues.ValueOf(currentSteering, float64(nearest.Left))
delta, err = c.gridMap.ValueOf(currentSteering, float64(nearest.Left))
} else {
delta, err = c.gridMap.ValueOf(currentSteering, float64(nearest.Left)+(float64(nearest.Right)-float64(nearest.Left))/2.)
}
return currentSteering + c.fixValues.ValueOf(currentSteering, float64(nearest.Left)+(float64(nearest.Right)-float64(nearest.Left))/2.)
if err != nil {
zap.S().Warnf("unable to compute delta to apply to steering, skip correction: %v", err)
delta = 0
}
return currentSteering + delta
}
// Search if current steering is near of Right or Left
@@ -92,12 +100,12 @@ func (c *Corrector) nearObject(objects []*events.Object) (*events.Object, error)
return result, nil
}
func NewFixesTableFromJson(fileName string) (*FixesTable, error) {
func NewGridMapFromJson(fileName string) (*GridMap, error) {
content, err := os.ReadFile(fileName)
if err != nil {
return nil, fmt.Errorf("unable to read content from %s file: %w", fileName, err)
}
var ft FixesTable
var ft GridMap
err = json.Unmarshal(content, &ft)
if err != nil {
return nil, fmt.Errorf("unable to unmarshal json content from %s file: %w", fileName, err)
@@ -106,13 +114,19 @@ func NewFixesTableFromJson(fileName string) (*FixesTable, error) {
return &ft, nil
}
type FixesTable struct {
type GridMap struct {
DistanceSteps []float64 `json:"distance_steps"`
SteeringSteps []float64 `json:"steering_steps"`
Data [][]float64 `json:"data"`
}
func (f *FixesTable) ValueOf(steering float64, distance float64) float64 {
func (f *GridMap) ValueOf(steering float64, distance float64) (float64, error) {
if steering < f.SteeringSteps[0] || steering > f.SteeringSteps[len(f.SteeringSteps)-1] {
return 0., fmt.Errorf("invalid steering value: %v, must be between %v and %v", steering, f.SteeringSteps[0], f.SteeringSteps[len(f.SteeringSteps)-1])
}
if distance < f.DistanceSteps[0] || distance > f.DistanceSteps[len(f.DistanceSteps)-1] {
return 0., fmt.Errorf("invalid distance value: %v, must be between %v and %v", steering, f.DistanceSteps[0], f.DistanceSteps[len(f.DistanceSteps)-1])
}
// search column index
var idxCol int
// Start loop at 1 because first column should be skipped
@@ -132,5 +146,5 @@ func (f *FixesTable) ValueOf(steering float64, distance float64) float64 {
}
}
return f.Data[idxRow][idxCol]
return f.Data[idxRow][idxCol], nil
}

197
pkg/steering/corrector_test.go
View File

@@ -41,6 +41,20 @@ var (
}
)
var (
defaultGridMap = 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_FixFromObjectPosition(t *testing.T) {
type args struct {
currentSteering float64
@@ -130,8 +144,8 @@ func TestCorrector_FixFromObjectPosition(t *testing.T) {
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)
if got := c.AdjustFromObjectPosition(tt.args.currentSteering, tt.args.objects); got != tt.want {
t.Errorf("AdjustFromObjectPosition() = %v, want %v", got, tt.want)
}
})
}
@@ -187,14 +201,14 @@ func TestCorrector_nearObject(t *testing.T) {
}
}
func TestNewFixesTableFromJson(t *testing.T) {
func TestNewGridMapFromJson(t *testing.T) {
type args struct {
fileName string
}
tests := []struct {
name string
args args
want *FixesTable
want *GridMap
wantErr bool
}{
{
@@ -202,18 +216,183 @@ func TestNewFixesTableFromJson(t *testing.T) {
args: args{
fileName: "test_data/config.json",
},
want: &defaultGridMap,
},
// TODO: Add test cases.
}
for _, tt := range tests {
t.Run(tt.name, func(t *testing.T) {
got, err := NewFixesTableFromJson(tt.args.fileName)
got, err := NewGridMapFromJson(tt.args.fileName)
if (err != nil) != tt.wantErr {
t.Errorf("NewFixesTableFromJson() error = %v, wantErr %v", err, tt.wantErr)
t.Errorf("NewGridMapFromJson() error = %v, wantErr %v", err, tt.wantErr)
return
}
if !reflect.DeepEqual(got, tt.want) {
t.Errorf("NewFixesTableFromJson() got = %v, want %v", got, tt.want)
if !reflect.DeepEqual(*got, *tt.want) {
t.Errorf("NewGridMapFromJson() got = %v, want %v", got, tt.want)
}
if !reflect.DeepEqual(got.SteeringSteps, tt.want.SteeringSteps) {
t.Errorf("NewGridMapFromJson(), bad steering limits: got = %v, want %v", got.SteeringSteps, tt.want.SteeringSteps)
}
if !reflect.DeepEqual(got.DistanceSteps, tt.want.DistanceSteps) {
t.Errorf("NewGridMapFromJson(), bad distance limits: got = %v, want %v", got.DistanceSteps, tt.want.DistanceSteps)
}
})
}
}
func TestGridMap_ValueOf(t *testing.T) {
type fields struct {
DistanceSteps []float64
SteeringSteps []float64
Data [][]float64
}
type args struct {
steering float64
distance float64
}
tests := []struct {
name string
fields fields
args args
want float64
wantErr bool
}{
{
name: "nominal",
fields: fields{
DistanceSteps: defaultGridMap.DistanceSteps,
SteeringSteps: defaultGridMap.SteeringSteps,
Data: defaultGridMap.Data,
},
args: args{
steering: 0.,
distance: 0.,
},
want: 0,
wantErr: false,
},
{
name: "limit distance <",
fields: fields{
DistanceSteps: defaultGridMap.DistanceSteps,
SteeringSteps: defaultGridMap.SteeringSteps,
Data: defaultGridMap.Data,
},
args: args{
steering: 0,
distance: 0.39999,
},
want: 0,
wantErr: false,
},
{
name: "limit distance >",
fields: fields{
DistanceSteps: defaultGridMap.DistanceSteps,
SteeringSteps: defaultGridMap.SteeringSteps,
Data: defaultGridMap.Data,
},
args: args{
steering: 0,
distance: 0.400001,
},
want: -0.25,
wantErr: false,
},
{
name: "limit steering <",
fields: fields{
DistanceSteps: defaultGridMap.DistanceSteps,
SteeringSteps: defaultGridMap.SteeringSteps,
Data: defaultGridMap.Data,
},
args: args{
steering: -0.660001,
distance: 0.85,
},
want: 0.25,
wantErr: false,
},
{
name: "limit steering >",
fields: fields{
DistanceSteps: defaultGridMap.DistanceSteps,
SteeringSteps: defaultGridMap.SteeringSteps,
Data: defaultGridMap.Data,
},
args: args{
steering: -0.66,
distance: 0.85,
},
want: 0.5,
wantErr: false,
},
{
name: "steering < min value",
fields: fields{
DistanceSteps: defaultGridMap.DistanceSteps,
SteeringSteps: defaultGridMap.SteeringSteps,
Data: defaultGridMap.Data,
},
args: args{
steering: defaultGridMap.SteeringSteps[0] - 0.1,
distance: 0.85,
},
wantErr: true,
},
{
name: "steering > max value",
fields: fields{
DistanceSteps: defaultGridMap.DistanceSteps,
SteeringSteps: defaultGridMap.SteeringSteps,
Data: defaultGridMap.Data,
},
args: args{
steering: defaultGridMap.SteeringSteps[len(defaultGridMap.SteeringSteps)-1] + 0.1,
distance: 0.85,
},
wantErr: true,
},
{
name: "distance < min value",
fields: fields{
DistanceSteps: defaultGridMap.DistanceSteps,
SteeringSteps: defaultGridMap.SteeringSteps,
Data: defaultGridMap.Data,
},
args: args{
steering: -0.65,
distance: defaultGridMap.DistanceSteps[0] - 0.1,
},
wantErr: true,
},
{
name: "distance > max value",
fields: fields{
DistanceSteps: defaultGridMap.DistanceSteps,
SteeringSteps: defaultGridMap.SteeringSteps,
Data: defaultGridMap.Data,
},
args: args{
steering: -0.65,
distance: defaultGridMap.DistanceSteps[len(defaultGridMap.DistanceSteps)-1] + 0.1,
},
wantErr: true,
},
}
for _, tt := range tests {
t.Run(tt.name, func(t *testing.T) {
f := &GridMap{
DistanceSteps: tt.fields.DistanceSteps,
SteeringSteps: tt.fields.SteeringSteps,
Data: tt.fields.Data,
}
got, err := f.ValueOf(tt.args.steering, tt.args.distance)
if (err != nil) != tt.wantErr {
t.Errorf("ValueOf() error = %v, wantErr %v", err, tt.wantErr)
return
}
if !reflect.DeepEqual(got, tt.want) {
t.Errorf("ValueOf() = %v, want %v", got, tt.want)
}
})
}

1
pkg/steering/test_data/config.json
View File

@@ -2,7 +2,6 @@
"steering_steps":[-1, -0.66, -0.33, 0, 0.33, 0.66, 1],
"distance_steps": [0, 0.2, 0.4, 0.6, 0.8, 1],
"data": [
[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],