Compare commits

..

No commits in common. "838c9b4ef34ff0e8aadae7b0c3d173aee532f24c" and "34c4433836ff89ef717331ae994682d77c9a7e1c" have entirely different histories.

11 changed files with 48 additions and 479 deletions

View File

@ -3,9 +3,7 @@ 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"
@ -18,11 +16,9 @@ const (
func main() { func main() {
var mqttBroker, username, password, clientId string var mqttBroker, username, password, clientId string
var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic 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 {
@ -42,14 +38,11 @@ func main() {
flag.StringVar(&driveModeTopic, "mqtt-topic-drive-mode", os.Getenv("MQTT_TOPIC_DRIVE_MODE"), "Mqtt topic that contains DriveMode value, use MQTT_TOPIC_DRIVE_MODE if args not set") flag.StringVar(&driveModeTopic, "mqtt-topic-drive-mode", os.Getenv("MQTT_TOPIC_DRIVE_MODE"), "Mqtt topic that contains DriveMode value, use MQTT_TOPIC_DRIVE_MODE if args not set")
flag.StringVar(&rcThrottleTopic, "mqtt-topic-rc-throttle", os.Getenv("MQTT_TOPIC_RC_THROTTLE"), "Mqtt topic that contains RC Throttle value, use MQTT_TOPIC_RC_THROTTLE if args not set") flag.StringVar(&rcThrottleTopic, "mqtt-topic-rc-throttle", os.Getenv("MQTT_TOPIC_RC_THROTTLE"), "Mqtt topic that contains RC Throttle value, use MQTT_TOPIC_RC_THROTTLE if args not set")
flag.StringVar(&steeringTopic, "mqtt-topic-steering", os.Getenv("MQTT_TOPIC_STEERING"), "Mqtt topic that contains steering value, use MQTT_TOPIC_STEERING if args not set") flag.StringVar(&steeringTopic, "mqtt-topic-steering", os.Getenv("MQTT_TOPIC_STEERING"), "Mqtt topic that contains steering value, use MQTT_TOPIC_STEERING if args not set")
flag.StringVar(&throttleFeedbackTopic, "mqtt-topic-throttle-feedback", os.Getenv("MQTT_TOPIC_THROTTLE_FEEDBACK"), "Mqtt topic where to publish throttle feedback, use MQTT_TOPIC_THROTTLE_FEEDBACK if args not set")
flag.Float64Var(&minThrottle, "throttle-min", minThrottle, "Minimum throttle value, use THROTTLE_MIN if args not set") flag.Float64Var(&minThrottle, "throttle-min", minThrottle, "Minimum throttle value, use THROTTLE_MIN if args not set")
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()
@ -77,14 +70,7 @@ func main() {
} }
defer client.Disconnect(50) defer client.Disconnect(50)
var brakeCtrl brake.Controller p := throttle.New(client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, float32(minThrottle), float32(maxThrottle), 2)
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)

View File

@ -1,54 +0,0 @@
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]
}

View File

@ -1,132 +0,0 @@
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)
}
})
}
}

View File

@ -1,55 +0,0 @@
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

@ -1,92 +0,0 @@
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

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

View File

@ -3,8 +3,6 @@ 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"
@ -12,39 +10,25 @@ import (
"time" "time"
) )
func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string, func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic 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,
rcThrottleTopic: rcThrottleTopic, rcThrottleTopic: rcThrottleTopic,
steeringTopic: steeringTopic, steeringTopic: steeringTopic,
throttleFeedbackTopic: throttleFeedbackTopic,
maxThrottle: maxValue, maxThrottle: maxValue,
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 types.Throttle maxThrottle float32
steeringProcessor *SteeringProcessor steeringProcessor *SteeringProcessor
muDriveMode sync.RWMutex muDriveMode sync.RWMutex
@ -53,11 +37,9 @@ type Controller struct {
muSteering sync.RWMutex muSteering sync.RWMutex
steering float32 steering float32
brakeCtrl brake.Controller
cancel chan interface{} cancel chan interface{}
publishPilotFrequency int publishPilotFrequency int
driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string driveModeTopic, rcThrottleTopic, steeringTopic string
} }
func (c *Controller) Start() error { func (c *Controller) Start() error {
@ -86,10 +68,8 @@ func (c *Controller) onPublishPilotValue() {
return return
} }
throttleFromSteering := c.steeringProcessor.Process(c.readSteering())
throttleMsg := events.ThrottleMessage{ throttleMsg := events.ThrottleMessage{
Throttle: float32(c.brakeCtrl.AdjustThrottle(throttleFromSteering)), Throttle: c.steeringProcessor.Process(c.readSteering()),
Confidence: 1.0, Confidence: 1.0,
} }
payload, err := proto.Marshal(&throttleMsg) payload, err := proto.Marshal(&throttleMsg)
@ -110,24 +90,14 @@ func (c *Controller) readSteering() float32 {
func (c *Controller) Stop() { func (c *Controller) Stop() {
close(c.cancel) close(c.cancel)
service.StopService("throttle", c.client, c.driveModeTopic, c.rcThrottleTopic, c.steeringTopic, c.throttleFeedbackTopic) service.StopService("throttle", c.client, c.driveModeTopic, c.rcThrottleTopic, c.steeringTopic)
}
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)
return
}
c.brakeCtrl.SetRealThrottle(types.Throttle(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
} }
@ -149,9 +119,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 types.Throttle(throttleMsg.GetThrottle()) > c.maxThrottle { if 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 = float32(c.maxThrottle) throttleMsg.Throttle = 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)
@ -192,10 +162,6 @@ var registerCallbacks = func(p *Controller) error {
if err != nil { if err != nil {
return err return err
} }
err = service.RegisterCallback(p.client, p.throttleFeedbackTopic, p.onThrottleFeedback)
if err != nil {
return err
}
return nil return nil
} }

View File

@ -3,8 +3,6 @@ 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,15 +33,14 @@ func TestDefaultThrottle(t *testing.T) {
driveModeTopic := "topic/driveMode" driveModeTopic := "topic/driveMode"
rcThrottleTopic := "topic/rcThrottle" rcThrottleTopic := "topic/rcThrottle"
steeringTopic := "topic/rcThrottle" steeringTopic := "topic/rcThrottle"
throttleFeedbackTopic := "topic/feedback/throttle"
minValue := types.Throttle(0.56) minValue := float32(0.56)
p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, minValue, 1., 200) p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, minValue, 1., 200)
cases := []*struct { cases := []struct {
name string name string
maxThrottle types.Throttle maxThrottle float32
driveMode events.DriveModeMessage driveMode events.DriveModeMessage
rcThrottle events.ThrottleMessage rcThrottle events.ThrottleMessage
expectedThrottle events.ThrottleMessage expectedThrottle events.ThrottleMessage
@ -116,19 +113,16 @@ func TestController_Start(t *testing.T) {
steeringTopic := "topic/steering" steeringTopic := "topic/steering"
driveModeTopic := "topic/driveMode" driveModeTopic := "topic/driveMode"
rcThrottleTopic := "topic/rcThrottle" rcThrottleTopic := "topic/rcThrottle"
throttleFeedbackTopic := "topic/feedback/throttle"
type fields struct { type fields struct {
driveMode events.DriveMode driveMode events.DriveMode
min, max types.Throttle min, max float32
publishPilotFrequency int publishPilotFrequency int
brakeCtl brake.Controller
} }
type msgEvents struct { type msgEvents struct {
driveMode *events.DriveModeMessage driveMode *events.DriveModeMessage
steering *events.SteeringMessage steering *events.SteeringMessage
rcThrottle *events.ThrottleMessage rcThrottle *events.ThrottleMessage
throttleFeedback *events.ThrottleMessage
} }
tests := []struct { tests := []struct {
@ -145,13 +139,11 @@ 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},
steering: &events.SteeringMessage{Steering: 0.0, Confidence: 1.0}, steering: &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
rcThrottle: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}, rcThrottle: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
}, },
want: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}, want: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
}, },
@ -162,13 +154,11 @@ 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},
steering: &events.SteeringMessage{Steering: 0.0, Confidence: 1.0}, steering: &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
rcThrottle: &events.ThrottleMessage{Throttle: 0.9, Confidence: 1.0}, rcThrottle: &events.ThrottleMessage{Throttle: 0.9, Confidence: 1.0},
throttleFeedback: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
}, },
want: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0}, want: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
}, },
@ -179,13 +169,11 @@ 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},
steering: &events.SteeringMessage{Steering: 0.0, Confidence: 1.0}, steering: &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
rcThrottle: &events.ThrottleMessage{Throttle: 0.1, Confidence: 1.0}, rcThrottle: &events.ThrottleMessage{Throttle: 0.1, Confidence: 1.0},
throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
}, },
want: &events.ThrottleMessage{Throttle: 0.1, Confidence: 1.0}, want: &events.ThrottleMessage{Throttle: 0.1, Confidence: 1.0},
}, },
@ -196,13 +184,11 @@ 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},
steering: &events.SteeringMessage{Steering: 0.0, Confidence: 1.0}, steering: &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
rcThrottle: &events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0}, rcThrottle: &events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0},
throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
}, },
want: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0}, want: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
}, },
@ -213,13 +199,11 @@ 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},
steering: &events.SteeringMessage{Steering: -1.0, Confidence: 1.0}, steering: &events.SteeringMessage{Steering: -1.0, Confidence: 1.0},
rcThrottle: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0}, rcThrottle: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
}, },
want: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0}, want: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
}, },
@ -230,42 +214,22 @@ 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},
steering: &events.SteeringMessage{Steering: 1.0, Confidence: 1.0}, steering: &events.SteeringMessage{Steering: 1.0, Confidence: 1.0},
rcThrottle: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0}, rcThrottle: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
}, },
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 {
t.Run(tt.name, func(t *testing.T) { t.Run(tt.name, func(t *testing.T) {
c := New(nil, c := New(nil,
throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic,
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()
@ -277,7 +241,6 @@ func TestController_Start(t *testing.T) {
c.onDriveMode(nil, testtools.NewFakeMessageFromProtobuf(driveModeTopic, tt.msgEvents.driveMode)) c.onDriveMode(nil, testtools.NewFakeMessageFromProtobuf(driveModeTopic, tt.msgEvents.driveMode))
c.onRCThrottle(nil, testtools.NewFakeMessageFromProtobuf(rcThrottleTopic, tt.msgEvents.rcThrottle)) c.onRCThrottle(nil, testtools.NewFakeMessageFromProtobuf(rcThrottleTopic, tt.msgEvents.rcThrottle))
c.onSteering(nil, testtools.NewFakeMessageFromProtobuf(steeringTopic, tt.msgEvents.steering)) c.onSteering(nil, testtools.NewFakeMessageFromProtobuf(steeringTopic, tt.msgEvents.steering))
c.onThrottleFeedback(nil, testtools.NewFakeMessageFromProtobuf(throttleFeedbackTopic, tt.msgEvents.throttleFeedback))
waitPublish.Wait() waitPublish.Wait()
var msg events.ThrottleMessage var msg events.ThrottleMessage

View File

@ -1,16 +1,13 @@
package throttle package throttle
import ( import "math"
"github.com/cyrilix/robocar-throttle/pkg/types"
"math"
)
type SteeringProcessor struct { type SteeringProcessor struct {
minThrottle, maxThrottle types.Throttle minThrottle, maxThrottle float32
} }
// Process compute throttle from steering value // Process compute throttle from steering value
func (sp *SteeringProcessor) Process(steering float32) types.Throttle { func (sp *SteeringProcessor) Process(steering float32) float32 {
absSteering := math.Abs(float64(steering)) absSteering := math.Abs(float64(steering))
return sp.minThrottle + types.Throttle(float64(sp.maxThrottle-sp.minThrottle)*(1-absSteering)) return sp.minThrottle + float32(float64(sp.maxThrottle-sp.minThrottle)*(1-absSteering))
} }

View File

@ -1,14 +1,11 @@
package throttle package throttle
import ( import "testing"
"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 types.Throttle minThrottle float32
maxThrottle types.Throttle maxThrottle float32
} }
type args struct { type args struct {
steering float32 steering float32
@ -17,7 +14,7 @@ func TestSteeringProcessor_Process(t *testing.T) {
name string name string
fields fields fields fields
args args args args
want types.Throttle want float32
}{ }{
{ {
name: "steering straight", name: "steering straight",

View File

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