feat(brake): subscribe to ThrottleFeedback messages

cmd/rc-throttle/rc-throttle.go

@@ -16,7 +16,7 @@ const (
 
 func main() {
 	var mqttBroker, username, password, clientId string
-	var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic string
+	var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string
 	var minThrottle, maxThrottle float64
 	var publishPilotFrequency int
 
@@ -38,6 +38,7 @@ func main() {
 	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(&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.Float64Var(&minThrottle, "throttle-min", minThrottle, "Minimum throttle value, use THROTTLE_MIN if args not set")
 	flag.Float64Var(&maxThrottle, "throttle-max", maxThrottle, "Minimum throttle value, use THROTTLE_MAX if args not set")

pkg/throttle/controller.go

@@ -10,13 +10,15 @@ import (
 	"time"
 )
 
-func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic string, minValue, maxValue float32, publishPilotFrequency int) *Controller {
+func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string,
+	minValue, maxValue float32, publishPilotFrequency int) *Controller {
 	return &Controller{
 		client:                client,
 		throttleTopic:         throttleTopic,
 		driveModeTopic:        driveModeTopic,
 		rcThrottleTopic:       rcThrottleTopic,
 		steeringTopic:         steeringTopic,
+		throttleFeedbackTopic: throttleFeedbackTopic,
 		maxThrottle:           maxValue,
 		driveMode:             events.DriveMode_USER,
 		publishPilotFrequency: publishPilotFrequency,
@@ -37,9 +39,12 @@ type Controller struct {
 	muSteering sync.RWMutex
 	steering   float32
 
+	muThrottleFeedback sync.RWMutex
+	throttleFeedback   float32
+
 	cancel                                                                chan interface{}
 	publishPilotFrequency                                                 int
-	driveModeTopic, rcThrottleTopic, steeringTopic string
+	driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string
 }
 
 func (c *Controller) Start() error {
@@ -90,7 +95,20 @@ func (c *Controller) readSteering() float32 {
 
 func (c *Controller) Stop() {
 	close(c.cancel)
-	service.StopService("throttle", c.client, c.driveModeTopic, c.rcThrottleTopic, c.steeringTopic)
+	service.StopService("throttle", c.client, c.driveModeTopic, c.rcThrottleTopic, c.steeringTopic, c.throttleFeedbackTopic)
+}
+
+func (c *Controller) onThrottleFeedback(_ 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.muThrottleFeedback.Lock()
+	defer c.muThrottleFeedback.Unlock()
+	c.throttleFeedback = msg.GetThrottle()
 }
 
 func (c *Controller) onDriveMode(_ mqtt.Client, message mqtt.Message) {
@@ -147,6 +165,13 @@ func (c *Controller) onSteering(_ mqtt.Client, message mqtt.Message) {
 	c.steering = steeringMsg.GetSteering()
 }
 
+func (c *Controller) ThrottleFeedback() float32 {
+	c.muThrottleFeedback.RLock()
+	defer c.muThrottleFeedback.RUnlock()
+	tf := c.throttleFeedback
+	return tf
+}
+
 var registerCallbacks = func(p *Controller) error {
 	err := service.RegisterCallback(p.client, p.driveModeTopic, p.onDriveMode)
 	if err != nil {
@@ -162,6 +187,10 @@ var registerCallbacks = func(p *Controller) error {
 	if err != nil {
 		return err
 	}
+	err = service.RegisterCallback(p.client, p.throttleFeedbackTopic, p.onThrottleFeedback)
+	if err != nil {
+		return err
+	}
 	return nil
 }
 

pkg/throttle/controller_test.go

@@ -33,10 +33,11 @@ func TestDefaultThrottle(t *testing.T) {
 	driveModeTopic := "topic/driveMode"
 	rcThrottleTopic := "topic/rcThrottle"
 	steeringTopic := "topic/rcThrottle"
+	throttleFeedbackTopic := "topic/feedback/throttle"
 
 	minValue := float32(0.56)
 
-	p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, minValue, 1., 200)
+	p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, minValue, 1., 200)
 
 	cases := []struct {
 		name             string
@@ -113,6 +114,7 @@ func TestController_Start(t *testing.T) {
 	steeringTopic := "topic/steering"
 	driveModeTopic := "topic/driveMode"
 	rcThrottleTopic := "topic/rcThrottle"
+	throttleFeedbackTopic := "topic/feedback/throttle"
 
 	type fields struct {
 		driveMode             events.DriveMode
@@ -123,6 +125,7 @@ func TestController_Start(t *testing.T) {
 		driveMode        *events.DriveModeMessage
 		steering         *events.SteeringMessage
 		rcThrottle       *events.ThrottleMessage
+		throttleFeedback *events.ThrottleMessage
 	}
 
 	tests := []struct {
@@ -144,6 +147,7 @@ func TestController_Start(t *testing.T) {
 				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},
+				throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
 			},
 			want: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
 		},
@@ -159,6 +163,7 @@ func TestController_Start(t *testing.T) {
 				driveMode:        &events.DriveModeMessage{DriveMode: events.DriveMode_USER},
 				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},
 			},
 			want: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
 		},
@@ -174,6 +179,7 @@ func TestController_Start(t *testing.T) {
 				driveMode:        &events.DriveModeMessage{DriveMode: events.DriveMode_USER},
 				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},
 			},
 			want: &events.ThrottleMessage{Throttle: 0.1, Confidence: 1.0},
 		},
@@ -189,6 +195,7 @@ func TestController_Start(t *testing.T) {
 				driveMode:        &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
 				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},
 			},
 			want: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
 		},
@@ -204,6 +211,7 @@ func TestController_Start(t *testing.T) {
 				driveMode:        &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
 				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},
 			},
 			want: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
 		},
@@ -219,6 +227,7 @@ func TestController_Start(t *testing.T) {
 				driveMode:        &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
 				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},
 			},
 			want: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
 		},
@@ -227,7 +236,7 @@ func TestController_Start(t *testing.T) {
 	for _, tt := range tests {
 		t.Run(tt.name, func(t *testing.T) {
 			c := New(nil,
-				throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic,
+				throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
 				tt.fields.min, tt.fields.max,
 				tt.fields.publishPilotFrequency,
 			)
@@ -241,6 +250,7 @@ func TestController_Start(t *testing.T) {
 			c.onDriveMode(nil, testtools.NewFakeMessageFromProtobuf(driveModeTopic, tt.msgEvents.driveMode))
 			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))
 			waitPublish.Wait()
 
 			var msg events.ThrottleMessage