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

Compare commits

base: robocars:5eac26a73859505e4af7f6f86e16858c45bbbc51
Branches Tags
robocars:master
robocars:feat/brake
robocars:feat/use_steering_to_adjust_throttle
robocars:v0.9.0
robocars:v0.8.0
robocars:v0.7.0
robocars:v0.6.0
robocars:v0.5.0
robocars:v0.4.2
robocars:v0.4.1
robocars:v0.4.0
robocars:v0.3.0
robocars:v0.2.0
robocars:v0.1.0
..
compare: robocars:f4807ee9e60ecaed640b1068f6ebdb402bfe2ae9
Branches Tags
robocars:master
robocars:feat/brake
robocars:feat/use_steering_to_adjust_throttle
robocars:v0.9.0
robocars:v0.8.0
robocars:v0.7.0
robocars:v0.6.0
robocars:v0.5.0
robocars:v0.4.2
robocars:v0.4.1
robocars:v0.4.0
robocars:v0.3.0
robocars:v0.2.0
robocars:v0.1.0

No commits in common. "5eac26a73859505e4af7f6f86e16858c45bbbc51" and "f4807ee9e60ecaed640b1068f6ebdb402bfe2ae9" have entirely different histories.
5eac26a738 ... f4807ee9e6

3 changed files with 20 additions and 112 deletions
Show Stats Expand all files Collapse all files

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

@ -18,8 +18,7 @@ const (
func main() {
var mqttBroker, username, password, clientId string
var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, maxThrottleCtrlTopic,
speedZoneTopic string
var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, speedZoneTopic string
var minThrottle, maxThrottle float64
var publishPilotFrequency int
var brakeConfig string
@ -47,7 +46,6 @@ 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")
@ -144,7 +142,7 @@ func main() {
}
p := throttle.New(client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
maxThrottleCtrlTopic, speedZoneTopic, types.Throttle(maxThrottle), 2,
speedZoneTopic, types.Throttle(maxThrottle), 2,
throttle.WithThrottleProcessor(throttleProcessor),
throttle.WithBrakeController(brakeCtrl))
defer p.Stop()

30
pkg/throttle/controller.go
View File

@ -13,7 +13,7 @@ import (
)
func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
maxThrottleCtrlTopic, speedZoneTopic string,
speedZoneTopic string,
maxValue types.Throttle, publishPilotFrequency int, opts ...Option) *Controller {
c := &Controller{
client: client,
@ -22,7 +22,6 @@ 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,
@ -67,7 +66,6 @@ type Controller struct {
cancel chan interface{}
publishPilotFrequency int
driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string
maxThrottleCtrlTopic string
speedZoneTopic string
}
@ -122,7 +120,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.maxThrottleCtrlTopic, c.speedZoneTopic)
c.throttleFeedbackTopic, c.speedZoneTopic)
}
func (c *Controller) onThrottleFeedback(_ mqtt.Client, message mqtt.Message) {
@ -135,18 +133,6 @@ 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)
@ -173,11 +159,9 @@ func (c *Controller) onRCThrottle(_ mqtt.Client, message mqtt.Message) {
return
}
zap.S().Debugf("publish new throttle value from rc: %v", throttleMsg.GetThrottle())
if types.Throttle(throttleMsg.GetThrottle()) > 0. {
maxTh := c.maxThrottle
current := types.Throttle(throttleMsg.GetThrottle())
throttleMsg.Throttle = float32(current * maxTh)
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)
throttleMsg.Throttle = float32(c.maxThrottle)
payloadPatched, err := proto.Marshal(&throttleMsg)
if err != nil {
zap.S().Errorf("unable to marshall throttle msg: %v", err)
@ -233,10 +217,6 @@ 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

96
pkg/throttle/controller_test.go
View File

@ -36,11 +36,9 @@ 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,
maxThrottleCtrlTopic, speedZoneTopic, 1., 200)
p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, speedZoneTopic, 1., 200)
cases := []*struct {
name string
@ -59,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}},
{"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}},
{"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}},
{"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}},
{"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}},
{"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}},
}
go p.Start()
@ -123,13 +121,11 @@ 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
}
@ -138,7 +134,6 @@ func TestController_Start(t *testing.T) {
steering *events.SteeringMessage
rcThrottle *events.ThrottleMessage
throttleFeedback *events.ThrottleMessage
maxThrottleCtrl *events.ThrottleMessage
}
tests := []struct {
@ -151,23 +146,22 @@ 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{},
},
msgEvents: msgEvents{
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_USER},
steering: &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
rcThrottle: &events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0},
rcThrottle: &events.ThrottleMessage{Throttle: 0.4, 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},
},
{
name: "On user drive mode, rescale throttle with max allowed value",
name: "On user drive mode, limit throttle to max allowed value",
fields: fields{
driveMode: events.DriveMode_USER,
max: 0.8,
@ -180,9 +174,8 @@ 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},
want: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
},
{
name: "On user drive mode, throttle can be < to min allowed value",
@ -198,27 +191,8 @@ 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},
},
{
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},
maxThrottleCtrl: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
},
want: &events.ThrottleMessage{Throttle: -0.8, Confidence: 1.0},
want: &events.ThrottleMessage{Throttle: 0.1, Confidence: 1.0},
},
{
name: "On copilot drive mode, throttle from rc",
@ -232,30 +206,11 @@ func TestController_Start(t *testing.T) {
msgEvents: msgEvents{
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_COPILOT},
steering: &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
rcThrottle: &events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0},
rcThrottle: &events.ThrottleMessage{Throttle: 0.4, 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},
},
{
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},
maxThrottleCtrl: &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",
fields: fields{
@ -270,9 +225,8 @@ 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},
want: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
},
{
name: "On copilot drive mode, throttle can be < to min allowed value",
@ -288,27 +242,8 @@ 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},
},
{
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},
maxThrottleCtrl: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
},
want: &events.ThrottleMessage{Throttle: -0.8, Confidence: 1.0},
want: &events.ThrottleMessage{Throttle: 0.1, Confidence: 1.0},
},
{
name: "On pilot drive mode and straight steering, use max throttle allowed",
@ -324,7 +259,6 @@ 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},
},
@ -342,7 +276,6 @@ 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},
},
@ -360,7 +293,6 @@ 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},
},
@ -378,7 +310,6 @@ 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},
},
@ -388,7 +319,7 @@ func TestController_Start(t *testing.T) {
t.Run(tt.name, func(t *testing.T) {
c := New(nil,
throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
maxThrottleCtrlTopic, speedZoneTopic, tt.fields.max,
speedZoneTopic, tt.fields.max,
tt.fields.publishPilotFrequency,
WithThrottleProcessor(&SteeringProcessor{
minThrottle: tt.fields.min,
@ -407,7 +338,6 @@ 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