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

feat: rescale throttle on USER/COPILOT modes

Browse Source
This commit is contained in:
Cyrille Nofficial 2023-11-02 17:03:49 +01:00
parent f4807ee9e6
commit 3920e4a369
2 changed files with 65 additions and 12 deletions

8
pkg/throttle/controller.go

@ -159,9 +159,11 @@ func (c *Controller) onRCThrottle(_ mqtt.Client, message mqtt.Message) {
return return
} }
zap.S().Debugf("publish new throttle value from rc: %v", throttleMsg.GetThrottle()) zap.S().Debugf("publish new throttle value from rc: %v", throttleMsg.GetThrottle())
if types.Throttle(throttleMsg.GetThrottle()) > c.maxThrottle {
zap.S().Debugf("throttle upper that max value allowed, patch value from %v to %v", throttleMsg.GetThrottle(), c.maxThrottle) if types.Throttle(throttleMsg.GetThrottle()) > 0. {
throttleMsg.Throttle = float32(c.maxThrottle) maxTh := c.maxThrottle
current := types.Throttle(throttleMsg.GetThrottle())
throttleMsg.Throttle = float32(current * maxTh)
payloadPatched, err := proto.Marshal(&throttleMsg) payloadPatched, err := proto.Marshal(&throttleMsg)
if err != nil { if err != nil {
zap.S().Errorf("unable to marshall throttle msg: %v", err) zap.S().Errorf("unable to marshall throttle msg: %v", err)

69
pkg/throttle/controller_test.go

@ -57,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}}, {"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}}, {"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}}, {"test10", 1., events.DriveModeMessage{DriveMode: events.DriveMode_COPILOT}, events.ThrottleMessage{Throttle: 0.6, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.6, 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 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 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}}, {"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}},
} }
go p.Start() go p.Start()
@ -155,13 +155,13 @@ func TestController_Start(t *testing.T) {
msgEvents: msgEvents{ msgEvents: msgEvents{
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_USER}, driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_USER},
steering: &events.SteeringMessage{Steering: 0.0, Confidence: 1.0}, steering: &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
rcThrottle: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}, rcThrottle: &events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0},
throttleFeedback: &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}, want: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
}, },
{ {
name: "On user drive mode, limit throttle to max allowed value", name: "On user drive mode, rescale throttle with max allowed value",
fields: fields{ fields: fields{
driveMode: events.DriveMode_USER, driveMode: events.DriveMode_USER,
max: 0.8, max: 0.8,
@ -175,7 +175,7 @@ func TestController_Start(t *testing.T) {
rcThrottle: &events.ThrottleMessage{Throttle: 0.9, Confidence: 1.0}, rcThrottle: &events.ThrottleMessage{Throttle: 0.9, Confidence: 1.0},
throttleFeedback: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0}, throttleFeedback: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
}, },
want: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0}, want: &events.ThrottleMessage{Throttle: 0.71999997, Confidence: 1.0},
}, },
{ {
name: "On user drive mode, throttle can be < to min allowed value", name: "On user drive mode, throttle can be < to min allowed value",
@ -192,7 +192,24 @@ func TestController_Start(t *testing.T) {
rcThrottle: &events.ThrottleMessage{Throttle: 0.1, Confidence: 1.0}, rcThrottle: &events.ThrottleMessage{Throttle: 0.1, Confidence: 1.0},
throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}, throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
}, },
want: &events.ThrottleMessage{Throttle: 0.1, Confidence: 1.0}, want: &events.ThrottleMessage{Throttle: 0.080000006, 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},
},
want: &events.ThrottleMessage{Throttle: -0.8, Confidence: 1.0},
}, },
{ {
name: "On copilot drive mode, throttle from rc", name: "On copilot drive mode, throttle from rc",
@ -206,11 +223,28 @@ func TestController_Start(t *testing.T) {
msgEvents: msgEvents{ msgEvents: msgEvents{
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_COPILOT}, driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_COPILOT},
steering: &events.SteeringMessage{Steering: 0.0, Confidence: 1.0}, steering: &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
rcThrottle: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}, rcThrottle: &events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0},
throttleFeedback: &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}, 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},
},
want: &events.ThrottleMessage{Throttle: 0.71999997, Confidence: 1.0},
},
{ {
name: "On copilot drive mode, limit throttle to max allowed value", name: "On copilot drive mode, limit throttle to max allowed value",
fields: fields{ fields: fields{
@ -226,7 +260,7 @@ func TestController_Start(t *testing.T) {
rcThrottle: &events.ThrottleMessage{Throttle: 0.9, Confidence: 1.0}, rcThrottle: &events.ThrottleMessage{Throttle: 0.9, Confidence: 1.0},
throttleFeedback: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0}, throttleFeedback: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
}, },
want: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0}, want: &events.ThrottleMessage{Throttle: 0.71999997, Confidence: 1.0},
}, },
{ {
name: "On copilot drive mode, throttle can be < to min allowed value", name: "On copilot drive mode, throttle can be < to min allowed value",
@ -243,7 +277,24 @@ func TestController_Start(t *testing.T) {
rcThrottle: &events.ThrottleMessage{Throttle: 0.1, Confidence: 1.0}, rcThrottle: &events.ThrottleMessage{Throttle: 0.1, Confidence: 1.0},
throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}, throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
}, },
want: &events.ThrottleMessage{Throttle: 0.1, Confidence: 1.0}, want: &events.ThrottleMessage{Throttle: 0.080000006, 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},
},
want: &events.ThrottleMessage{Throttle: -0.8, Confidence: 1.0},
}, },
{ {
name: "On pilot drive mode and straight steering, use max throttle allowed", name: "On pilot drive mode and straight steering, use max throttle allowed",