diff --git a/cmd/rc-throttle/rc-throttle.go b/cmd/rc-throttle/rc-throttle.go index 4e758f1..0815d54 100644 --- a/cmd/rc-throttle/rc-throttle.go +++ b/cmd/rc-throttle/rc-throttle.go @@ -18,7 +18,8 @@ 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 @@ -46,6 +47,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") @@ -142,7 +144,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() diff --git a/pkg/throttle/controller.go b/pkg/throttle/controller.go index 4c4484a..a0b5359 100644 --- a/pkg/throttle/controller.go +++ b/pkg/throttle/controller.go @@ -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) @@ -219,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 diff --git a/pkg/throttle/controller_test.go b/pkg/throttle/controller_test.go index b80e334..29a7253 100644 --- a/pkg/throttle/controller_test.go +++ b/pkg/throttle/controller_test.go @@ -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 @@ -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,9 +151,9 @@ 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{}, }, @@ -157,6 +162,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.4, Confidence: 1.0}, }, @@ -174,6 +180,7 @@ 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.71999997, Confidence: 1.0}, }, @@ -191,6 +198,7 @@ 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.080000006, Confidence: 1.0}, }, @@ -208,6 +216,7 @@ func TestController_Start(t *testing.T) { 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}, }, @@ -225,6 +234,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.4, Confidence: 1.0}, }, @@ -242,6 +252,7 @@ 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.71999997, Confidence: 1.0}, }, @@ -259,6 +270,7 @@ 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.71999997, Confidence: 1.0}, }, @@ -276,6 +288,7 @@ 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.080000006, Confidence: 1.0}, }, @@ -293,6 +306,7 @@ func TestController_Start(t *testing.T) { 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}, }, @@ -310,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}, }, @@ -327,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}, }, @@ -344,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}, }, @@ -361,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}, }, @@ -370,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, @@ -389,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