feat(brake): implement brake feature
This commit is contained in:
parent
dd0102ff44
commit
838c9b4ef3
@ -3,7 +3,9 @@ package main
|
|||||||
import (
|
import (
|
||||||
"flag"
|
"flag"
|
||||||
"github.com/cyrilix/robocar-base/cli"
|
"github.com/cyrilix/robocar-base/cli"
|
||||||
|
"github.com/cyrilix/robocar-throttle/pkg/brake"
|
||||||
"github.com/cyrilix/robocar-throttle/pkg/throttle"
|
"github.com/cyrilix/robocar-throttle/pkg/throttle"
|
||||||
|
"github.com/cyrilix/robocar-throttle/pkg/types"
|
||||||
"go.uber.org/zap"
|
"go.uber.org/zap"
|
||||||
"log"
|
"log"
|
||||||
"os"
|
"os"
|
||||||
@ -19,6 +21,8 @@ func main() {
|
|||||||
var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string
|
var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string
|
||||||
var minThrottle, maxThrottle float64
|
var minThrottle, maxThrottle float64
|
||||||
var publishPilotFrequency int
|
var publishPilotFrequency int
|
||||||
|
var brakeConfig string
|
||||||
|
var enableBrake bool
|
||||||
|
|
||||||
err := cli.SetFloat64DefaultValueFromEnv(&minThrottle, "THROTTLE_MIN", DefaultThrottleMin)
|
err := cli.SetFloat64DefaultValueFromEnv(&minThrottle, "THROTTLE_MIN", DefaultThrottleMin)
|
||||||
if err != nil {
|
if err != nil {
|
||||||
@ -44,6 +48,8 @@ func main() {
|
|||||||
flag.Float64Var(&maxThrottle, "throttle-max", maxThrottle, "Minimum throttle value, use THROTTLE_MAX if args not set")
|
flag.Float64Var(&maxThrottle, "throttle-max", maxThrottle, "Minimum throttle value, use THROTTLE_MAX if args not set")
|
||||||
flag.IntVar(&publishPilotFrequency, "update-pwm-frequency", 2, "Number of throttle event to publish when pilot mode is enabled")
|
flag.IntVar(&publishPilotFrequency, "update-pwm-frequency", 2, "Number of throttle event to publish when pilot mode is enabled")
|
||||||
|
|
||||||
|
flag.BoolVar(&enableBrake, "enable-brake-feature", false, "Enable brake to slow car on throttle changes")
|
||||||
|
flag.StringVar(&brakeConfig, "brake-configuration", "", "Json file to use to configure brake adaptation when --enable-brake is `true`")
|
||||||
logLevel := zap.LevelFlag("log", zap.InfoLevel, "log level")
|
logLevel := zap.LevelFlag("log", zap.InfoLevel, "log level")
|
||||||
|
|
||||||
flag.Parse()
|
flag.Parse()
|
||||||
@ -71,7 +77,14 @@ func main() {
|
|||||||
}
|
}
|
||||||
defer client.Disconnect(50)
|
defer client.Disconnect(50)
|
||||||
|
|
||||||
p := throttle.New(client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, float32(minThrottle), float32(maxThrottle), 2)
|
var brakeCtrl brake.Controller
|
||||||
|
if enableBrake {
|
||||||
|
brakeCtrl = brake.NewCustomControllerWithJsonConfig(brakeConfig)
|
||||||
|
} else {
|
||||||
|
brakeCtrl = &brake.DisabledController{}
|
||||||
|
}
|
||||||
|
p := throttle.New(client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
|
||||||
|
types.Throttle(minThrottle), types.Throttle(maxThrottle), 2, throttle.WithBrakeController(brakeCtrl))
|
||||||
defer p.Stop()
|
defer p.Stop()
|
||||||
|
|
||||||
cli.HandleExit(p)
|
cli.HandleExit(p)
|
||||||
|
54
pkg/brake/config.go
Normal file
54
pkg/brake/config.go
Normal file
@ -0,0 +1,54 @@
|
|||||||
|
package brake
|
||||||
|
|
||||||
|
import (
|
||||||
|
"encoding/json"
|
||||||
|
"fmt"
|
||||||
|
"github.com/cyrilix/robocar-throttle/pkg/types"
|
||||||
|
"os"
|
||||||
|
)
|
||||||
|
|
||||||
|
var (
|
||||||
|
defaultBrakeConfig = Config{
|
||||||
|
DeltaSteps: []float32{0.05, 0.3, 0.5},
|
||||||
|
Data: []types.Throttle{-0.1, -0.5, -1.},
|
||||||
|
}
|
||||||
|
)
|
||||||
|
|
||||||
|
func NewConfig() *Config {
|
||||||
|
return &defaultBrakeConfig
|
||||||
|
}
|
||||||
|
|
||||||
|
func NewConfigFromJson(fileName string) (*Config, 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 Config
|
||||||
|
err = json.Unmarshal(content, &ft)
|
||||||
|
if err != nil {
|
||||||
|
return nil, fmt.Errorf("unable to unmarshal json content from %s file: %w", fileName, err)
|
||||||
|
}
|
||||||
|
return &ft, nil
|
||||||
|
}
|
||||||
|
|
||||||
|
type Config struct {
|
||||||
|
DeltaSteps []float32 `json:"delta_steps"`
|
||||||
|
Data []types.Throttle `json:"data"`
|
||||||
|
}
|
||||||
|
|
||||||
|
func (tc *Config) ValueOf(currentThrottle, targetThrottle types.Throttle) types.Throttle {
|
||||||
|
delta := float32(currentThrottle - targetThrottle)
|
||||||
|
|
||||||
|
if delta < tc.DeltaSteps[0] {
|
||||||
|
return targetThrottle
|
||||||
|
}
|
||||||
|
if delta >= tc.DeltaSteps[len(tc.DeltaSteps)-1] {
|
||||||
|
return tc.Data[len(tc.Data)-1]
|
||||||
|
}
|
||||||
|
for idx, step := range tc.DeltaSteps {
|
||||||
|
if delta < step {
|
||||||
|
return tc.Data[idx-1]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return tc.Data[len(tc.Data)-1]
|
||||||
|
}
|
132
pkg/brake/config_test.go
Normal file
132
pkg/brake/config_test.go
Normal file
@ -0,0 +1,132 @@
|
|||||||
|
package brake
|
||||||
|
|
||||||
|
import (
|
||||||
|
"github.com/cyrilix/robocar-throttle/pkg/types"
|
||||||
|
"reflect"
|
||||||
|
"testing"
|
||||||
|
)
|
||||||
|
|
||||||
|
func TestNewConfigFromJson(t *testing.T) {
|
||||||
|
type args struct {
|
||||||
|
fileName string
|
||||||
|
}
|
||||||
|
tests := []struct {
|
||||||
|
name string
|
||||||
|
args args
|
||||||
|
want *Config
|
||||||
|
wantErr bool
|
||||||
|
}{
|
||||||
|
{
|
||||||
|
name: "default config",
|
||||||
|
args: args{
|
||||||
|
fileName: "test_data/config.json",
|
||||||
|
},
|
||||||
|
want: &defaultBrakeConfig,
|
||||||
|
},
|
||||||
|
}
|
||||||
|
for _, tt := range tests {
|
||||||
|
t.Run(tt.name, func(t *testing.T) {
|
||||||
|
got, err := NewConfigFromJson(tt.args.fileName)
|
||||||
|
if (err != nil) != tt.wantErr {
|
||||||
|
t.Errorf("NewConfigFromJson() error = %v, wantErr %v", err, tt.wantErr)
|
||||||
|
return
|
||||||
|
}
|
||||||
|
if !reflect.DeepEqual(*got, *tt.want) {
|
||||||
|
t.Errorf("NewConfigFromJson() got = %v, want %v", got, tt.want)
|
||||||
|
}
|
||||||
|
if !reflect.DeepEqual(got.DeltaSteps, tt.want.DeltaSteps) {
|
||||||
|
t.Errorf("NewConfigFromJson(), bad DeltaSteps: got = %v, want %v", got.DeltaSteps, tt.want.DeltaSteps)
|
||||||
|
}
|
||||||
|
})
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
func TestConfig_ValueOf(t *testing.T) {
|
||||||
|
type fields struct {
|
||||||
|
DeltaSteps []float32
|
||||||
|
MinValue int
|
||||||
|
Data []types.Throttle
|
||||||
|
}
|
||||||
|
type args struct {
|
||||||
|
currentThrottle, targetThrottle types.Throttle
|
||||||
|
}
|
||||||
|
tests := []struct {
|
||||||
|
name string
|
||||||
|
fields fields
|
||||||
|
args args
|
||||||
|
want types.Throttle
|
||||||
|
}{
|
||||||
|
{
|
||||||
|
name: "delta > 0",
|
||||||
|
fields: fields{
|
||||||
|
DeltaSteps: defaultBrakeConfig.DeltaSteps,
|
||||||
|
Data: defaultBrakeConfig.Data,
|
||||||
|
},
|
||||||
|
args: args{
|
||||||
|
currentThrottle: 0.5,
|
||||||
|
targetThrottle: 0.8,
|
||||||
|
},
|
||||||
|
want: 0.8,
|
||||||
|
},
|
||||||
|
{
|
||||||
|
name: "no delta",
|
||||||
|
fields: fields{
|
||||||
|
DeltaSteps: defaultBrakeConfig.DeltaSteps,
|
||||||
|
Data: defaultBrakeConfig.Data,
|
||||||
|
},
|
||||||
|
args: args{
|
||||||
|
currentThrottle: 0.5,
|
||||||
|
targetThrottle: 0.5,
|
||||||
|
},
|
||||||
|
want: 0.5,
|
||||||
|
},
|
||||||
|
{
|
||||||
|
name: "delta very low (< 1st step)",
|
||||||
|
fields: fields{
|
||||||
|
DeltaSteps: defaultBrakeConfig.DeltaSteps,
|
||||||
|
Data: defaultBrakeConfig.Data,
|
||||||
|
},
|
||||||
|
args: args{
|
||||||
|
currentThrottle: 0.5,
|
||||||
|
targetThrottle: 0.495,
|
||||||
|
},
|
||||||
|
want: 0.495,
|
||||||
|
},
|
||||||
|
{
|
||||||
|
name: "low delta ( 1st step < delta < 2nd step )",
|
||||||
|
fields: fields{
|
||||||
|
DeltaSteps: defaultBrakeConfig.DeltaSteps,
|
||||||
|
Data: defaultBrakeConfig.Data,
|
||||||
|
},
|
||||||
|
args: args{
|
||||||
|
currentThrottle: 0.5,
|
||||||
|
targetThrottle: 0.38,
|
||||||
|
},
|
||||||
|
want: -0.1,
|
||||||
|
},
|
||||||
|
{
|
||||||
|
name: "high delta",
|
||||||
|
fields: fields{
|
||||||
|
DeltaSteps: defaultBrakeConfig.DeltaSteps,
|
||||||
|
Data: defaultBrakeConfig.Data,
|
||||||
|
},
|
||||||
|
args: args{
|
||||||
|
currentThrottle: 0.8,
|
||||||
|
targetThrottle: 0.3,
|
||||||
|
},
|
||||||
|
want: -1.,
|
||||||
|
},
|
||||||
|
}
|
||||||
|
for _, tt := range tests {
|
||||||
|
t.Run(tt.name, func(t *testing.T) {
|
||||||
|
f := &Config{
|
||||||
|
DeltaSteps: tt.fields.DeltaSteps,
|
||||||
|
Data: tt.fields.Data,
|
||||||
|
}
|
||||||
|
got := f.ValueOf(tt.args.currentThrottle, tt.args.targetThrottle)
|
||||||
|
if !reflect.DeepEqual(got, tt.want) {
|
||||||
|
t.Errorf("ValueOf() = %v, want %v", got, tt.want)
|
||||||
|
}
|
||||||
|
})
|
||||||
|
}
|
||||||
|
}
|
55
pkg/brake/controller.go
Normal file
55
pkg/brake/controller.go
Normal file
@ -0,0 +1,55 @@
|
|||||||
|
package brake
|
||||||
|
|
||||||
|
import (
|
||||||
|
"github.com/cyrilix/robocar-throttle/pkg/types"
|
||||||
|
"go.uber.org/zap"
|
||||||
|
"sync"
|
||||||
|
)
|
||||||
|
|
||||||
|
type Controller interface {
|
||||||
|
SetRealThrottle(t types.Throttle)
|
||||||
|
AdjustThrottle(targetThrottle types.Throttle) types.Throttle
|
||||||
|
}
|
||||||
|
|
||||||
|
func NewCustomController() *CustomController {
|
||||||
|
return &CustomController{cfg: NewConfig()}
|
||||||
|
}
|
||||||
|
|
||||||
|
func NewCustomControllerWithJsonConfig(filename string) *CustomController {
|
||||||
|
config, err := NewConfigFromJson(filename)
|
||||||
|
if err != nil {
|
||||||
|
zap.S().Panicf("unable to init brake controller with json config '%s': %v", filename, err)
|
||||||
|
}
|
||||||
|
return &CustomController{cfg: config}
|
||||||
|
}
|
||||||
|
|
||||||
|
type CustomController struct {
|
||||||
|
muRealThrottle sync.RWMutex
|
||||||
|
realThrottle types.Throttle
|
||||||
|
cfg *Config
|
||||||
|
}
|
||||||
|
|
||||||
|
func (b *CustomController) SetRealThrottle(t types.Throttle) {
|
||||||
|
b.muRealThrottle.Lock()
|
||||||
|
defer b.muRealThrottle.Unlock()
|
||||||
|
b.realThrottle = t
|
||||||
|
}
|
||||||
|
|
||||||
|
func (b *CustomController) GetRealThrottle() types.Throttle {
|
||||||
|
b.muRealThrottle.RLock()
|
||||||
|
defer b.muRealThrottle.RUnlock()
|
||||||
|
res := b.realThrottle
|
||||||
|
return res
|
||||||
|
}
|
||||||
|
|
||||||
|
func (b *CustomController) AdjustThrottle(targetThrottle types.Throttle) types.Throttle {
|
||||||
|
return b.cfg.ValueOf(b.GetRealThrottle(), targetThrottle)
|
||||||
|
}
|
||||||
|
|
||||||
|
type DisabledController struct{}
|
||||||
|
|
||||||
|
func (d *DisabledController) SetRealThrottle(_ types.Throttle) {}
|
||||||
|
|
||||||
|
func (d *DisabledController) AdjustThrottle(targetThrottle types.Throttle) types.Throttle {
|
||||||
|
return targetThrottle
|
||||||
|
}
|
92
pkg/brake/controller_test.go
Normal file
92
pkg/brake/controller_test.go
Normal file
@ -0,0 +1,92 @@
|
|||||||
|
package brake
|
||||||
|
|
||||||
|
import (
|
||||||
|
"github.com/cyrilix/robocar-throttle/pkg/types"
|
||||||
|
"testing"
|
||||||
|
)
|
||||||
|
|
||||||
|
func TestController_AdjustThrottle(t *testing.T) {
|
||||||
|
type fields struct {
|
||||||
|
realThrottle types.Throttle
|
||||||
|
}
|
||||||
|
type args struct {
|
||||||
|
targetThrottle types.Throttle
|
||||||
|
}
|
||||||
|
tests := []struct {
|
||||||
|
name string
|
||||||
|
fields fields
|
||||||
|
args args
|
||||||
|
want types.Throttle
|
||||||
|
}{
|
||||||
|
{
|
||||||
|
name: "target same as current throttle",
|
||||||
|
fields: fields{realThrottle: 0.2},
|
||||||
|
args: args{targetThrottle: 0.2},
|
||||||
|
want: 0.2,
|
||||||
|
},
|
||||||
|
{
|
||||||
|
name: "target > as current throttle",
|
||||||
|
fields: fields{realThrottle: 0.2},
|
||||||
|
args: args{targetThrottle: 0.3},
|
||||||
|
want: 0.3,
|
||||||
|
},
|
||||||
|
{
|
||||||
|
name: "target >> as current throttle",
|
||||||
|
fields: fields{realThrottle: 0.2},
|
||||||
|
args: args{targetThrottle: 0.5},
|
||||||
|
want: 0.5,
|
||||||
|
},
|
||||||
|
{
|
||||||
|
name: "target < as current throttle",
|
||||||
|
fields: fields{realThrottle: 0.8},
|
||||||
|
args: args{targetThrottle: 0.7},
|
||||||
|
want: -0.1,
|
||||||
|
},
|
||||||
|
{
|
||||||
|
name: "target << as current throttle",
|
||||||
|
fields: fields{realThrottle: 0.8},
|
||||||
|
args: args{targetThrottle: 0.5},
|
||||||
|
want: -0.5,
|
||||||
|
},
|
||||||
|
{
|
||||||
|
name: "target <<< as current throttle",
|
||||||
|
fields: fields{realThrottle: 0.8},
|
||||||
|
args: args{targetThrottle: 0.2},
|
||||||
|
want: -1.,
|
||||||
|
},
|
||||||
|
}
|
||||||
|
for _, tt := range tests {
|
||||||
|
t.Run(tt.name, func(t *testing.T) {
|
||||||
|
b := &CustomController{cfg: NewConfig()}
|
||||||
|
b.SetRealThrottle(tt.fields.realThrottle)
|
||||||
|
if got := b.AdjustThrottle(tt.args.targetThrottle); got != tt.want {
|
||||||
|
t.Errorf("AdjustThrottle() = %v, want %v", got, tt.want)
|
||||||
|
}
|
||||||
|
})
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
func TestDisabledController_AdjustThrottle(t *testing.T) {
|
||||||
|
type args struct {
|
||||||
|
targetThrottle types.Throttle
|
||||||
|
}
|
||||||
|
tests := []struct {
|
||||||
|
name string
|
||||||
|
args args
|
||||||
|
want types.Throttle
|
||||||
|
}{
|
||||||
|
{
|
||||||
|
name: "doesn't modify value",
|
||||||
|
args: args{targetThrottle: 0.5},
|
||||||
|
want: 0.5,
|
||||||
|
},
|
||||||
|
}
|
||||||
|
for _, tt := range tests {
|
||||||
|
t.Run(tt.name, func(t *testing.T) {
|
||||||
|
d := &DisabledController{}
|
||||||
|
if got := d.AdjustThrottle(tt.args.targetThrottle); got != tt.want {
|
||||||
|
t.Errorf("AdjustThrottle() = %v, want %v", got, tt.want)
|
||||||
|
}
|
||||||
|
})
|
||||||
|
}
|
||||||
|
}
|
4
pkg/brake/test_data/config.json
Normal file
4
pkg/brake/test_data/config.json
Normal file
@ -0,0 +1,4 @@
|
|||||||
|
{
|
||||||
|
"delta_steps": [ 0.05, 0.3, 0.5 ],
|
||||||
|
"data": [ -0.1, -0.5, -1.0 ]
|
||||||
|
}
|
@ -3,6 +3,8 @@ package throttle
|
|||||||
import (
|
import (
|
||||||
"github.com/cyrilix/robocar-base/service"
|
"github.com/cyrilix/robocar-base/service"
|
||||||
"github.com/cyrilix/robocar-protobuf/go/events"
|
"github.com/cyrilix/robocar-protobuf/go/events"
|
||||||
|
"github.com/cyrilix/robocar-throttle/pkg/brake"
|
||||||
|
"github.com/cyrilix/robocar-throttle/pkg/types"
|
||||||
mqtt "github.com/eclipse/paho.mqtt.golang"
|
mqtt "github.com/eclipse/paho.mqtt.golang"
|
||||||
"go.uber.org/zap"
|
"go.uber.org/zap"
|
||||||
"google.golang.org/protobuf/proto"
|
"google.golang.org/protobuf/proto"
|
||||||
@ -11,8 +13,8 @@ import (
|
|||||||
)
|
)
|
||||||
|
|
||||||
func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string,
|
func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string,
|
||||||
minValue, maxValue float32, publishPilotFrequency int) *Controller {
|
minValue, maxValue types.Throttle, publishPilotFrequency int, opts ...Option) *Controller {
|
||||||
return &Controller{
|
c := &Controller{
|
||||||
client: client,
|
client: client,
|
||||||
throttleTopic: throttleTopic,
|
throttleTopic: throttleTopic,
|
||||||
driveModeTopic: driveModeTopic,
|
driveModeTopic: driveModeTopic,
|
||||||
@ -23,14 +25,26 @@ func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, ste
|
|||||||
driveMode: events.DriveMode_USER,
|
driveMode: events.DriveMode_USER,
|
||||||
publishPilotFrequency: publishPilotFrequency,
|
publishPilotFrequency: publishPilotFrequency,
|
||||||
steeringProcessor: &SteeringProcessor{minThrottle: minValue, maxThrottle: maxValue},
|
steeringProcessor: &SteeringProcessor{minThrottle: minValue, maxThrottle: maxValue},
|
||||||
|
brakeCtrl: &brake.DisabledController{},
|
||||||
}
|
}
|
||||||
|
for _, o := range opts {
|
||||||
|
o(c)
|
||||||
|
}
|
||||||
|
return c
|
||||||
|
}
|
||||||
|
|
||||||
|
type Option func(c *Controller)
|
||||||
|
|
||||||
|
func WithBrakeController(bc brake.Controller) Option {
|
||||||
|
return func(c *Controller) {
|
||||||
|
c.brakeCtrl = bc
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
type Controller struct {
|
type Controller struct {
|
||||||
client mqtt.Client
|
client mqtt.Client
|
||||||
throttleTopic string
|
throttleTopic string
|
||||||
maxThrottle float32
|
maxThrottle types.Throttle
|
||||||
steeringProcessor *SteeringProcessor
|
steeringProcessor *SteeringProcessor
|
||||||
|
|
||||||
muDriveMode sync.RWMutex
|
muDriveMode sync.RWMutex
|
||||||
@ -39,8 +53,7 @@ type Controller struct {
|
|||||||
muSteering sync.RWMutex
|
muSteering sync.RWMutex
|
||||||
steering float32
|
steering float32
|
||||||
|
|
||||||
muThrottleFeedback sync.RWMutex
|
brakeCtrl brake.Controller
|
||||||
throttleFeedback float32
|
|
||||||
|
|
||||||
cancel chan interface{}
|
cancel chan interface{}
|
||||||
publishPilotFrequency int
|
publishPilotFrequency int
|
||||||
@ -73,8 +86,10 @@ func (c *Controller) onPublishPilotValue() {
|
|||||||
return
|
return
|
||||||
}
|
}
|
||||||
|
|
||||||
|
throttleFromSteering := c.steeringProcessor.Process(c.readSteering())
|
||||||
|
|
||||||
throttleMsg := events.ThrottleMessage{
|
throttleMsg := events.ThrottleMessage{
|
||||||
Throttle: c.steeringProcessor.Process(c.readSteering()),
|
Throttle: float32(c.brakeCtrl.AdjustThrottle(throttleFromSteering)),
|
||||||
Confidence: 1.0,
|
Confidence: 1.0,
|
||||||
}
|
}
|
||||||
payload, err := proto.Marshal(&throttleMsg)
|
payload, err := proto.Marshal(&throttleMsg)
|
||||||
@ -102,20 +117,17 @@ func (c *Controller) onThrottleFeedback(_ mqtt.Client, message mqtt.Message) {
|
|||||||
var msg events.ThrottleMessage
|
var msg events.ThrottleMessage
|
||||||
err := proto.Unmarshal(message.Payload(), &msg)
|
err := proto.Unmarshal(message.Payload(), &msg)
|
||||||
if err != nil {
|
if err != nil {
|
||||||
zap.S().Errorf("unable to unmarshal protobuf %T message: %v", msg, err)
|
zap.S().Errorf("unable to unmarshal protobuf %T message: %v", &msg, err)
|
||||||
return
|
return
|
||||||
}
|
}
|
||||||
|
c.brakeCtrl.SetRealThrottle(types.Throttle(msg.GetThrottle()))
|
||||||
c.muThrottleFeedback.Lock()
|
|
||||||
defer c.muThrottleFeedback.Unlock()
|
|
||||||
c.throttleFeedback = msg.GetThrottle()
|
|
||||||
}
|
}
|
||||||
|
|
||||||
func (c *Controller) onDriveMode(_ mqtt.Client, message mqtt.Message) {
|
func (c *Controller) onDriveMode(_ mqtt.Client, message mqtt.Message) {
|
||||||
var msg events.DriveModeMessage
|
var msg events.DriveModeMessage
|
||||||
err := proto.Unmarshal(message.Payload(), &msg)
|
err := proto.Unmarshal(message.Payload(), &msg)
|
||||||
if err != nil {
|
if err != nil {
|
||||||
zap.S().Errorf("unable to unmarshal protobuf %T message: %v", msg, err)
|
zap.S().Errorf("unable to unmarshal protobuf %T message: %v", &msg, err)
|
||||||
return
|
return
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -137,9 +149,9 @@ func (c *Controller) onRCThrottle(_ mqtt.Client, message mqtt.Message) {
|
|||||||
return
|
return
|
||||||
}
|
}
|
||||||
zap.S().Debugf("publish new throttle value from rc: %v", throttleMsg.GetThrottle())
|
zap.S().Debugf("publish new throttle value from rc: %v", throttleMsg.GetThrottle())
|
||||||
if throttleMsg.GetThrottle() > c.maxThrottle {
|
if types.Throttle(throttleMsg.GetThrottle()) > c.maxThrottle {
|
||||||
zap.S().Debugf("throttle upper that max value allowed, patch value from %v to %v", throttleMsg.GetThrottle(), c.maxThrottle)
|
zap.S().Debugf("throttle upper that max value allowed, patch value from %v to %v", throttleMsg.GetThrottle(), c.maxThrottle)
|
||||||
throttleMsg.Throttle = c.maxThrottle
|
throttleMsg.Throttle = float32(c.maxThrottle)
|
||||||
payloadPatched, err := proto.Marshal(&throttleMsg)
|
payloadPatched, err := proto.Marshal(&throttleMsg)
|
||||||
if err != nil {
|
if err != nil {
|
||||||
zap.S().Errorf("unable to marshall throttle msg: %v", err)
|
zap.S().Errorf("unable to marshall throttle msg: %v", err)
|
||||||
@ -165,13 +177,6 @@ func (c *Controller) onSteering(_ mqtt.Client, message mqtt.Message) {
|
|||||||
c.steering = steeringMsg.GetSteering()
|
c.steering = steeringMsg.GetSteering()
|
||||||
}
|
}
|
||||||
|
|
||||||
func (c *Controller) ThrottleFeedback() float32 {
|
|
||||||
c.muThrottleFeedback.RLock()
|
|
||||||
defer c.muThrottleFeedback.RUnlock()
|
|
||||||
tf := c.throttleFeedback
|
|
||||||
return tf
|
|
||||||
}
|
|
||||||
|
|
||||||
var registerCallbacks = func(p *Controller) error {
|
var registerCallbacks = func(p *Controller) error {
|
||||||
err := service.RegisterCallback(p.client, p.driveModeTopic, p.onDriveMode)
|
err := service.RegisterCallback(p.client, p.driveModeTopic, p.onDriveMode)
|
||||||
if err != nil {
|
if err != nil {
|
||||||
|
@ -3,6 +3,8 @@ package throttle
|
|||||||
import (
|
import (
|
||||||
"github.com/cyrilix/robocar-base/testtools"
|
"github.com/cyrilix/robocar-base/testtools"
|
||||||
"github.com/cyrilix/robocar-protobuf/go/events"
|
"github.com/cyrilix/robocar-protobuf/go/events"
|
||||||
|
"github.com/cyrilix/robocar-throttle/pkg/brake"
|
||||||
|
"github.com/cyrilix/robocar-throttle/pkg/types"
|
||||||
mqtt "github.com/eclipse/paho.mqtt.golang"
|
mqtt "github.com/eclipse/paho.mqtt.golang"
|
||||||
"google.golang.org/protobuf/proto"
|
"google.golang.org/protobuf/proto"
|
||||||
"sync"
|
"sync"
|
||||||
@ -35,13 +37,13 @@ func TestDefaultThrottle(t *testing.T) {
|
|||||||
steeringTopic := "topic/rcThrottle"
|
steeringTopic := "topic/rcThrottle"
|
||||||
throttleFeedbackTopic := "topic/feedback/throttle"
|
throttleFeedbackTopic := "topic/feedback/throttle"
|
||||||
|
|
||||||
minValue := float32(0.56)
|
minValue := types.Throttle(0.56)
|
||||||
|
|
||||||
p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, minValue, 1., 200)
|
p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, minValue, 1., 200)
|
||||||
|
|
||||||
cases := []struct {
|
cases := []*struct {
|
||||||
name string
|
name string
|
||||||
maxThrottle float32
|
maxThrottle types.Throttle
|
||||||
driveMode events.DriveModeMessage
|
driveMode events.DriveModeMessage
|
||||||
rcThrottle events.ThrottleMessage
|
rcThrottle events.ThrottleMessage
|
||||||
expectedThrottle events.ThrottleMessage
|
expectedThrottle events.ThrottleMessage
|
||||||
@ -118,8 +120,9 @@ func TestController_Start(t *testing.T) {
|
|||||||
|
|
||||||
type fields struct {
|
type fields struct {
|
||||||
driveMode events.DriveMode
|
driveMode events.DriveMode
|
||||||
min, max float32
|
min, max types.Throttle
|
||||||
publishPilotFrequency int
|
publishPilotFrequency int
|
||||||
|
brakeCtl brake.Controller
|
||||||
}
|
}
|
||||||
type msgEvents struct {
|
type msgEvents struct {
|
||||||
driveMode *events.DriveModeMessage
|
driveMode *events.DriveModeMessage
|
||||||
@ -142,6 +145,7 @@ func TestController_Start(t *testing.T) {
|
|||||||
max: 0.8,
|
max: 0.8,
|
||||||
min: 0.3,
|
min: 0.3,
|
||||||
publishPilotFrequency: publishPilotFrequency,
|
publishPilotFrequency: publishPilotFrequency,
|
||||||
|
brakeCtl: &brake.DisabledController{},
|
||||||
},
|
},
|
||||||
msgEvents: msgEvents{
|
msgEvents: msgEvents{
|
||||||
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_USER},
|
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_USER},
|
||||||
@ -158,6 +162,7 @@ func TestController_Start(t *testing.T) {
|
|||||||
max: 0.8,
|
max: 0.8,
|
||||||
min: 0.3,
|
min: 0.3,
|
||||||
publishPilotFrequency: publishPilotFrequency,
|
publishPilotFrequency: publishPilotFrequency,
|
||||||
|
brakeCtl: &brake.DisabledController{},
|
||||||
},
|
},
|
||||||
msgEvents: msgEvents{
|
msgEvents: msgEvents{
|
||||||
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_USER},
|
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_USER},
|
||||||
@ -174,6 +179,7 @@ func TestController_Start(t *testing.T) {
|
|||||||
max: 0.8,
|
max: 0.8,
|
||||||
min: 0.3,
|
min: 0.3,
|
||||||
publishPilotFrequency: publishPilotFrequency,
|
publishPilotFrequency: publishPilotFrequency,
|
||||||
|
brakeCtl: &brake.DisabledController{},
|
||||||
},
|
},
|
||||||
msgEvents: msgEvents{
|
msgEvents: msgEvents{
|
||||||
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_USER},
|
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_USER},
|
||||||
@ -190,6 +196,7 @@ func TestController_Start(t *testing.T) {
|
|||||||
max: 0.8,
|
max: 0.8,
|
||||||
min: 0.3,
|
min: 0.3,
|
||||||
publishPilotFrequency: publishPilotFrequency,
|
publishPilotFrequency: publishPilotFrequency,
|
||||||
|
brakeCtl: &brake.DisabledController{},
|
||||||
},
|
},
|
||||||
msgEvents: msgEvents{
|
msgEvents: msgEvents{
|
||||||
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
|
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
|
||||||
@ -206,6 +213,7 @@ func TestController_Start(t *testing.T) {
|
|||||||
max: 0.8,
|
max: 0.8,
|
||||||
min: 0.3,
|
min: 0.3,
|
||||||
publishPilotFrequency: publishPilotFrequency,
|
publishPilotFrequency: publishPilotFrequency,
|
||||||
|
brakeCtl: &brake.DisabledController{},
|
||||||
},
|
},
|
||||||
msgEvents: msgEvents{
|
msgEvents: msgEvents{
|
||||||
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
|
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
|
||||||
@ -222,6 +230,7 @@ func TestController_Start(t *testing.T) {
|
|||||||
max: 0.8,
|
max: 0.8,
|
||||||
min: 0.3,
|
min: 0.3,
|
||||||
publishPilotFrequency: publishPilotFrequency,
|
publishPilotFrequency: publishPilotFrequency,
|
||||||
|
brakeCtl: &brake.DisabledController{},
|
||||||
},
|
},
|
||||||
msgEvents: msgEvents{
|
msgEvents: msgEvents{
|
||||||
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
|
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
|
||||||
@ -231,6 +240,23 @@ func TestController_Start(t *testing.T) {
|
|||||||
},
|
},
|
||||||
want: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
|
want: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
|
||||||
},
|
},
|
||||||
|
{
|
||||||
|
name: "On pilot drive mode, should brake on brutal change",
|
||||||
|
fields: fields{
|
||||||
|
driveMode: events.DriveMode_PILOT,
|
||||||
|
max: 1.0,
|
||||||
|
min: 0.3,
|
||||||
|
publishPilotFrequency: publishPilotFrequency,
|
||||||
|
brakeCtl: brake.NewCustomController(),
|
||||||
|
},
|
||||||
|
msgEvents: msgEvents{
|
||||||
|
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
|
||||||
|
steering: &events.SteeringMessage{Steering: -1.0, Confidence: 1.0},
|
||||||
|
rcThrottle: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
|
||||||
|
throttleFeedback: &events.ThrottleMessage{Throttle: 1.0, Confidence: 1.0},
|
||||||
|
},
|
||||||
|
want: &events.ThrottleMessage{Throttle: -1.0, Confidence: 1.0},
|
||||||
|
},
|
||||||
}
|
}
|
||||||
|
|
||||||
for _, tt := range tests {
|
for _, tt := range tests {
|
||||||
@ -239,6 +265,7 @@ func TestController_Start(t *testing.T) {
|
|||||||
throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
|
throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
|
||||||
tt.fields.min, tt.fields.max,
|
tt.fields.min, tt.fields.max,
|
||||||
tt.fields.publishPilotFrequency,
|
tt.fields.publishPilotFrequency,
|
||||||
|
WithBrakeController(tt.fields.brakeCtl),
|
||||||
)
|
)
|
||||||
|
|
||||||
go c.Start()
|
go c.Start()
|
||||||
|
@ -1,13 +1,16 @@
|
|||||||
package throttle
|
package throttle
|
||||||
|
|
||||||
import "math"
|
import (
|
||||||
|
"github.com/cyrilix/robocar-throttle/pkg/types"
|
||||||
|
"math"
|
||||||
|
)
|
||||||
|
|
||||||
type SteeringProcessor struct {
|
type SteeringProcessor struct {
|
||||||
minThrottle, maxThrottle float32
|
minThrottle, maxThrottle types.Throttle
|
||||||
}
|
}
|
||||||
|
|
||||||
// Process compute throttle from steering value
|
// Process compute throttle from steering value
|
||||||
func (sp *SteeringProcessor) Process(steering float32) float32 {
|
func (sp *SteeringProcessor) Process(steering float32) types.Throttle {
|
||||||
absSteering := math.Abs(float64(steering))
|
absSteering := math.Abs(float64(steering))
|
||||||
return sp.minThrottle + float32(float64(sp.maxThrottle-sp.minThrottle)*(1-absSteering))
|
return sp.minThrottle + types.Throttle(float64(sp.maxThrottle-sp.minThrottle)*(1-absSteering))
|
||||||
}
|
}
|
||||||
|
@ -1,11 +1,14 @@
|
|||||||
package throttle
|
package throttle
|
||||||
|
|
||||||
import "testing"
|
import (
|
||||||
|
"github.com/cyrilix/robocar-throttle/pkg/types"
|
||||||
|
"testing"
|
||||||
|
)
|
||||||
|
|
||||||
func TestSteeringProcessor_Process(t *testing.T) {
|
func TestSteeringProcessor_Process(t *testing.T) {
|
||||||
type fields struct {
|
type fields struct {
|
||||||
minThrottle float32
|
minThrottle types.Throttle
|
||||||
maxThrottle float32
|
maxThrottle types.Throttle
|
||||||
}
|
}
|
||||||
type args struct {
|
type args struct {
|
||||||
steering float32
|
steering float32
|
||||||
@ -14,7 +17,7 @@ func TestSteeringProcessor_Process(t *testing.T) {
|
|||||||
name string
|
name string
|
||||||
fields fields
|
fields fields
|
||||||
args args
|
args args
|
||||||
want float32
|
want types.Throttle
|
||||||
}{
|
}{
|
||||||
{
|
{
|
||||||
name: "steering straight",
|
name: "steering straight",
|
||||||
|
3
pkg/types/types.go
Normal file
3
pkg/types/types.go
Normal file
@ -0,0 +1,3 @@
|
|||||||
|
package types
|
||||||
|
|
||||||
|
type Throttle float32
|
Loading…
x
Reference in New Issue
Block a user