feat(brake): implement brake feature

This commit is contained in:
Cyrille Nofficial 2022-09-05 15:30:26 +02:00
parent dd0102ff44
commit 838c9b4ef3
11 changed files with 425 additions and 34 deletions

View File

@ -3,7 +3,9 @@ package main
import (
"flag"
"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/types"
"go.uber.org/zap"
"log"
"os"
@ -19,6 +21,8 @@ func main() {
var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string
var minThrottle, maxThrottle float64
var publishPilotFrequency int
var brakeConfig string
var enableBrake bool
err := cli.SetFloat64DefaultValueFromEnv(&minThrottle, "THROTTLE_MIN", DefaultThrottleMin)
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.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")
flag.Parse()
@ -71,7 +77,14 @@ func main() {
}
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()
cli.HandleExit(p)

54
pkg/brake/config.go Normal file
View 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
View 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
View 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
}

View 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)
}
})
}
}

View File

@ -0,0 +1,4 @@
{
"delta_steps": [ 0.05, 0.3, 0.5 ],
"data": [ -0.1, -0.5, -1.0 ]
}

View File

@ -3,6 +3,8 @@ package throttle
import (
"github.com/cyrilix/robocar-base/service"
"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"
"go.uber.org/zap"
"google.golang.org/protobuf/proto"
@ -11,8 +13,8 @@ import (
)
func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string,
minValue, maxValue float32, publishPilotFrequency int) *Controller {
return &Controller{
minValue, maxValue types.Throttle, publishPilotFrequency int, opts ...Option) *Controller {
c := &Controller{
client: client,
throttleTopic: throttleTopic,
driveModeTopic: driveModeTopic,
@ -23,14 +25,26 @@ func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, ste
driveMode: events.DriveMode_USER,
publishPilotFrequency: publishPilotFrequency,
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 {
client mqtt.Client
throttleTopic string
maxThrottle float32
maxThrottle types.Throttle
steeringProcessor *SteeringProcessor
muDriveMode sync.RWMutex
@ -39,8 +53,7 @@ type Controller struct {
muSteering sync.RWMutex
steering float32
muThrottleFeedback sync.RWMutex
throttleFeedback float32
brakeCtrl brake.Controller
cancel chan interface{}
publishPilotFrequency int
@ -73,8 +86,10 @@ func (c *Controller) onPublishPilotValue() {
return
}
throttleFromSteering := c.steeringProcessor.Process(c.readSteering())
throttleMsg := events.ThrottleMessage{
Throttle: c.steeringProcessor.Process(c.readSteering()),
Throttle: float32(c.brakeCtrl.AdjustThrottle(throttleFromSteering)),
Confidence: 1.0,
}
payload, err := proto.Marshal(&throttleMsg)
@ -102,20 +117,17 @@ func (c *Controller) onThrottleFeedback(_ mqtt.Client, message mqtt.Message) {
var msg events.ThrottleMessage
err := proto.Unmarshal(message.Payload(), &msg)
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
}
c.muThrottleFeedback.Lock()
defer c.muThrottleFeedback.Unlock()
c.throttleFeedback = msg.GetThrottle()
c.brakeCtrl.SetRealThrottle(types.Throttle(msg.GetThrottle()))
}
func (c *Controller) onDriveMode(_ mqtt.Client, message mqtt.Message) {
var msg events.DriveModeMessage
err := proto.Unmarshal(message.Payload(), &msg)
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
}
@ -137,9 +149,9 @@ func (c *Controller) onRCThrottle(_ mqtt.Client, message mqtt.Message) {
return
}
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)
throttleMsg.Throttle = c.maxThrottle
throttleMsg.Throttle = float32(c.maxThrottle)
payloadPatched, err := proto.Marshal(&throttleMsg)
if err != nil {
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()
}
func (c *Controller) ThrottleFeedback() float32 {
c.muThrottleFeedback.RLock()
defer c.muThrottleFeedback.RUnlock()
tf := c.throttleFeedback
return tf
}
var registerCallbacks = func(p *Controller) error {
err := service.RegisterCallback(p.client, p.driveModeTopic, p.onDriveMode)
if err != nil {

View File

@ -3,6 +3,8 @@ package throttle
import (
"github.com/cyrilix/robocar-base/testtools"
"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"
"google.golang.org/protobuf/proto"
"sync"
@ -35,13 +37,13 @@ func TestDefaultThrottle(t *testing.T) {
steeringTopic := "topic/rcThrottle"
throttleFeedbackTopic := "topic/feedback/throttle"
minValue := float32(0.56)
minValue := types.Throttle(0.56)
p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, minValue, 1., 200)
cases := []struct {
cases := []*struct {
name string
maxThrottle float32
maxThrottle types.Throttle
driveMode events.DriveModeMessage
rcThrottle events.ThrottleMessage
expectedThrottle events.ThrottleMessage
@ -118,8 +120,9 @@ func TestController_Start(t *testing.T) {
type fields struct {
driveMode events.DriveMode
min, max float32
min, max types.Throttle
publishPilotFrequency int
brakeCtl brake.Controller
}
type msgEvents struct {
driveMode *events.DriveModeMessage
@ -142,6 +145,7 @@ func TestController_Start(t *testing.T) {
max: 0.8,
min: 0.3,
publishPilotFrequency: publishPilotFrequency,
brakeCtl: &brake.DisabledController{},
},
msgEvents: msgEvents{
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_USER},
@ -158,6 +162,7 @@ func TestController_Start(t *testing.T) {
max: 0.8,
min: 0.3,
publishPilotFrequency: publishPilotFrequency,
brakeCtl: &brake.DisabledController{},
},
msgEvents: msgEvents{
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_USER},
@ -174,6 +179,7 @@ func TestController_Start(t *testing.T) {
max: 0.8,
min: 0.3,
publishPilotFrequency: publishPilotFrequency,
brakeCtl: &brake.DisabledController{},
},
msgEvents: msgEvents{
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_USER},
@ -190,6 +196,7 @@ func TestController_Start(t *testing.T) {
max: 0.8,
min: 0.3,
publishPilotFrequency: publishPilotFrequency,
brakeCtl: &brake.DisabledController{},
},
msgEvents: msgEvents{
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
@ -206,6 +213,7 @@ func TestController_Start(t *testing.T) {
max: 0.8,
min: 0.3,
publishPilotFrequency: publishPilotFrequency,
brakeCtl: &brake.DisabledController{},
},
msgEvents: msgEvents{
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
@ -222,6 +230,7 @@ func TestController_Start(t *testing.T) {
max: 0.8,
min: 0.3,
publishPilotFrequency: publishPilotFrequency,
brakeCtl: &brake.DisabledController{},
},
msgEvents: msgEvents{
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},
},
{
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 {
@ -239,6 +265,7 @@ func TestController_Start(t *testing.T) {
throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
tt.fields.min, tt.fields.max,
tt.fields.publishPilotFrequency,
WithBrakeController(tt.fields.brakeCtl),
)
go c.Start()

View File

@ -1,13 +1,16 @@
package throttle
import "math"
import (
"github.com/cyrilix/robocar-throttle/pkg/types"
"math"
)
type SteeringProcessor struct {
minThrottle, maxThrottle float32
minThrottle, maxThrottle types.Throttle
}
// 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))
return sp.minThrottle + float32(float64(sp.maxThrottle-sp.minThrottle)*(1-absSteering))
return sp.minThrottle + types.Throttle(float64(sp.maxThrottle-sp.minThrottle)*(1-absSteering))
}

View File

@ -1,11 +1,14 @@
package throttle
import "testing"
import (
"github.com/cyrilix/robocar-throttle/pkg/types"
"testing"
)
func TestSteeringProcessor_Process(t *testing.T) {
type fields struct {
minThrottle float32
maxThrottle float32
minThrottle types.Throttle
maxThrottle types.Throttle
}
type args struct {
steering float32
@ -14,7 +17,7 @@ func TestSteeringProcessor_Process(t *testing.T) {
name string
fields fields
args args
want float32
want types.Throttle
}{
{
name: "steering straight",

3
pkg/types/types.go Normal file
View File

@ -0,0 +1,3 @@
package types
type Throttle float32