From dd0102ff44597cb3f470c99133abfd6b64cfa531 Mon Sep 17 00:00:00 2001 From: Cyrille Nofficial Date: Mon, 5 Sep 2022 11:22:31 +0200 Subject: [PATCH] feat(brake): subscribe to ThrottleFeedback messages --- cmd/rc-throttle/rc-throttle.go | 3 +- pkg/throttle/controller.go | 39 ++++++++++++++++++++--- pkg/throttle/controller_test.go | 56 +++++++++++++++++++-------------- 3 files changed, 69 insertions(+), 29 deletions(-) diff --git a/cmd/rc-throttle/rc-throttle.go b/cmd/rc-throttle/rc-throttle.go index 8e2630f..96ef209 100644 --- a/cmd/rc-throttle/rc-throttle.go +++ b/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") diff --git a/pkg/throttle/controller.go b/pkg/throttle/controller.go index 4bfc334..5878b43 100644 --- a/pkg/throttle/controller.go +++ b/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 - cancel chan interface{} - publishPilotFrequency int - driveModeTopic, rcThrottleTopic, steeringTopic string + muThrottleFeedback sync.RWMutex + throttleFeedback float32 + + cancel chan interface{} + publishPilotFrequency int + 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 } diff --git a/pkg/throttle/controller_test.go b/pkg/throttle/controller_test.go index 5032d3b..ff8db1e 100644 --- a/pkg/throttle/controller_test.go +++ b/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 @@ -120,9 +122,10 @@ func TestController_Start(t *testing.T) { publishPilotFrequency int } type msgEvents struct { - driveMode *events.DriveModeMessage - steering *events.SteeringMessage - rcThrottle *events.ThrottleMessage + driveMode *events.DriveModeMessage + steering *events.SteeringMessage + rcThrottle *events.ThrottleMessage + throttleFeedback *events.ThrottleMessage } tests := []struct { @@ -141,9 +144,10 @@ func TestController_Start(t *testing.T) { publishPilotFrequency: publishPilotFrequency, }, 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}, + 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}, }, @@ -156,9 +160,10 @@ func TestController_Start(t *testing.T) { publishPilotFrequency: publishPilotFrequency, }, msgEvents: msgEvents{ - 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}, + 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}, }, @@ -171,9 +176,10 @@ func TestController_Start(t *testing.T) { publishPilotFrequency: publishPilotFrequency, }, msgEvents: msgEvents{ - 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}, + 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}, }, @@ -186,9 +192,10 @@ func TestController_Start(t *testing.T) { publishPilotFrequency: publishPilotFrequency, }, msgEvents: msgEvents{ - 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}, + 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}, }, @@ -201,9 +208,10 @@ func TestController_Start(t *testing.T) { publishPilotFrequency: publishPilotFrequency, }, msgEvents: msgEvents{ - 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}, + 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}, }, @@ -216,9 +224,10 @@ func TestController_Start(t *testing.T) { publishPilotFrequency: publishPilotFrequency, }, msgEvents: msgEvents{ - 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}, + 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