robocars/robocar-throttle
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

feat: configure maxThrottle with radio-command

Browse Source
This commit is contained in:
Cyrille Nofficial 2023-11-02 19:03:40 +01:00
parent 3920e4a369
commit 5eac26a738
3 changed files with 47 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
cmd/rc-throttle/rc-throttle.go
View File

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

22
pkg/throttle/controller.go
View File

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

27
pkg/throttle/controller_test.go
View File

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