cmd/rc-throttle/rc-throttle.go

@@ -18,8 +18,7 @@ const (
 
 func main() {
 	var mqttBroker, username, password, clientId string
-	var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, maxThrottleCtrlTopic,
+	var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, speedZoneTopic string
-		speedZoneTopic string
 	var minThrottle, maxThrottle float64
 	var publishPilotFrequency int
 	var brakeConfig string
@@ -47,7 +46,6 @@ 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")
@@ -144,7 +142,7 @@ func main() {
 	}
 
 	p := throttle.New(client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
-		maxThrottleCtrlTopic, speedZoneTopic, types.Throttle(maxThrottle), 2,
+		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,
-	maxThrottleCtrlTopic, speedZoneTopic string,
+	speedZoneTopic string,
 	maxValue types.Throttle, publishPilotFrequency int, opts ...Option) *Controller {
 	c := &Controller{
 		client:                client,
@@ -22,7 +22,6 @@ 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,
@@ -67,7 +66,6 @@ type Controller struct {
 	cancel                                                                chan interface{}
 	publishPilotFrequency                                                 int
 	driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string
-	maxThrottleCtrlTopic                                                  string
 	speedZoneTopic                                                        string
 }
 
@@ -122,7 +120,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.maxThrottleCtrlTopic, c.speedZoneTopic)
+		c.throttleFeedbackTopic, c.speedZoneTopic)
 }
 
 func (c *Controller) onThrottleFeedback(_ mqtt.Client, message mqtt.Message) {
@@ -135,18 +133,6 @@ 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)
@@ -173,11 +159,9 @@ 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 {
-		if types.Throttle(throttleMsg.GetThrottle()) > 0. {
+			zap.S().Debugf("throttle upper that max value allowed, patch value from %v to %v", throttleMsg.GetThrottle(), c.maxThrottle)
-			maxTh := c.maxThrottle
+			throttleMsg.Throttle = float32(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)
@@ -233,10 +217,6 @@ 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,11 +36,9 @@ 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,
+	p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, speedZoneTopic, 1., 200)
-		maxThrottleCtrlTopic, speedZoneTopic, 1., 200)
 
 	cases := []*struct {
 		name             string
@@ -59,8 +57,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}},
-		{"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 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 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}},
+		{"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}},
 	}
 
 	go p.Start()
@@ -123,13 +121,11 @@ 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
 	}
@@ -138,7 +134,6 @@ func TestController_Start(t *testing.T) {
 		steering         *events.SteeringMessage
 		rcThrottle       *events.ThrottleMessage
 		throttleFeedback *events.ThrottleMessage
-		maxThrottleCtrl  *events.ThrottleMessage
 	}
 
 	tests := []struct {
@@ -151,23 +146,22 @@ 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.5, Confidence: 1.0},
+				rcThrottle:       &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},
 		},
 		{
-			name: "On user drive mode, rescale throttle with max allowed value",
+			name: "On user drive mode, limit throttle to max allowed value",
 			fields: fields{
 				driveMode:             events.DriveMode_USER,
 				max:                   0.8,
@@ -180,9 +174,8 @@ 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},
+			want: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
 		},
 		{
 			name: "On user drive mode, throttle can be < to  min allowed value",
@@ -198,27 +191,8 @@ 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},
+			want: &events.ThrottleMessage{Throttle: 0.1, 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",
@@ -232,30 +206,11 @@ 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.5, Confidence: 1.0},
+				rcThrottle:       &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},
 		},
-		{
-			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{
@@ -270,9 +225,8 @@ 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},
+			want: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
 		},
 		{
 			name: "On copilot drive mode, throttle can be < to  min allowed value",
@@ -288,27 +242,8 @@ 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},
+			want: &events.ThrottleMessage{Throttle: 0.1, 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",
@@ -324,7 +259,6 @@ 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},
 		},
@@ -342,7 +276,6 @@ 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},
 		},
@@ -360,7 +293,6 @@ 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},
 		},
@@ -378,7 +310,6 @@ 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},
 		},
@@ -388,7 +319,7 @@ func TestController_Start(t *testing.T) {
 		t.Run(tt.name, func(t *testing.T) {
 			c := New(nil,
 				throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
-				maxThrottleCtrlTopic, speedZoneTopic, tt.fields.max,
+				speedZoneTopic, tt.fields.max,
 				tt.fields.publishPilotFrequency,
 				WithThrottleProcessor(&SteeringProcessor{
 					minThrottle: tt.fields.min,
@@ -407,7 +338,6 @@ 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