feat(brake): subscribe to ThrottleFeedback messages

This commit is contained in:
Cyrille Nofficial
2022-09-05 11:22:31 +02:00
parent 34c4433836
commit dd0102ff44
3 changed files with 69 additions and 29 deletions

View File

@ -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
}

View File

@ -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