3 Commits

5 changed files with 188 additions and 33 deletions

View File

@ -18,11 +18,13 @@ const (
func main() { func main() {
var mqttBroker, username, password, clientId string var mqttBroker, username, password, clientId string
var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, speedZoneTopic string var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, maxThrottleCtrlTopic,
speedZoneTopic string
var minThrottle, maxThrottle float64 var minThrottle, maxThrottle float64
var publishPilotFrequency int var publishPilotFrequency int
var brakeConfig string var brakeConfig string
var enableBrake bool var enableBrake bool
var acceleratorFactor float64
var enableSpeedZone bool var enableSpeedZone bool
var enableCustomSteeringProcessor bool var enableCustomSteeringProcessor bool
var configFileSteeringProcessor string var configFileSteeringProcessor string
@ -46,6 +48,7 @@ func main() {
flag.StringVar(&throttleTopic, "mqtt-topic-throttle", os.Getenv("MQTT_TOPIC_THROTTLE"), "Mqtt topic to publish throttle result, use MQTT_TOPIC_THROTTLE if args not set") flag.StringVar(&throttleTopic, "mqtt-topic-throttle", os.Getenv("MQTT_TOPIC_THROTTLE"), "Mqtt topic to publish throttle result, use MQTT_TOPIC_THROTTLE 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(&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(&maxThrottleCtrlTopic, "mqtt-topic-max-throttle-ctrl", os.Getenv("MQTT_TOPIC_MAX_THROTTLE_CTRL"), "Mqtt topic where to publish max throttle value allowed, use MQTT_TOPIC_MAX_THROTTLE_CTRL 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.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.StringVar(&speedZoneTopic, "mqtt-topic-speed-zone", os.Getenv("MQTT_TOPIC_SPEED_ZONE"), "Mqtt topic where to subscribe speed zone events, use MQTT_TOPIC_SPEED_ZONE if args not set") flag.StringVar(&speedZoneTopic, "mqtt-topic-speed-zone", os.Getenv("MQTT_TOPIC_SPEED_ZONE"), "Mqtt topic where to subscribe speed zone events, use MQTT_TOPIC_SPEED_ZONE if args not set")
@ -56,6 +59,7 @@ func main() {
flag.BoolVar(&enableBrake, "enable-brake-feature", false, "Enable brake to slow car on throttle changes") 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`") flag.StringVar(&brakeConfig, "brake-configuration", "", "Json file to use to configure brake adaptation when --enable-brake is `true`")
flag.Float64Var(&acceleratorFactor, "accelerator-factor", 1.0, "Accelerator factor when --enable-bake is 'true'")
flag.BoolVar(&enableCustomSteeringProcessor, "enable-custom-steering-processor", false, "Enable custom steering processor to estimate throttle") flag.BoolVar(&enableCustomSteeringProcessor, "enable-custom-steering-processor", false, "Enable custom steering processor to estimate throttle")
flag.StringVar(&configFileSteeringProcessor, "custom-steering-processor-config", "", "Path to json config to parameter custom steering processor") flag.StringVar(&configFileSteeringProcessor, "custom-steering-processor-config", "", "Path to json config to parameter custom steering processor")
@ -98,6 +102,7 @@ func main() {
zap.S().Infof("Max throttle : %v", maxThrottle) zap.S().Infof("Max throttle : %v", maxThrottle)
zap.S().Infof("Publish frequency : %vHz", publishPilotFrequency) zap.S().Infof("Publish frequency : %vHz", publishPilotFrequency)
zap.S().Infof("Brake enabled : %v", enableBrake) zap.S().Infof("Brake enabled : %v", enableBrake)
zap.S().Infof("Accelerator factor : %v", acceleratorFactor)
zap.S().Infof("CustomSteeringProcessor enabled: %v", enableCustomSteeringProcessor) zap.S().Infof("CustomSteeringProcessor enabled: %v", enableCustomSteeringProcessor)
zap.S().Infof("SpeedZone enabled : %v", enableSpeedZone) zap.S().Infof("SpeedZone enabled : %v", enableSpeedZone)
zap.S().Infof("SpeedZone slow throttle : %v", slowZoneThrottle) zap.S().Infof("SpeedZone slow throttle : %v", slowZoneThrottle)
@ -114,7 +119,7 @@ func main() {
var brakeCtrl brake.Controller var brakeCtrl brake.Controller
if enableBrake { if enableBrake {
brakeCtrl = brake.NewCustomControllerWithJsonConfig(brakeConfig) brakeCtrl = brake.NewCustomControllerWithJsonConfigAndAcceleratorFactor(brakeConfig, acceleratorFactor)
} else { } else {
brakeCtrl = &brake.DisabledController{} brakeCtrl = &brake.DisabledController{}
} }
@ -142,7 +147,7 @@ func main() {
} }
p := throttle.New(client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, p := throttle.New(client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
speedZoneTopic, types.Throttle(maxThrottle), 2, maxThrottleCtrlTopic, speedZoneTopic, types.Throttle(maxThrottle), 2,
throttle.WithThrottleProcessor(throttleProcessor), throttle.WithThrottleProcessor(throttleProcessor),
throttle.WithBrakeController(brakeCtrl)) throttle.WithBrakeController(brakeCtrl))
defer p.Stop() defer p.Stop()

View File

@ -20,13 +20,22 @@ func NewCustomControllerWithJsonConfig(filename string) *CustomController {
if err != nil { if err != nil {
zap.S().Panicf("unable to init brake controller with json config '%s': %v", filename, err) zap.S().Panicf("unable to init brake controller with json config '%s': %v", filename, err)
} }
return &CustomController{cfg: config} return &CustomController{cfg: config, acceleratorFactor: 1.0}
}
func NewCustomControllerWithJsonConfigAndAcceleratorFactor(filename string, acceleratorFactor float64) *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, acceleratorFactor: acceleratorFactor}
} }
type CustomController struct { type CustomController struct {
muRealThrottle sync.RWMutex muRealThrottle sync.RWMutex
realThrottle types.Throttle realThrottle types.Throttle
cfg *Config cfg *Config
acceleratorFactor float64
} }
func (b *CustomController) SetRealThrottle(t types.Throttle) { func (b *CustomController) SetRealThrottle(t types.Throttle) {
@ -43,6 +52,13 @@ func (b *CustomController) GetRealThrottle() types.Throttle {
} }
func (b *CustomController) AdjustThrottle(targetThrottle types.Throttle) types.Throttle { func (b *CustomController) AdjustThrottle(targetThrottle types.Throttle) types.Throttle {
if targetThrottle > b.GetRealThrottle() {
throttle := b.GetRealThrottle() + (targetThrottle-b.GetRealThrottle())*types.Throttle(b.acceleratorFactor)
if throttle > 1.0 {
throttle = 1.0
}
return throttle
}
return b.cfg.ValueOf(b.GetRealThrottle(), targetThrottle) return b.cfg.ValueOf(b.GetRealThrottle(), targetThrottle)
} }

View File

@ -7,7 +7,8 @@ import (
func TestController_AdjustThrottle(t *testing.T) { func TestController_AdjustThrottle(t *testing.T) {
type fields struct { type fields struct {
realThrottle types.Throttle realThrottle types.Throttle
acceleratorFactor float64
} }
type args struct { type args struct {
targetThrottle types.Throttle targetThrottle types.Throttle
@ -18,46 +19,89 @@ func TestController_AdjustThrottle(t *testing.T) {
args args args args
want types.Throttle want types.Throttle
}{ }{
{
name: "target same as current throttle with big acceleration",
fields: fields{realThrottle: 0.2, acceleratorFactor: 2.},
args: args{targetThrottle: 0.2},
want: 0.2,
},
{
name: "target > as current throttle with big acceleration",
fields: fields{realThrottle: 0.2, acceleratorFactor: 2.},
args: args{targetThrottle: 0.3},
want: 0.40000004,
},
{
name: "target >> as current throttle with big acceleration",
fields: fields{realThrottle: 0.2, acceleratorFactor: 2.},
args: args{targetThrottle: 0.5},
want: 0.8,
},
{
name: "target >> as current throttle with big acceleration, result > 1",
fields: fields{realThrottle: 0.2, acceleratorFactor: 3.},
args: args{targetThrottle: 0.5},
want: 1.0,
},
{
name: "target < as current throttle with big acceleration",
fields: fields{realThrottle: 0.8, acceleratorFactor: 2.},
args: args{targetThrottle: 0.7},
want: -0.1,
},
{
name: "target << as current throttle with big acceleration",
fields: fields{realThrottle: 0.8, acceleratorFactor: 2.},
args: args{targetThrottle: 0.5},
want: -0.5,
},
{
name: "target <<< as current throttle with big acceleration",
fields: fields{realThrottle: 0.8, acceleratorFactor: 2.},
args: args{targetThrottle: 0.2},
want: -1.,
},
{ {
name: "target same as current throttle", name: "target same as current throttle",
fields: fields{realThrottle: 0.2}, fields: fields{realThrottle: 0.2, acceleratorFactor: 1.},
args: args{targetThrottle: 0.2}, args: args{targetThrottle: 0.2},
want: 0.2, want: 0.2,
}, },
{ {
name: "target > as current throttle", name: "target > as current throttle",
fields: fields{realThrottle: 0.2}, fields: fields{realThrottle: 0.2, acceleratorFactor: 1.},
args: args{targetThrottle: 0.3}, args: args{targetThrottle: 0.3},
want: 0.3, want: 0.3,
}, },
{ {
name: "target >> as current throttle", name: "target >> as current throttle",
fields: fields{realThrottle: 0.2}, fields: fields{realThrottle: 0.2, acceleratorFactor: 1.},
args: args{targetThrottle: 0.5}, args: args{targetThrottle: 0.5},
want: 0.5, want: 0.5,
}, },
{ {
name: "target < as current throttle", name: "target < as current throttle",
fields: fields{realThrottle: 0.8}, fields: fields{realThrottle: 0.8, acceleratorFactor: 1.},
args: args{targetThrottle: 0.7}, args: args{targetThrottle: 0.7},
want: -0.1, want: -0.1,
}, },
{ {
name: "target << as current throttle", name: "target << as current throttle",
fields: fields{realThrottle: 0.8}, fields: fields{realThrottle: 0.8, acceleratorFactor: 1.},
args: args{targetThrottle: 0.5}, args: args{targetThrottle: 0.5},
want: -0.5, want: -0.5,
}, },
{ {
name: "target <<< as current throttle", name: "target <<< as current throttle",
fields: fields{realThrottle: 0.8}, fields: fields{realThrottle: 0.8, acceleratorFactor: 1.},
args: args{targetThrottle: 0.2}, args: args{targetThrottle: 0.2},
want: -1., want: -1.,
}, },
} }
for _, tt := range tests { for _, tt := range tests {
t.Run(tt.name, func(t *testing.T) { t.Run(tt.name, func(t *testing.T) {
b := &CustomController{cfg: NewConfig()} b := &CustomController{cfg: NewConfig(), acceleratorFactor: tt.fields.acceleratorFactor}
b.SetRealThrottle(tt.fields.realThrottle) b.SetRealThrottle(tt.fields.realThrottle)
if got := b.AdjustThrottle(tt.args.targetThrottle); got != tt.want { if got := b.AdjustThrottle(tt.args.targetThrottle); got != tt.want {
t.Errorf("AdjustThrottle() = %v, want %v", got, tt.want) t.Errorf("AdjustThrottle() = %v, want %v", got, tt.want)

View File

@ -13,7 +13,7 @@ import (
) )
func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
speedZoneTopic string, maxThrottleCtrlTopic, speedZoneTopic string,
maxValue types.Throttle, publishPilotFrequency int, opts ...Option) *Controller { maxValue types.Throttle, publishPilotFrequency int, opts ...Option) *Controller {
c := &Controller{ c := &Controller{
client: client, client: client,
@ -22,6 +22,7 @@ func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, ste
rcThrottleTopic: rcThrottleTopic, rcThrottleTopic: rcThrottleTopic,
steeringTopic: steeringTopic, steeringTopic: steeringTopic,
throttleFeedbackTopic: throttleFeedbackTopic, throttleFeedbackTopic: throttleFeedbackTopic,
maxThrottleCtrlTopic: maxThrottleCtrlTopic,
speedZoneTopic: speedZoneTopic, speedZoneTopic: speedZoneTopic,
maxThrottle: maxValue, maxThrottle: maxValue,
driveMode: events.DriveMode_USER, driveMode: events.DriveMode_USER,
@ -66,6 +67,7 @@ type Controller struct {
cancel chan interface{} cancel chan interface{}
publishPilotFrequency int publishPilotFrequency int
driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string
maxThrottleCtrlTopic string
speedZoneTopic string speedZoneTopic string
} }
@ -120,7 +122,7 @@ func (c *Controller) readSteering() types.Steering {
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, service.StopService("throttle", c.client, c.driveModeTopic, c.rcThrottleTopic, c.steeringTopic,
c.throttleFeedbackTopic, c.speedZoneTopic) c.throttleFeedbackTopic, c.maxThrottleCtrlTopic, c.speedZoneTopic)
} }
func (c *Controller) onThrottleFeedback(_ mqtt.Client, message mqtt.Message) { func (c *Controller) onThrottleFeedback(_ mqtt.Client, message mqtt.Message) {
@ -133,6 +135,18 @@ func (c *Controller) onThrottleFeedback(_ mqtt.Client, message mqtt.Message) {
c.brakeCtrl.SetRealThrottle(types.Throttle(msg.GetThrottle())) c.brakeCtrl.SetRealThrottle(types.Throttle(msg.GetThrottle()))
} }
func (c *Controller) onMaxThrottleCtrl(_ 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.muDriveMode.Lock()
defer c.muDriveMode.Unlock()
c.maxThrottle = 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)
@ -159,9 +173,11 @@ 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 {
zap.S().Debugf("throttle upper that max value allowed, patch value from %v to %v", throttleMsg.GetThrottle(), c.maxThrottle) if types.Throttle(throttleMsg.GetThrottle()) > 0. {
throttleMsg.Throttle = float32(c.maxThrottle) maxTh := c.maxThrottle
current := types.Throttle(throttleMsg.GetThrottle())
throttleMsg.Throttle = float32(current * maxTh)
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)
@ -217,6 +233,10 @@ var registerCallbacks = func(p *Controller) error {
if err != nil { if err != nil {
return err return err
} }
err = service.RegisterCallback(p.client, p.maxThrottleCtrlTopic, p.onMaxThrottleCtrl)
if err != nil {
return err
}
err = service.RegisterCallback(p.client, p.speedZoneTopic, p.onSpeedZone) err = service.RegisterCallback(p.client, p.speedZoneTopic, p.onSpeedZone)
if err != nil { if err != nil {
return err return err

View File

@ -36,9 +36,11 @@ func TestDefaultThrottle(t *testing.T) {
rcThrottleTopic := "topic/rcThrottle" rcThrottleTopic := "topic/rcThrottle"
steeringTopic := "topic/rcThrottle" steeringTopic := "topic/rcThrottle"
throttleFeedbackTopic := "topic/feedback/throttle" throttleFeedbackTopic := "topic/feedback/throttle"
maxThrottleCtrlTopic := "topic/max/throttle"
speedZoneTopic := "topic/speedZone" speedZoneTopic := "topic/speedZone"
p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, speedZoneTopic, 1., 200) p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
maxThrottleCtrlTopic, speedZoneTopic, 1., 200)
cases := []*struct { cases := []*struct {
name string name string
@ -57,8 +59,8 @@ func TestDefaultThrottle(t *testing.T) {
{"test8", 1., events.DriveModeMessage{DriveMode: events.DriveMode_COPILOT}, events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0}}, {"test8", 1., events.DriveModeMessage{DriveMode: events.DriveMode_COPILOT}, events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0}},
{"test9", 1., events.DriveModeMessage{DriveMode: events.DriveMode_COPILOT}, events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}}, {"test9", 1., events.DriveModeMessage{DriveMode: events.DriveMode_COPILOT}, events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}},
{"test10", 1., events.DriveModeMessage{DriveMode: events.DriveMode_COPILOT}, events.ThrottleMessage{Throttle: 0.6, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.6, Confidence: 1.0}}, {"test10", 1., events.DriveModeMessage{DriveMode: events.DriveMode_COPILOT}, events.ThrottleMessage{Throttle: 0.6, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.6, Confidence: 1.0}},
{"limit max throttle on user mode", 0.4, events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.ThrottleMessage{Throttle: 0.6, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}}, {"rescale throttle on user mode", 0.4, events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.ThrottleMessage{Throttle: 0.6, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.24000001, Confidence: 1.0}},
{"limit max throttle on copilot mode", 0.4, events.DriveModeMessage{DriveMode: events.DriveMode_COPILOT}, events.ThrottleMessage{Throttle: 0.6, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}}, {"rescale throttle on copilot mode", 0.4, events.DriveModeMessage{DriveMode: events.DriveMode_COPILOT}, events.ThrottleMessage{Throttle: 0.6, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.24000001, Confidence: 1.0}},
} }
go p.Start() go p.Start()
@ -121,11 +123,13 @@ func TestController_Start(t *testing.T) {
driveModeTopic := "topic/driveMode" driveModeTopic := "topic/driveMode"
rcThrottleTopic := "topic/rcThrottle" rcThrottleTopic := "topic/rcThrottle"
throttleFeedbackTopic := "topic/feedback/throttle" throttleFeedbackTopic := "topic/feedback/throttle"
maxThrottleCtrlTopic := "topic/max/throttle"
speedZoneTopic := "topic/speedZone" speedZoneTopic := "topic/speedZone"
type fields struct { type fields struct {
max types.Throttle
min types.Throttle
driveMode events.DriveMode driveMode events.DriveMode
min, max types.Throttle
publishPilotFrequency int publishPilotFrequency int
brakeCtl brake.Controller brakeCtl brake.Controller
} }
@ -134,6 +138,7 @@ func TestController_Start(t *testing.T) {
steering *events.SteeringMessage steering *events.SteeringMessage
rcThrottle *events.ThrottleMessage rcThrottle *events.ThrottleMessage
throttleFeedback *events.ThrottleMessage throttleFeedback *events.ThrottleMessage
maxThrottleCtrl *events.ThrottleMessage
} }
tests := []struct { tests := []struct {
@ -146,22 +151,23 @@ func TestController_Start(t *testing.T) {
{ {
name: "On user drive mode, throttle from rc", name: "On user drive mode, throttle from rc",
fields: fields{ fields: fields{
driveMode: events.DriveMode_USER,
max: 0.8, max: 0.8,
min: 0.3, min: 0.3,
driveMode: events.DriveMode_USER,
publishPilotFrequency: publishPilotFrequency, publishPilotFrequency: publishPilotFrequency,
brakeCtl: &brake.DisabledController{}, 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.5, Confidence: 1.0},
throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}, throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
maxThrottleCtrl: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
}, },
want: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}, want: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
}, },
{ {
name: "On user drive mode, limit throttle to max allowed value", name: "On user drive mode, rescale throttle with max allowed value",
fields: fields{ fields: fields{
driveMode: events.DriveMode_USER, driveMode: events.DriveMode_USER,
max: 0.8, max: 0.8,
@ -174,8 +180,9 @@ func TestController_Start(t *testing.T) {
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}, throttleFeedback: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
maxThrottleCtrl: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
}, },
want: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0}, want: &events.ThrottleMessage{Throttle: 0.71999997, Confidence: 1.0},
}, },
{ {
name: "On user drive mode, throttle can be < to min allowed value", name: "On user drive mode, throttle can be < to min allowed value",
@ -191,8 +198,27 @@ func TestController_Start(t *testing.T) {
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}, throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
maxThrottleCtrl: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
}, },
want: &events.ThrottleMessage{Throttle: 0.1, Confidence: 1.0}, want: &events.ThrottleMessage{Throttle: 0.080000006, Confidence: 1.0},
},
{
name: "On user drive mode, brake doesn't rescale throttle",
fields: fields{
driveMode: events.DriveMode_USER,
max: 0.8,
min: 0.3,
publishPilotFrequency: publishPilotFrequency,
brakeCtl: &brake.DisabledController{},
},
msgEvents: msgEvents{
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_USER},
steering: &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
rcThrottle: &events.ThrottleMessage{Throttle: -0.8, Confidence: 1.0},
throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
maxThrottleCtrl: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
},
want: &events.ThrottleMessage{Throttle: -0.8, Confidence: 1.0},
}, },
{ {
name: "On copilot drive mode, throttle from rc", name: "On copilot drive mode, throttle from rc",
@ -206,11 +232,30 @@ func TestController_Start(t *testing.T) {
msgEvents: msgEvents{ msgEvents: msgEvents{
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_COPILOT}, driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_COPILOT},
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.5, Confidence: 1.0},
throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}, throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
maxThrottleCtrl: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
}, },
want: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}, want: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
}, },
{
name: "On copilot drive mode, rescale throttle with max allowed value",
fields: fields{
driveMode: events.DriveMode_COPILOT,
max: 0.8,
min: 0.3,
publishPilotFrequency: publishPilotFrequency,
brakeCtl: &brake.DisabledController{},
},
msgEvents: msgEvents{
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_COPILOT},
steering: &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
rcThrottle: &events.ThrottleMessage{Throttle: 0.9, Confidence: 1.0},
throttleFeedback: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
maxThrottleCtrl: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
},
want: &events.ThrottleMessage{Throttle: 0.71999997, Confidence: 1.0},
},
{ {
name: "On copilot drive mode, limit throttle to max allowed value", name: "On copilot drive mode, limit throttle to max allowed value",
fields: fields{ fields: fields{
@ -225,8 +270,9 @@ func TestController_Start(t *testing.T) {
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}, throttleFeedback: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
maxThrottleCtrl: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
}, },
want: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0}, want: &events.ThrottleMessage{Throttle: 0.71999997, Confidence: 1.0},
}, },
{ {
name: "On copilot drive mode, throttle can be < to min allowed value", name: "On copilot drive mode, throttle can be < to min allowed value",
@ -242,8 +288,27 @@ func TestController_Start(t *testing.T) {
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}, throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
maxThrottleCtrl: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
}, },
want: &events.ThrottleMessage{Throttle: 0.1, Confidence: 1.0}, want: &events.ThrottleMessage{Throttle: 0.080000006, Confidence: 1.0},
},
{
name: "On user drive mode, brake doesn't rescale throttle",
fields: fields{
driveMode: events.DriveMode_COPILOT,
max: 0.8,
min: 0.3,
publishPilotFrequency: publishPilotFrequency,
brakeCtl: &brake.DisabledController{},
},
msgEvents: msgEvents{
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_COPILOT},
steering: &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
rcThrottle: &events.ThrottleMessage{Throttle: -0.8, Confidence: 1.0},
throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
maxThrottleCtrl: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
},
want: &events.ThrottleMessage{Throttle: -0.8, Confidence: 1.0},
}, },
{ {
name: "On pilot drive mode and straight steering, use max throttle allowed", name: "On pilot drive mode and straight steering, use max throttle allowed",
@ -259,6 +324,7 @@ func TestController_Start(t *testing.T) {
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}, throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
maxThrottleCtrl: &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},
}, },
@ -276,6 +342,7 @@ func TestController_Start(t *testing.T) {
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}, throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
maxThrottleCtrl: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
}, },
want: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0}, want: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
}, },
@ -293,6 +360,7 @@ func TestController_Start(t *testing.T) {
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}, throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
maxThrottleCtrl: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
}, },
want: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0}, want: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
}, },
@ -310,6 +378,7 @@ func TestController_Start(t *testing.T) {
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: 1.0, Confidence: 1.0}, throttleFeedback: &events.ThrottleMessage{Throttle: 1.0, Confidence: 1.0},
maxThrottleCtrl: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
}, },
want: &events.ThrottleMessage{Throttle: -1.0, Confidence: 1.0}, want: &events.ThrottleMessage{Throttle: -1.0, Confidence: 1.0},
}, },
@ -319,7 +388,7 @@ func TestController_Start(t *testing.T) {
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, throttleFeedbackTopic,
speedZoneTopic, tt.fields.max, maxThrottleCtrlTopic, speedZoneTopic, tt.fields.max,
tt.fields.publishPilotFrequency, tt.fields.publishPilotFrequency,
WithThrottleProcessor(&SteeringProcessor{ WithThrottleProcessor(&SteeringProcessor{
minThrottle: tt.fields.min, minThrottle: tt.fields.min,
@ -338,6 +407,7 @@ func TestController_Start(t *testing.T) {
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)) c.onThrottleFeedback(nil, testtools.NewFakeMessageFromProtobuf(throttleFeedbackTopic, tt.msgEvents.throttleFeedback))
c.onMaxThrottleCtrl(nil, testtools.NewFakeMessageFromProtobuf(maxThrottleCtrlTopic, tt.msgEvents.maxThrottleCtrl))
waitPublish.Wait() waitPublish.Wait()
var msg events.ThrottleMessage var msg events.ThrottleMessage