feat: configure maxThrottle with radio-command

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()

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

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