diff --git a/cmd/rc-throttle/rc-throttle.go b/cmd/rc-throttle/rc-throttle.go index cd38108..85a0dac 100644 --- a/cmd/rc-throttle/rc-throttle.go +++ b/cmd/rc-throttle/rc-throttle.go @@ -13,16 +13,19 @@ import ( const ( DefaultClientId = "robocar-throttle" - DefaultThrottleMin = 0.3 + DefaultThrottleMin = 0.1 ) func main() { var mqttBroker, username, password, clientId string - var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string + var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, speedZoneTopic string var minThrottle, maxThrottle float64 var publishPilotFrequency int var brakeConfig string var enableBrake bool + var enableSpeedZone bool + var slowZoneThrottle, normalZoneThrottle, fastZoneThrottle float64 + var moderateSteering, fullSteering float64 err := cli.SetFloat64DefaultValueFromEnv(&minThrottle, "THROTTLE_MIN", DefaultThrottleMin) if err != nil { @@ -43,6 +46,7 @@ func main() { 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.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") 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") @@ -50,6 +54,13 @@ func main() { flag.BoolVar(&enableBrake, "enable-brake-feature", false, "Enable brake to slow car on throttle changes") flag.StringVar(&brakeConfig, "brake-configuration", "", "Json file to use to configure brake adaptation when --enable-brake is `true`") + flag.BoolVar(&enableSpeedZone, "enable-speed-zone", false, "Enable speed zone information to estimate throttle") + flag.Float64Var(&slowZoneThrottle, "slow-zone-throttle", 0.11, "Throttle target for slow speed zone") + flag.Float64Var(&normalZoneThrottle, "normal-zone-throttle", 0.12, "Throttle target for normal speed zone") + flag.Float64Var(&fastZoneThrottle, "fast-zone-throttle", 0.13, "Throttle target for fast speed zone") + flag.Float64Var(&moderateSteering, "moderate-steering", 0.3, "Steering above is considered as moderate") + flag.Float64Var(&fullSteering, "full-steering", 0.8, "Steering above is considered as full") + logLevel := zap.LevelFlag("log", zap.InfoLevel, "log level") flag.Parse() @@ -71,15 +82,22 @@ func main() { }() zap.ReplaceGlobals(lgr) - zap.S().Infof("Topic throttle : %s", throttleTopic) - zap.S().Infof("Topic rc-throttle : %s", rcThrottleTopic) - zap.S().Infof("Topic throttle feedback : %s", throttleFeedbackTopic) - zap.S().Infof("Topic steering : %s", steeringTopic) - zap.S().Infof("Topic drive mode : %s", driveModeTopic) - zap.S().Infof("Min throttle : %v", minThrottle) - zap.S().Infof("Max throttle : %v", maxThrottle) - zap.S().Infof("Publish frequency : %vHz", publishPilotFrequency) - zap.S().Infof("Brake enabled : %v", enableBrake) + zap.S().Infof("Topic throttle : %s", throttleTopic) + zap.S().Infof("Topic rc-throttle : %s", rcThrottleTopic) + zap.S().Infof("Topic throttle feedback : %s", throttleFeedbackTopic) + zap.S().Infof("Topic steering : %s", steeringTopic) + zap.S().Infof("Topic drive mode : %s", driveModeTopic) + zap.S().Infof("Topic speed zone : %s", speedZoneTopic) + zap.S().Infof("Min throttle : %v", minThrottle) + zap.S().Infof("Max throttle : %v", maxThrottle) + zap.S().Infof("Publish frequency : %vHz", publishPilotFrequency) + zap.S().Infof("Brake enabled : %v", enableBrake) + zap.S().Infof("SpeedZone enabled : %v", enableSpeedZone) + zap.S().Infof("SpeedZone slow throttle : %v", slowZoneThrottle) + zap.S().Infof("SpeedZone normal throttle : %v", normalZoneThrottle) + zap.S().Infof("SpeedZone fast throttle : %v", fastZoneThrottle) + zap.S().Infof("Steering moderate : %v", moderateSteering) + zap.S().Infof("Steering full : %v", fullSteering) client, err := cli.Connect(mqttBroker, username, password, clientId) if err != nil { diff --git a/pkg/throttle/controller.go b/pkg/throttle/controller.go index 75fe94d..ef1a73b 100644 --- a/pkg/throttle/controller.go +++ b/pkg/throttle/controller.go @@ -12,8 +12,9 @@ import ( "time" ) -func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string, - minValue, maxValue types.Throttle, publishPilotFrequency int, opts ...Option) *Controller { +func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, + speedZoneTopic string, + maxValue types.Throttle, publishPilotFrequency int, opts ...Option) *Controller { c := &Controller{ client: client, throttleTopic: throttleTopic, @@ -21,10 +22,11 @@ func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, ste rcThrottleTopic: rcThrottleTopic, steeringTopic: steeringTopic, throttleFeedbackTopic: throttleFeedbackTopic, + speedZoneTopic: speedZoneTopic, maxThrottle: maxValue, driveMode: events.DriveMode_USER, publishPilotFrequency: publishPilotFrequency, - steeringProcessor: &SteeringProcessor{minThrottle: minValue, maxThrottle: maxValue}, + processor: &SteeringProcessor{minThrottle: 0.1, maxThrottle: maxValue}, brakeCtrl: &brake.DisabledController{}, } for _, o := range opts { @@ -41,23 +43,30 @@ func WithBrakeController(bc brake.Controller) Option { } } +func WithThrottleProcessor(p Processor) Option { + return func(c *Controller) { + c.processor = p + } +} + type Controller struct { - client mqtt.Client - throttleTopic string - maxThrottle types.Throttle - steeringProcessor *SteeringProcessor + client mqtt.Client + throttleTopic string + maxThrottle types.Throttle + processor Processor muDriveMode sync.RWMutex driveMode events.DriveMode muSteering sync.RWMutex - steering float32 + steering types.Steering brakeCtrl brake.Controller cancel chan interface{} publishPilotFrequency int driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string + speedZoneTopic string } func (c *Controller) Start() error { @@ -86,7 +95,7 @@ func (c *Controller) onPublishPilotValue() { return } - throttleFromSteering := c.steeringProcessor.Process(c.readSteering()) + throttleFromSteering := c.processor.Process(c.readSteering()) throttleMsg := events.ThrottleMessage{ Throttle: float32(c.brakeCtrl.AdjustThrottle(throttleFromSteering)), @@ -102,7 +111,7 @@ func (c *Controller) onPublishPilotValue() { } -func (c *Controller) readSteering() float32 { +func (c *Controller) readSteering() types.Steering { c.muSteering.RLock() defer c.muSteering.RUnlock() return c.steering @@ -110,7 +119,8 @@ func (c *Controller) readSteering() float32 { func (c *Controller) Stop() { close(c.cancel) - service.StopService("throttle", c.client, c.driveModeTopic, c.rcThrottleTopic, c.steeringTopic, c.throttleFeedbackTopic) + service.StopService("throttle", c.client, c.driveModeTopic, c.rcThrottleTopic, c.steeringTopic, + c.throttleFeedbackTopic, c.speedZoneTopic) } func (c *Controller) onThrottleFeedback(_ mqtt.Client, message mqtt.Message) { @@ -174,7 +184,18 @@ func (c *Controller) onSteering(_ mqtt.Client, message mqtt.Message) { } c.muSteering.Lock() defer c.muSteering.Unlock() - c.steering = steeringMsg.GetSteering() + c.steering = types.Steering(steeringMsg.GetSteering()) +} + +func (c *Controller) onSpeedZone(_ mqtt.Client, message mqtt.Message) { + var szMsg events.SpeedZoneMessage + payload := message.Payload() + err := proto.Unmarshal(payload, &szMsg) + if err != nil { + zap.S().Errorf("unable to unmarshal speedZone message, skip value: %v", err) + return + } + c.processor.SetSpeedZone(szMsg.GetSpeedZone()) } var registerCallbacks = func(p *Controller) error { @@ -196,6 +217,10 @@ var registerCallbacks = func(p *Controller) error { if err != nil { return err } + err = service.RegisterCallback(p.client, p.speedZoneTopic, p.onSpeedZone) + if err != nil { + return err + } return nil } diff --git a/pkg/throttle/controller_test.go b/pkg/throttle/controller_test.go index 9033da9..b5e94e5 100644 --- a/pkg/throttle/controller_test.go +++ b/pkg/throttle/controller_test.go @@ -36,10 +36,9 @@ func TestDefaultThrottle(t *testing.T) { rcThrottleTopic := "topic/rcThrottle" steeringTopic := "topic/rcThrottle" throttleFeedbackTopic := "topic/feedback/throttle" + speedZoneTopic := "topic/speedZone" - minValue := types.Throttle(0.56) - - p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, minValue, 1., 200) + p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, speedZoneTopic, 1., 200) cases := []*struct { name string @@ -117,6 +116,7 @@ func TestController_Start(t *testing.T) { driveModeTopic := "topic/driveMode" rcThrottleTopic := "topic/rcThrottle" throttleFeedbackTopic := "topic/feedback/throttle" + speedZoneTopic := "topic/speedZone" type fields struct { driveMode events.DriveMode @@ -263,7 +263,7 @@ func TestController_Start(t *testing.T) { t.Run(tt.name, func(t *testing.T) { c := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, - tt.fields.min, tt.fields.max, + speedZoneTopic, tt.fields.max, tt.fields.publishPilotFrequency, WithBrakeController(tt.fields.brakeCtl), ) diff --git a/pkg/throttle/processor.go b/pkg/throttle/processor.go index 01381d2..7980722 100644 --- a/pkg/throttle/processor.go +++ b/pkg/throttle/processor.go @@ -1,16 +1,96 @@ package throttle import ( + "encoding/json" + "fmt" + "github.com/cyrilix/robocar-protobuf/go/events" "github.com/cyrilix/robocar-throttle/pkg/types" "math" + "os" + "sync" ) +type Processor interface { + // Process compute throttle from steering value + Process(steering types.Steering) types.Throttle + + SetSpeedZone(sz events.SpeedZone) +} + +func NewSteeringProcessor(minThrottle, maxThrottle types.Throttle) *SteeringProcessor { + return &SteeringProcessor{ + minThrottle: minThrottle, + maxThrottle: maxThrottle, + } +} + type SteeringProcessor struct { minThrottle, maxThrottle types.Throttle } +func (sp *SteeringProcessor) SetSpeedZone(_ events.SpeedZone) { + return +} + // Process compute throttle from steering value -func (sp *SteeringProcessor) Process(steering float32) types.Throttle { +func (sp *SteeringProcessor) Process(steering types.Steering) types.Throttle { absSteering := math.Abs(float64(steering)) return sp.minThrottle + types.Throttle(float64(sp.maxThrottle-sp.minThrottle)*(1-absSteering)) } + +func NewSpeedZoneProcessor(slowThrottle, normalThrottle, fastThrottle types.Throttle, + moderateSteering, fullSteering float64) *SpeedZoneProcessor { + return &SpeedZoneProcessor{ + muSz: sync.Mutex{}, + speedZone: events.SpeedZone_UNKNOWN, + slowThrottle: slowThrottle, + normalThrottle: normalThrottle, + fastThrottle: fastThrottle, + moderateSteering: moderateSteering, + fullSteering: fullSteering, + } +} + +type SpeedZoneProcessor struct { + muSz sync.Mutex + speedZone events.SpeedZone + slowThrottle, normalThrottle, fastThrottle types.Throttle + moderateSteering, fullSteering float64 +} + +func (sp *SpeedZoneProcessor) SpeedZone() events.SpeedZone { + sp.muSz.Lock() + defer sp.muSz.Unlock() + return sp.speedZone +} + +func (sp *SpeedZoneProcessor) SetSpeedZone(sz events.SpeedZone) { + sp.muSz.Lock() + defer sp.muSz.Unlock() + sp.speedZone = sz +} + +// Process compute throttle from steering value +func (sp *SpeedZoneProcessor) Process(steering types.Steering) types.Throttle { + st := math.Abs(float64(steering)) + + switch sp.SpeedZone() { + case events.SpeedZone_FAST: + if st >= sp.fullSteering { + return sp.slowThrottle + } else if st >= sp.moderateSteering { + return sp.normalThrottle + } + return sp.fastThrottle + case events.SpeedZone_NORMAL: + if st > sp.fullSteering { + return sp.slowThrottle + } + return sp.normalThrottle + case events.SpeedZone_SLOW: + return sp.slowThrottle + } + return sp.slowThrottle +} + +func NewCustomSteeringProcessor(cfg *Config) *CustomSteeringProcessor { diff --git a/pkg/throttle/processor_test.go b/pkg/throttle/processor_test.go index c0157ff..4414064 100644 --- a/pkg/throttle/processor_test.go +++ b/pkg/throttle/processor_test.go @@ -1,7 +1,11 @@ package throttle import ( + "github.com/cyrilix/robocar-protobuf/go/events" "github.com/cyrilix/robocar-throttle/pkg/types" + "os" + "path" + "reflect" "testing" ) @@ -11,7 +15,7 @@ func TestSteeringProcessor_Process(t *testing.T) { maxThrottle types.Throttle } type args struct { - steering float32 + steering types.Steering } tests := []struct { name string @@ -87,3 +91,134 @@ func TestSteeringProcessor_Process(t *testing.T) { }) } } + +func TestSpeedZoneProcessor_Process(t *testing.T) { + type fields struct { + slowThrottle types.Throttle + normalThrottle types.Throttle + fastThrottle types.Throttle + speedZone events.SpeedZone + } + type args struct { + steering types.Steering + } + tests := []struct { + name string + fields fields + args args + want types.Throttle + }{ + { + name: "steering straight, undefined zone", + fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_SLOW}, + args: args{steering: 0.}, + want: 0.2, + }, + { + name: "steering straight, slow zone", + fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_SLOW}, + args: args{steering: 0.}, + want: 0.2, + }, + { + name: "moderate left, slow speed", + fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_SLOW}, + args: args{steering: -0.5}, + want: 0.2, + }, + { + name: "moderate right, slow speed", + fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_SLOW}, + args: args{steering: 0.5}, + want: 0.2, + }, + { + name: "full left, slow speed", + fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_SLOW}, + args: args{steering: -0.95}, + want: 0.2, + }, + { + name: "full right, slow speed", + fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_SLOW}, + args: args{steering: 0.95}, + want: 0.2, + }, + { + name: "steering straight, normal zone", + fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_NORMAL}, + args: args{steering: 0.}, + want: 0.5, + }, + { + name: "moderate left, normal speed", + fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_NORMAL}, + args: args{steering: -0.5}, + want: 0.5, + }, + { + name: "moderate right, normal speed", + fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_NORMAL}, + args: args{steering: 0.5}, + want: 0.5, + }, + { + name: "full left, normal speed", + fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_NORMAL}, + args: args{steering: -0.95}, + want: 0.2, + }, + { + name: "full right, normal speed", + fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_NORMAL}, + args: args{steering: 0.95}, + want: 0.2, + }, + { + name: "steering straight, fast zone", + fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_FAST}, + args: args{steering: 0.}, + want: 0.8, + }, + { + name: "moderate left, fast speed", + fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_FAST}, + args: args{steering: -0.5}, + want: 0.5, + }, + { + name: "moderate right, fast speed", + fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_FAST}, + args: args{steering: 0.5}, + want: 0.5, + }, + { + name: "full left, fast speed", + fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_FAST}, + args: args{steering: -0.95}, + want: 0.2, + }, + { + name: "full right, fast speed", + fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_FAST}, + args: args{steering: 0.95}, + want: 0.2, + }, + } + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + sp := &SpeedZoneProcessor{ + slowThrottle: tt.fields.slowThrottle, + normalThrottle: tt.fields.normalThrottle, + fastThrottle: tt.fields.fastThrottle, + moderateSteering: 0.4, + fullSteering: 0.8, + } + sp.SetSpeedZone(tt.fields.speedZone) + if got := sp.Process(tt.args.steering); got != tt.want { + t.Errorf("Process() = %v, want %v", got, tt.want) + } + }) + } +} + diff --git a/pkg/types/types.go b/pkg/types/types.go index a9afd66..2d3a933 100644 --- a/pkg/types/types.go +++ b/pkg/types/types.go @@ -1,3 +1,5 @@ package types type Throttle float32 + +type Steering float32