3 Commits

5 changed files with 188 additions and 33 deletions

View File

@ -18,11 +18,13 @@ const (
func main() {
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 publishPilotFrequency int
var brakeConfig string
var enableBrake bool
var acceleratorFactor float64
var enableSpeedZone bool
var enableCustomSteeringProcessor bool
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(&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(&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(&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")
@ -56,6 +59,7 @@ func main() {
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.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.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("Publish frequency : %vHz", publishPilotFrequency)
zap.S().Infof("Brake enabled : %v", enableBrake)
zap.S().Infof("Accelerator factor : %v", acceleratorFactor)
zap.S().Infof("CustomSteeringProcessor enabled: %v", enableCustomSteeringProcessor)
zap.S().Infof("SpeedZone enabled : %v", enableSpeedZone)
zap.S().Infof("SpeedZone slow throttle : %v", slowZoneThrottle)
@ -114,7 +119,7 @@ func main() {
var brakeCtrl brake.Controller
if enableBrake {
brakeCtrl = brake.NewCustomControllerWithJsonConfig(brakeConfig)
brakeCtrl = brake.NewCustomControllerWithJsonConfigAndAcceleratorFactor(brakeConfig, acceleratorFactor)
} else {
brakeCtrl = &brake.DisabledController{}
}
@ -142,7 +147,7 @@ func main() {
}
p := throttle.New(client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
speedZoneTopic, types.Throttle(maxThrottle), 2,
maxThrottleCtrlTopic, speedZoneTopic, types.Throttle(maxThrottle), 2,
throttle.WithThrottleProcessor(throttleProcessor),
throttle.WithBrakeController(brakeCtrl))
defer p.Stop()

View File

@ -20,13 +20,22 @@ func NewCustomControllerWithJsonConfig(filename string) *CustomController {
if err != nil {
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 {
muRealThrottle sync.RWMutex
realThrottle types.Throttle
cfg *Config
acceleratorFactor float64
}
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 {
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)
}

View File

@ -8,6 +8,7 @@ import (
func TestController_AdjustThrottle(t *testing.T) {
type fields struct {
realThrottle types.Throttle
acceleratorFactor float64
}
type args struct {
targetThrottle types.Throttle
@ -18,46 +19,89 @@ func TestController_AdjustThrottle(t *testing.T) {
args args
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",
fields: fields{realThrottle: 0.2},
fields: fields{realThrottle: 0.2, acceleratorFactor: 1.},
args: args{targetThrottle: 0.2},
want: 0.2,
},
{
name: "target > as current throttle",
fields: fields{realThrottle: 0.2},
fields: fields{realThrottle: 0.2, acceleratorFactor: 1.},
args: args{targetThrottle: 0.3},
want: 0.3,
},
{
name: "target >> as current throttle",
fields: fields{realThrottle: 0.2},
fields: fields{realThrottle: 0.2, acceleratorFactor: 1.},
args: args{targetThrottle: 0.5},
want: 0.5,
},
{
name: "target < as current throttle",
fields: fields{realThrottle: 0.8},
fields: fields{realThrottle: 0.8, acceleratorFactor: 1.},
args: args{targetThrottle: 0.7},
want: -0.1,
},
{
name: "target << as current throttle",
fields: fields{realThrottle: 0.8},
fields: fields{realThrottle: 0.8, acceleratorFactor: 1.},
args: args{targetThrottle: 0.5},
want: -0.5,
},
{
name: "target <<< as current throttle",
fields: fields{realThrottle: 0.8},
fields: fields{realThrottle: 0.8, acceleratorFactor: 1.},
args: args{targetThrottle: 0.2},
want: -1.,
},
}
for _, tt := range tests {
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)
if got := b.AdjustThrottle(tt.args.targetThrottle); 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,
speedZoneTopic string,
maxThrottleCtrlTopic, speedZoneTopic string,
maxValue types.Throttle, publishPilotFrequency int, opts ...Option) *Controller {
c := &Controller{
client: client,
@ -22,6 +22,7 @@ func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, ste
rcThrottleTopic: rcThrottleTopic,
steeringTopic: steeringTopic,
throttleFeedbackTopic: throttleFeedbackTopic,
maxThrottleCtrlTopic: maxThrottleCtrlTopic,
speedZoneTopic: speedZoneTopic,
maxThrottle: maxValue,
driveMode: events.DriveMode_USER,
@ -66,6 +67,7 @@ type Controller struct {
cancel chan interface{}
publishPilotFrequency int
driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string
maxThrottleCtrlTopic string
speedZoneTopic string
}
@ -120,7 +122,7 @@ func (c *Controller) readSteering() types.Steering {
func (c *Controller) Stop() {
close(c.cancel)
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) {
@ -133,6 +135,18 @@ func (c *Controller) onThrottleFeedback(_ mqtt.Client, message mqtt.Message) {
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) {
var msg events.DriveModeMessage
err := proto.Unmarshal(message.Payload(), &msg)
@ -159,9 +173,11 @@ func (c *Controller) onRCThrottle(_ mqtt.Client, message mqtt.Message) {
return
}
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)
throttleMsg.Throttle = float32(c.maxThrottle)
if types.Throttle(throttleMsg.GetThrottle()) > 0. {
maxTh := c.maxThrottle
current := types.Throttle(throttleMsg.GetThrottle())
throttleMsg.Throttle = float32(current * maxTh)
payloadPatched, err := proto.Marshal(&throttleMsg)
if err != nil {
zap.S().Errorf("unable to marshall throttle msg: %v", err)
@ -217,6 +233,10 @@ var registerCallbacks = func(p *Controller) error {
if err != nil {
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)
if err != nil {
return err

View File

@ -36,9 +36,11 @@ func TestDefaultThrottle(t *testing.T) {
rcThrottleTopic := "topic/rcThrottle"
steeringTopic := "topic/rcThrottle"
throttleFeedbackTopic := "topic/feedback/throttle"
maxThrottleCtrlTopic := "topic/max/throttle"
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 {
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}},
{"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}},
{"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}},
{"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 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}},
{"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()
@ -121,11 +123,13 @@ func TestController_Start(t *testing.T) {
driveModeTopic := "topic/driveMode"
rcThrottleTopic := "topic/rcThrottle"
throttleFeedbackTopic := "topic/feedback/throttle"
maxThrottleCtrlTopic := "topic/max/throttle"
speedZoneTopic := "topic/speedZone"
type fields struct {
max types.Throttle
min types.Throttle
driveMode events.DriveMode
min, max types.Throttle
publishPilotFrequency int
brakeCtl brake.Controller
}
@ -134,6 +138,7 @@ func TestController_Start(t *testing.T) {
steering *events.SteeringMessage
rcThrottle *events.ThrottleMessage
throttleFeedback *events.ThrottleMessage
maxThrottleCtrl *events.ThrottleMessage
}
tests := []struct {
@ -146,22 +151,23 @@ func TestController_Start(t *testing.T) {
{
name: "On user drive mode, throttle from rc",
fields: fields{
driveMode: events.DriveMode_USER,
max: 0.8,
min: 0.3,
driveMode: events.DriveMode_USER,
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.4, Confidence: 1.0},
rcThrottle: &events.ThrottleMessage{Throttle: 0.5, 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},
},
{
name: "On user drive mode, limit throttle to max allowed value",
name: "On user drive mode, rescale throttle with max allowed value",
fields: fields{
driveMode: events.DriveMode_USER,
max: 0.8,
@ -174,8 +180,9 @@ func TestController_Start(t *testing.T) {
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.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",
@ -191,8 +198,27 @@ func TestController_Start(t *testing.T) {
steering: &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
rcThrottle: &events.ThrottleMessage{Throttle: 0.1, 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",
@ -206,11 +232,30 @@ func TestController_Start(t *testing.T) {
msgEvents: msgEvents{
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_COPILOT},
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},
maxThrottleCtrl: &events.ThrottleMessage{Throttle: 0.8, 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",
fields: fields{
@ -225,8 +270,9 @@ func TestController_Start(t *testing.T) {
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.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",
@ -242,8 +288,27 @@ func TestController_Start(t *testing.T) {
steering: &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
rcThrottle: &events.ThrottleMessage{Throttle: 0.1, 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",
@ -259,6 +324,7 @@ func TestController_Start(t *testing.T) {
steering: &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
rcThrottle: &events.ThrottleMessage{Throttle: 0.5, 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},
},
@ -276,6 +342,7 @@ func TestController_Start(t *testing.T) {
steering: &events.SteeringMessage{Steering: -1.0, Confidence: 1.0},
rcThrottle: &events.ThrottleMessage{Throttle: 0.3, 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},
},
@ -293,6 +360,7 @@ func TestController_Start(t *testing.T) {
steering: &events.SteeringMessage{Steering: 1.0, Confidence: 1.0},
rcThrottle: &events.ThrottleMessage{Throttle: 0.3, 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},
},
@ -310,6 +378,7 @@ func TestController_Start(t *testing.T) {
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},
maxThrottleCtrl: &events.ThrottleMessage{Throttle: 0.8, 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) {
c := New(nil,
throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
speedZoneTopic, tt.fields.max,
maxThrottleCtrlTopic, speedZoneTopic, tt.fields.max,
tt.fields.publishPilotFrequency,
WithThrottleProcessor(&SteeringProcessor{
minThrottle: tt.fields.min,
@ -338,6 +407,7 @@ func TestController_Start(t *testing.T) {
c.onRCThrottle(nil, testtools.NewFakeMessageFromProtobuf(rcThrottleTopic, tt.msgEvents.rcThrottle))
c.onSteering(nil, testtools.NewFakeMessageFromProtobuf(steeringTopic, tt.msgEvents.steering))
c.onThrottleFeedback(nil, testtools.NewFakeMessageFromProtobuf(throttleFeedbackTopic, tt.msgEvents.throttleFeedback))
c.onMaxThrottleCtrl(nil, testtools.NewFakeMessageFromProtobuf(maxThrottleCtrlTopic, tt.msgEvents.maxThrottleCtrl))
waitPublish.Wait()
var msg events.ThrottleMessage