Compare commits
3 Commits
f4807ee9e6
...
v0.9.0
Author | SHA1 | Date | |
---|---|---|---|
36efa2bef0 | |||
5eac26a738 | |||
3920e4a369 |
@ -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()
|
||||
|
@ -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
|
||||
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)
|
||||
}
|
||||
|
||||
|
@ -7,7 +7,8 @@ import (
|
||||
|
||||
func TestController_AdjustThrottle(t *testing.T) {
|
||||
type fields struct {
|
||||
realThrottle types.Throttle
|
||||
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)
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
Reference in New Issue
Block a user