Compare commits
No commits in common. "838c9b4ef34ff0e8aadae7b0c3d173aee532f24c" and "34c4433836ff89ef717331ae994682d77c9a7e1c" have entirely different histories.
838c9b4ef3
...
34c4433836
@ -3,9 +3,7 @@ package main
|
|||||||
import (
|
import (
|
||||||
"flag"
|
"flag"
|
||||||
"github.com/cyrilix/robocar-base/cli"
|
"github.com/cyrilix/robocar-base/cli"
|
||||||
"github.com/cyrilix/robocar-throttle/pkg/brake"
|
|
||||||
"github.com/cyrilix/robocar-throttle/pkg/throttle"
|
"github.com/cyrilix/robocar-throttle/pkg/throttle"
|
||||||
"github.com/cyrilix/robocar-throttle/pkg/types"
|
|
||||||
"go.uber.org/zap"
|
"go.uber.org/zap"
|
||||||
"log"
|
"log"
|
||||||
"os"
|
"os"
|
||||||
@ -18,11 +16,9 @@ const (
|
|||||||
|
|
||||||
func main() {
|
func main() {
|
||||||
var mqttBroker, username, password, clientId string
|
var mqttBroker, username, password, clientId string
|
||||||
var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string
|
var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic string
|
||||||
var minThrottle, maxThrottle float64
|
var minThrottle, maxThrottle float64
|
||||||
var publishPilotFrequency int
|
var publishPilotFrequency int
|
||||||
var brakeConfig string
|
|
||||||
var enableBrake bool
|
|
||||||
|
|
||||||
err := cli.SetFloat64DefaultValueFromEnv(&minThrottle, "THROTTLE_MIN", DefaultThrottleMin)
|
err := cli.SetFloat64DefaultValueFromEnv(&minThrottle, "THROTTLE_MIN", DefaultThrottleMin)
|
||||||
if err != nil {
|
if err != nil {
|
||||||
@ -42,14 +38,11 @@ func main() {
|
|||||||
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(&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(&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(&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.Float64Var(&minThrottle, "throttle-min", minThrottle, "Minimum throttle value, use THROTTLE_MIN 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")
|
flag.Float64Var(&maxThrottle, "throttle-max", maxThrottle, "Minimum throttle value, use THROTTLE_MAX if args not set")
|
||||||
flag.IntVar(&publishPilotFrequency, "update-pwm-frequency", 2, "Number of throttle event to publish when pilot mode is enabled")
|
flag.IntVar(&publishPilotFrequency, "update-pwm-frequency", 2, "Number of throttle event to publish when pilot mode is enabled")
|
||||||
|
|
||||||
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`")
|
|
||||||
logLevel := zap.LevelFlag("log", zap.InfoLevel, "log level")
|
logLevel := zap.LevelFlag("log", zap.InfoLevel, "log level")
|
||||||
|
|
||||||
flag.Parse()
|
flag.Parse()
|
||||||
@ -77,14 +70,7 @@ func main() {
|
|||||||
}
|
}
|
||||||
defer client.Disconnect(50)
|
defer client.Disconnect(50)
|
||||||
|
|
||||||
var brakeCtrl brake.Controller
|
p := throttle.New(client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, float32(minThrottle), float32(maxThrottle), 2)
|
||||||
if enableBrake {
|
|
||||||
brakeCtrl = brake.NewCustomControllerWithJsonConfig(brakeConfig)
|
|
||||||
} else {
|
|
||||||
brakeCtrl = &brake.DisabledController{}
|
|
||||||
}
|
|
||||||
p := throttle.New(client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
|
|
||||||
types.Throttle(minThrottle), types.Throttle(maxThrottle), 2, throttle.WithBrakeController(brakeCtrl))
|
|
||||||
defer p.Stop()
|
defer p.Stop()
|
||||||
|
|
||||||
cli.HandleExit(p)
|
cli.HandleExit(p)
|
||||||
|
@ -1,54 +0,0 @@
|
|||||||
package brake
|
|
||||||
|
|
||||||
import (
|
|
||||||
"encoding/json"
|
|
||||||
"fmt"
|
|
||||||
"github.com/cyrilix/robocar-throttle/pkg/types"
|
|
||||||
"os"
|
|
||||||
)
|
|
||||||
|
|
||||||
var (
|
|
||||||
defaultBrakeConfig = Config{
|
|
||||||
DeltaSteps: []float32{0.05, 0.3, 0.5},
|
|
||||||
Data: []types.Throttle{-0.1, -0.5, -1.},
|
|
||||||
}
|
|
||||||
)
|
|
||||||
|
|
||||||
func NewConfig() *Config {
|
|
||||||
return &defaultBrakeConfig
|
|
||||||
}
|
|
||||||
|
|
||||||
func NewConfigFromJson(fileName string) (*Config, error) {
|
|
||||||
content, err := os.ReadFile(fileName)
|
|
||||||
if err != nil {
|
|
||||||
return nil, fmt.Errorf("unable to read content from %s file: %w", fileName, err)
|
|
||||||
}
|
|
||||||
var ft Config
|
|
||||||
err = json.Unmarshal(content, &ft)
|
|
||||||
if err != nil {
|
|
||||||
return nil, fmt.Errorf("unable to unmarshal json content from %s file: %w", fileName, err)
|
|
||||||
}
|
|
||||||
return &ft, nil
|
|
||||||
}
|
|
||||||
|
|
||||||
type Config struct {
|
|
||||||
DeltaSteps []float32 `json:"delta_steps"`
|
|
||||||
Data []types.Throttle `json:"data"`
|
|
||||||
}
|
|
||||||
|
|
||||||
func (tc *Config) ValueOf(currentThrottle, targetThrottle types.Throttle) types.Throttle {
|
|
||||||
delta := float32(currentThrottle - targetThrottle)
|
|
||||||
|
|
||||||
if delta < tc.DeltaSteps[0] {
|
|
||||||
return targetThrottle
|
|
||||||
}
|
|
||||||
if delta >= tc.DeltaSteps[len(tc.DeltaSteps)-1] {
|
|
||||||
return tc.Data[len(tc.Data)-1]
|
|
||||||
}
|
|
||||||
for idx, step := range tc.DeltaSteps {
|
|
||||||
if delta < step {
|
|
||||||
return tc.Data[idx-1]
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return tc.Data[len(tc.Data)-1]
|
|
||||||
}
|
|
@ -1,132 +0,0 @@
|
|||||||
package brake
|
|
||||||
|
|
||||||
import (
|
|
||||||
"github.com/cyrilix/robocar-throttle/pkg/types"
|
|
||||||
"reflect"
|
|
||||||
"testing"
|
|
||||||
)
|
|
||||||
|
|
||||||
func TestNewConfigFromJson(t *testing.T) {
|
|
||||||
type args struct {
|
|
||||||
fileName string
|
|
||||||
}
|
|
||||||
tests := []struct {
|
|
||||||
name string
|
|
||||||
args args
|
|
||||||
want *Config
|
|
||||||
wantErr bool
|
|
||||||
}{
|
|
||||||
{
|
|
||||||
name: "default config",
|
|
||||||
args: args{
|
|
||||||
fileName: "test_data/config.json",
|
|
||||||
},
|
|
||||||
want: &defaultBrakeConfig,
|
|
||||||
},
|
|
||||||
}
|
|
||||||
for _, tt := range tests {
|
|
||||||
t.Run(tt.name, func(t *testing.T) {
|
|
||||||
got, err := NewConfigFromJson(tt.args.fileName)
|
|
||||||
if (err != nil) != tt.wantErr {
|
|
||||||
t.Errorf("NewConfigFromJson() error = %v, wantErr %v", err, tt.wantErr)
|
|
||||||
return
|
|
||||||
}
|
|
||||||
if !reflect.DeepEqual(*got, *tt.want) {
|
|
||||||
t.Errorf("NewConfigFromJson() got = %v, want %v", got, tt.want)
|
|
||||||
}
|
|
||||||
if !reflect.DeepEqual(got.DeltaSteps, tt.want.DeltaSteps) {
|
|
||||||
t.Errorf("NewConfigFromJson(), bad DeltaSteps: got = %v, want %v", got.DeltaSteps, tt.want.DeltaSteps)
|
|
||||||
}
|
|
||||||
})
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
func TestConfig_ValueOf(t *testing.T) {
|
|
||||||
type fields struct {
|
|
||||||
DeltaSteps []float32
|
|
||||||
MinValue int
|
|
||||||
Data []types.Throttle
|
|
||||||
}
|
|
||||||
type args struct {
|
|
||||||
currentThrottle, targetThrottle types.Throttle
|
|
||||||
}
|
|
||||||
tests := []struct {
|
|
||||||
name string
|
|
||||||
fields fields
|
|
||||||
args args
|
|
||||||
want types.Throttle
|
|
||||||
}{
|
|
||||||
{
|
|
||||||
name: "delta > 0",
|
|
||||||
fields: fields{
|
|
||||||
DeltaSteps: defaultBrakeConfig.DeltaSteps,
|
|
||||||
Data: defaultBrakeConfig.Data,
|
|
||||||
},
|
|
||||||
args: args{
|
|
||||||
currentThrottle: 0.5,
|
|
||||||
targetThrottle: 0.8,
|
|
||||||
},
|
|
||||||
want: 0.8,
|
|
||||||
},
|
|
||||||
{
|
|
||||||
name: "no delta",
|
|
||||||
fields: fields{
|
|
||||||
DeltaSteps: defaultBrakeConfig.DeltaSteps,
|
|
||||||
Data: defaultBrakeConfig.Data,
|
|
||||||
},
|
|
||||||
args: args{
|
|
||||||
currentThrottle: 0.5,
|
|
||||||
targetThrottle: 0.5,
|
|
||||||
},
|
|
||||||
want: 0.5,
|
|
||||||
},
|
|
||||||
{
|
|
||||||
name: "delta very low (< 1st step)",
|
|
||||||
fields: fields{
|
|
||||||
DeltaSteps: defaultBrakeConfig.DeltaSteps,
|
|
||||||
Data: defaultBrakeConfig.Data,
|
|
||||||
},
|
|
||||||
args: args{
|
|
||||||
currentThrottle: 0.5,
|
|
||||||
targetThrottle: 0.495,
|
|
||||||
},
|
|
||||||
want: 0.495,
|
|
||||||
},
|
|
||||||
{
|
|
||||||
name: "low delta ( 1st step < delta < 2nd step )",
|
|
||||||
fields: fields{
|
|
||||||
DeltaSteps: defaultBrakeConfig.DeltaSteps,
|
|
||||||
Data: defaultBrakeConfig.Data,
|
|
||||||
},
|
|
||||||
args: args{
|
|
||||||
currentThrottle: 0.5,
|
|
||||||
targetThrottle: 0.38,
|
|
||||||
},
|
|
||||||
want: -0.1,
|
|
||||||
},
|
|
||||||
{
|
|
||||||
name: "high delta",
|
|
||||||
fields: fields{
|
|
||||||
DeltaSteps: defaultBrakeConfig.DeltaSteps,
|
|
||||||
Data: defaultBrakeConfig.Data,
|
|
||||||
},
|
|
||||||
args: args{
|
|
||||||
currentThrottle: 0.8,
|
|
||||||
targetThrottle: 0.3,
|
|
||||||
},
|
|
||||||
want: -1.,
|
|
||||||
},
|
|
||||||
}
|
|
||||||
for _, tt := range tests {
|
|
||||||
t.Run(tt.name, func(t *testing.T) {
|
|
||||||
f := &Config{
|
|
||||||
DeltaSteps: tt.fields.DeltaSteps,
|
|
||||||
Data: tt.fields.Data,
|
|
||||||
}
|
|
||||||
got := f.ValueOf(tt.args.currentThrottle, tt.args.targetThrottle)
|
|
||||||
if !reflect.DeepEqual(got, tt.want) {
|
|
||||||
t.Errorf("ValueOf() = %v, want %v", got, tt.want)
|
|
||||||
}
|
|
||||||
})
|
|
||||||
}
|
|
||||||
}
|
|
@ -1,55 +0,0 @@
|
|||||||
package brake
|
|
||||||
|
|
||||||
import (
|
|
||||||
"github.com/cyrilix/robocar-throttle/pkg/types"
|
|
||||||
"go.uber.org/zap"
|
|
||||||
"sync"
|
|
||||||
)
|
|
||||||
|
|
||||||
type Controller interface {
|
|
||||||
SetRealThrottle(t types.Throttle)
|
|
||||||
AdjustThrottle(targetThrottle types.Throttle) types.Throttle
|
|
||||||
}
|
|
||||||
|
|
||||||
func NewCustomController() *CustomController {
|
|
||||||
return &CustomController{cfg: NewConfig()}
|
|
||||||
}
|
|
||||||
|
|
||||||
func NewCustomControllerWithJsonConfig(filename string) *CustomController {
|
|
||||||
config, err := NewConfigFromJson(filename)
|
|
||||||
if err != nil {
|
|
||||||
zap.S().Panicf("unable to init brake controller with json config '%s': %v", filename, err)
|
|
||||||
}
|
|
||||||
return &CustomController{cfg: config}
|
|
||||||
}
|
|
||||||
|
|
||||||
type CustomController struct {
|
|
||||||
muRealThrottle sync.RWMutex
|
|
||||||
realThrottle types.Throttle
|
|
||||||
cfg *Config
|
|
||||||
}
|
|
||||||
|
|
||||||
func (b *CustomController) SetRealThrottle(t types.Throttle) {
|
|
||||||
b.muRealThrottle.Lock()
|
|
||||||
defer b.muRealThrottle.Unlock()
|
|
||||||
b.realThrottle = t
|
|
||||||
}
|
|
||||||
|
|
||||||
func (b *CustomController) GetRealThrottle() types.Throttle {
|
|
||||||
b.muRealThrottle.RLock()
|
|
||||||
defer b.muRealThrottle.RUnlock()
|
|
||||||
res := b.realThrottle
|
|
||||||
return res
|
|
||||||
}
|
|
||||||
|
|
||||||
func (b *CustomController) AdjustThrottle(targetThrottle types.Throttle) types.Throttle {
|
|
||||||
return b.cfg.ValueOf(b.GetRealThrottle(), targetThrottle)
|
|
||||||
}
|
|
||||||
|
|
||||||
type DisabledController struct{}
|
|
||||||
|
|
||||||
func (d *DisabledController) SetRealThrottle(_ types.Throttle) {}
|
|
||||||
|
|
||||||
func (d *DisabledController) AdjustThrottle(targetThrottle types.Throttle) types.Throttle {
|
|
||||||
return targetThrottle
|
|
||||||
}
|
|
@ -1,92 +0,0 @@
|
|||||||
package brake
|
|
||||||
|
|
||||||
import (
|
|
||||||
"github.com/cyrilix/robocar-throttle/pkg/types"
|
|
||||||
"testing"
|
|
||||||
)
|
|
||||||
|
|
||||||
func TestController_AdjustThrottle(t *testing.T) {
|
|
||||||
type fields struct {
|
|
||||||
realThrottle types.Throttle
|
|
||||||
}
|
|
||||||
type args struct {
|
|
||||||
targetThrottle types.Throttle
|
|
||||||
}
|
|
||||||
tests := []struct {
|
|
||||||
name string
|
|
||||||
fields fields
|
|
||||||
args args
|
|
||||||
want types.Throttle
|
|
||||||
}{
|
|
||||||
{
|
|
||||||
name: "target same as current throttle",
|
|
||||||
fields: fields{realThrottle: 0.2},
|
|
||||||
args: args{targetThrottle: 0.2},
|
|
||||||
want: 0.2,
|
|
||||||
},
|
|
||||||
{
|
|
||||||
name: "target > as current throttle",
|
|
||||||
fields: fields{realThrottle: 0.2},
|
|
||||||
args: args{targetThrottle: 0.3},
|
|
||||||
want: 0.3,
|
|
||||||
},
|
|
||||||
{
|
|
||||||
name: "target >> as current throttle",
|
|
||||||
fields: fields{realThrottle: 0.2},
|
|
||||||
args: args{targetThrottle: 0.5},
|
|
||||||
want: 0.5,
|
|
||||||
},
|
|
||||||
{
|
|
||||||
name: "target < as current throttle",
|
|
||||||
fields: fields{realThrottle: 0.8},
|
|
||||||
args: args{targetThrottle: 0.7},
|
|
||||||
want: -0.1,
|
|
||||||
},
|
|
||||||
{
|
|
||||||
name: "target << as current throttle",
|
|
||||||
fields: fields{realThrottle: 0.8},
|
|
||||||
args: args{targetThrottle: 0.5},
|
|
||||||
want: -0.5,
|
|
||||||
},
|
|
||||||
{
|
|
||||||
name: "target <<< as current throttle",
|
|
||||||
fields: fields{realThrottle: 0.8},
|
|
||||||
args: args{targetThrottle: 0.2},
|
|
||||||
want: -1.,
|
|
||||||
},
|
|
||||||
}
|
|
||||||
for _, tt := range tests {
|
|
||||||
t.Run(tt.name, func(t *testing.T) {
|
|
||||||
b := &CustomController{cfg: NewConfig()}
|
|
||||||
b.SetRealThrottle(tt.fields.realThrottle)
|
|
||||||
if got := b.AdjustThrottle(tt.args.targetThrottle); got != tt.want {
|
|
||||||
t.Errorf("AdjustThrottle() = %v, want %v", got, tt.want)
|
|
||||||
}
|
|
||||||
})
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
func TestDisabledController_AdjustThrottle(t *testing.T) {
|
|
||||||
type args struct {
|
|
||||||
targetThrottle types.Throttle
|
|
||||||
}
|
|
||||||
tests := []struct {
|
|
||||||
name string
|
|
||||||
args args
|
|
||||||
want types.Throttle
|
|
||||||
}{
|
|
||||||
{
|
|
||||||
name: "doesn't modify value",
|
|
||||||
args: args{targetThrottle: 0.5},
|
|
||||||
want: 0.5,
|
|
||||||
},
|
|
||||||
}
|
|
||||||
for _, tt := range tests {
|
|
||||||
t.Run(tt.name, func(t *testing.T) {
|
|
||||||
d := &DisabledController{}
|
|
||||||
if got := d.AdjustThrottle(tt.args.targetThrottle); got != tt.want {
|
|
||||||
t.Errorf("AdjustThrottle() = %v, want %v", got, tt.want)
|
|
||||||
}
|
|
||||||
})
|
|
||||||
}
|
|
||||||
}
|
|
@ -1,4 +0,0 @@
|
|||||||
{
|
|
||||||
"delta_steps": [ 0.05, 0.3, 0.5 ],
|
|
||||||
"data": [ -0.1, -0.5, -1.0 ]
|
|
||||||
}
|
|
@ -3,8 +3,6 @@ package throttle
|
|||||||
import (
|
import (
|
||||||
"github.com/cyrilix/robocar-base/service"
|
"github.com/cyrilix/robocar-base/service"
|
||||||
"github.com/cyrilix/robocar-protobuf/go/events"
|
"github.com/cyrilix/robocar-protobuf/go/events"
|
||||||
"github.com/cyrilix/robocar-throttle/pkg/brake"
|
|
||||||
"github.com/cyrilix/robocar-throttle/pkg/types"
|
|
||||||
mqtt "github.com/eclipse/paho.mqtt.golang"
|
mqtt "github.com/eclipse/paho.mqtt.golang"
|
||||||
"go.uber.org/zap"
|
"go.uber.org/zap"
|
||||||
"google.golang.org/protobuf/proto"
|
"google.golang.org/protobuf/proto"
|
||||||
@ -12,39 +10,25 @@ import (
|
|||||||
"time"
|
"time"
|
||||||
)
|
)
|
||||||
|
|
||||||
func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string,
|
func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic string, minValue, maxValue float32, publishPilotFrequency int) *Controller {
|
||||||
minValue, maxValue types.Throttle, publishPilotFrequency int, opts ...Option) *Controller {
|
return &Controller{
|
||||||
c := &Controller{
|
|
||||||
client: client,
|
client: client,
|
||||||
throttleTopic: throttleTopic,
|
throttleTopic: throttleTopic,
|
||||||
driveModeTopic: driveModeTopic,
|
driveModeTopic: driveModeTopic,
|
||||||
rcThrottleTopic: rcThrottleTopic,
|
rcThrottleTopic: rcThrottleTopic,
|
||||||
steeringTopic: steeringTopic,
|
steeringTopic: steeringTopic,
|
||||||
throttleFeedbackTopic: throttleFeedbackTopic,
|
|
||||||
maxThrottle: maxValue,
|
maxThrottle: maxValue,
|
||||||
driveMode: events.DriveMode_USER,
|
driveMode: events.DriveMode_USER,
|
||||||
publishPilotFrequency: publishPilotFrequency,
|
publishPilotFrequency: publishPilotFrequency,
|
||||||
steeringProcessor: &SteeringProcessor{minThrottle: minValue, maxThrottle: maxValue},
|
steeringProcessor: &SteeringProcessor{minThrottle: minValue, maxThrottle: maxValue},
|
||||||
brakeCtrl: &brake.DisabledController{},
|
|
||||||
}
|
}
|
||||||
for _, o := range opts {
|
|
||||||
o(c)
|
|
||||||
}
|
|
||||||
return c
|
|
||||||
}
|
|
||||||
|
|
||||||
type Option func(c *Controller)
|
|
||||||
|
|
||||||
func WithBrakeController(bc brake.Controller) Option {
|
|
||||||
return func(c *Controller) {
|
|
||||||
c.brakeCtrl = bc
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
type Controller struct {
|
type Controller struct {
|
||||||
client mqtt.Client
|
client mqtt.Client
|
||||||
throttleTopic string
|
throttleTopic string
|
||||||
maxThrottle types.Throttle
|
maxThrottle float32
|
||||||
steeringProcessor *SteeringProcessor
|
steeringProcessor *SteeringProcessor
|
||||||
|
|
||||||
muDriveMode sync.RWMutex
|
muDriveMode sync.RWMutex
|
||||||
@ -53,11 +37,9 @@ type Controller struct {
|
|||||||
muSteering sync.RWMutex
|
muSteering sync.RWMutex
|
||||||
steering float32
|
steering float32
|
||||||
|
|
||||||
brakeCtrl brake.Controller
|
|
||||||
|
|
||||||
cancel chan interface{}
|
cancel chan interface{}
|
||||||
publishPilotFrequency int
|
publishPilotFrequency int
|
||||||
driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string
|
driveModeTopic, rcThrottleTopic, steeringTopic string
|
||||||
}
|
}
|
||||||
|
|
||||||
func (c *Controller) Start() error {
|
func (c *Controller) Start() error {
|
||||||
@ -86,10 +68,8 @@ func (c *Controller) onPublishPilotValue() {
|
|||||||
return
|
return
|
||||||
}
|
}
|
||||||
|
|
||||||
throttleFromSteering := c.steeringProcessor.Process(c.readSteering())
|
|
||||||
|
|
||||||
throttleMsg := events.ThrottleMessage{
|
throttleMsg := events.ThrottleMessage{
|
||||||
Throttle: float32(c.brakeCtrl.AdjustThrottle(throttleFromSteering)),
|
Throttle: c.steeringProcessor.Process(c.readSteering()),
|
||||||
Confidence: 1.0,
|
Confidence: 1.0,
|
||||||
}
|
}
|
||||||
payload, err := proto.Marshal(&throttleMsg)
|
payload, err := proto.Marshal(&throttleMsg)
|
||||||
@ -110,24 +90,14 @@ func (c *Controller) readSteering() float32 {
|
|||||||
|
|
||||||
func (c *Controller) Stop() {
|
func (c *Controller) Stop() {
|
||||||
close(c.cancel)
|
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)
|
||||||
}
|
|
||||||
|
|
||||||
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.brakeCtrl.SetRealThrottle(types.Throttle(msg.GetThrottle()))
|
|
||||||
}
|
}
|
||||||
|
|
||||||
func (c *Controller) onDriveMode(_ mqtt.Client, message mqtt.Message) {
|
func (c *Controller) onDriveMode(_ mqtt.Client, message mqtt.Message) {
|
||||||
var msg events.DriveModeMessage
|
var msg events.DriveModeMessage
|
||||||
err := proto.Unmarshal(message.Payload(), &msg)
|
err := proto.Unmarshal(message.Payload(), &msg)
|
||||||
if err != nil {
|
if err != nil {
|
||||||
zap.S().Errorf("unable to unmarshal protobuf %T message: %v", &msg, err)
|
zap.S().Errorf("unable to unmarshal protobuf %T message: %v", msg, err)
|
||||||
return
|
return
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -149,9 +119,9 @@ 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 {
|
if throttleMsg.GetThrottle() > c.maxThrottle {
|
||||||
zap.S().Debugf("throttle upper that max value allowed, patch value from %v to %v", 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)
|
throttleMsg.Throttle = c.maxThrottle
|
||||||
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)
|
||||||
@ -192,10 +162,6 @@ var registerCallbacks = func(p *Controller) error {
|
|||||||
if err != nil {
|
if err != nil {
|
||||||
return err
|
return err
|
||||||
}
|
}
|
||||||
err = service.RegisterCallback(p.client, p.throttleFeedbackTopic, p.onThrottleFeedback)
|
|
||||||
if err != nil {
|
|
||||||
return err
|
|
||||||
}
|
|
||||||
return nil
|
return nil
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -3,8 +3,6 @@ package throttle
|
|||||||
import (
|
import (
|
||||||
"github.com/cyrilix/robocar-base/testtools"
|
"github.com/cyrilix/robocar-base/testtools"
|
||||||
"github.com/cyrilix/robocar-protobuf/go/events"
|
"github.com/cyrilix/robocar-protobuf/go/events"
|
||||||
"github.com/cyrilix/robocar-throttle/pkg/brake"
|
|
||||||
"github.com/cyrilix/robocar-throttle/pkg/types"
|
|
||||||
mqtt "github.com/eclipse/paho.mqtt.golang"
|
mqtt "github.com/eclipse/paho.mqtt.golang"
|
||||||
"google.golang.org/protobuf/proto"
|
"google.golang.org/protobuf/proto"
|
||||||
"sync"
|
"sync"
|
||||||
@ -35,15 +33,14 @@ func TestDefaultThrottle(t *testing.T) {
|
|||||||
driveModeTopic := "topic/driveMode"
|
driveModeTopic := "topic/driveMode"
|
||||||
rcThrottleTopic := "topic/rcThrottle"
|
rcThrottleTopic := "topic/rcThrottle"
|
||||||
steeringTopic := "topic/rcThrottle"
|
steeringTopic := "topic/rcThrottle"
|
||||||
throttleFeedbackTopic := "topic/feedback/throttle"
|
|
||||||
|
|
||||||
minValue := types.Throttle(0.56)
|
minValue := float32(0.56)
|
||||||
|
|
||||||
p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, minValue, 1., 200)
|
p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, minValue, 1., 200)
|
||||||
|
|
||||||
cases := []*struct {
|
cases := []struct {
|
||||||
name string
|
name string
|
||||||
maxThrottle types.Throttle
|
maxThrottle float32
|
||||||
driveMode events.DriveModeMessage
|
driveMode events.DriveModeMessage
|
||||||
rcThrottle events.ThrottleMessage
|
rcThrottle events.ThrottleMessage
|
||||||
expectedThrottle events.ThrottleMessage
|
expectedThrottle events.ThrottleMessage
|
||||||
@ -116,19 +113,16 @@ func TestController_Start(t *testing.T) {
|
|||||||
steeringTopic := "topic/steering"
|
steeringTopic := "topic/steering"
|
||||||
driveModeTopic := "topic/driveMode"
|
driveModeTopic := "topic/driveMode"
|
||||||
rcThrottleTopic := "topic/rcThrottle"
|
rcThrottleTopic := "topic/rcThrottle"
|
||||||
throttleFeedbackTopic := "topic/feedback/throttle"
|
|
||||||
|
|
||||||
type fields struct {
|
type fields struct {
|
||||||
driveMode events.DriveMode
|
driveMode events.DriveMode
|
||||||
min, max types.Throttle
|
min, max float32
|
||||||
publishPilotFrequency int
|
publishPilotFrequency int
|
||||||
brakeCtl brake.Controller
|
|
||||||
}
|
}
|
||||||
type msgEvents struct {
|
type msgEvents struct {
|
||||||
driveMode *events.DriveModeMessage
|
driveMode *events.DriveModeMessage
|
||||||
steering *events.SteeringMessage
|
steering *events.SteeringMessage
|
||||||
rcThrottle *events.ThrottleMessage
|
rcThrottle *events.ThrottleMessage
|
||||||
throttleFeedback *events.ThrottleMessage
|
|
||||||
}
|
}
|
||||||
|
|
||||||
tests := []struct {
|
tests := []struct {
|
||||||
@ -145,13 +139,11 @@ func TestController_Start(t *testing.T) {
|
|||||||
max: 0.8,
|
max: 0.8,
|
||||||
min: 0.3,
|
min: 0.3,
|
||||||
publishPilotFrequency: publishPilotFrequency,
|
publishPilotFrequency: publishPilotFrequency,
|
||||||
brakeCtl: &brake.DisabledController{},
|
|
||||||
},
|
},
|
||||||
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.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},
|
||||||
},
|
},
|
||||||
@ -162,13 +154,11 @@ func TestController_Start(t *testing.T) {
|
|||||||
max: 0.8,
|
max: 0.8,
|
||||||
min: 0.3,
|
min: 0.3,
|
||||||
publishPilotFrequency: publishPilotFrequency,
|
publishPilotFrequency: publishPilotFrequency,
|
||||||
brakeCtl: &brake.DisabledController{},
|
|
||||||
},
|
},
|
||||||
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.9, 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},
|
want: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
|
||||||
},
|
},
|
||||||
@ -179,13 +169,11 @@ func TestController_Start(t *testing.T) {
|
|||||||
max: 0.8,
|
max: 0.8,
|
||||||
min: 0.3,
|
min: 0.3,
|
||||||
publishPilotFrequency: publishPilotFrequency,
|
publishPilotFrequency: publishPilotFrequency,
|
||||||
brakeCtl: &brake.DisabledController{},
|
|
||||||
},
|
},
|
||||||
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.1, 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},
|
want: &events.ThrottleMessage{Throttle: 0.1, Confidence: 1.0},
|
||||||
},
|
},
|
||||||
@ -196,13 +184,11 @@ func TestController_Start(t *testing.T) {
|
|||||||
max: 0.8,
|
max: 0.8,
|
||||||
min: 0.3,
|
min: 0.3,
|
||||||
publishPilotFrequency: publishPilotFrequency,
|
publishPilotFrequency: publishPilotFrequency,
|
||||||
brakeCtl: &brake.DisabledController{},
|
|
||||||
},
|
},
|
||||||
msgEvents: msgEvents{
|
msgEvents: msgEvents{
|
||||||
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
|
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
|
||||||
steering: &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
|
steering: &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
|
||||||
rcThrottle: &events.ThrottleMessage{Throttle: 0.5, 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},
|
want: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
|
||||||
},
|
},
|
||||||
@ -213,13 +199,11 @@ func TestController_Start(t *testing.T) {
|
|||||||
max: 0.8,
|
max: 0.8,
|
||||||
min: 0.3,
|
min: 0.3,
|
||||||
publishPilotFrequency: publishPilotFrequency,
|
publishPilotFrequency: publishPilotFrequency,
|
||||||
brakeCtl: &brake.DisabledController{},
|
|
||||||
},
|
},
|
||||||
msgEvents: msgEvents{
|
msgEvents: msgEvents{
|
||||||
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
|
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
|
||||||
steering: &events.SteeringMessage{Steering: -1.0, Confidence: 1.0},
|
steering: &events.SteeringMessage{Steering: -1.0, Confidence: 1.0},
|
||||||
rcThrottle: &events.ThrottleMessage{Throttle: 0.3, 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},
|
want: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
|
||||||
},
|
},
|
||||||
@ -230,42 +214,22 @@ func TestController_Start(t *testing.T) {
|
|||||||
max: 0.8,
|
max: 0.8,
|
||||||
min: 0.3,
|
min: 0.3,
|
||||||
publishPilotFrequency: publishPilotFrequency,
|
publishPilotFrequency: publishPilotFrequency,
|
||||||
brakeCtl: &brake.DisabledController{},
|
|
||||||
},
|
},
|
||||||
msgEvents: msgEvents{
|
msgEvents: msgEvents{
|
||||||
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
|
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
|
||||||
steering: &events.SteeringMessage{Steering: 1.0, Confidence: 1.0},
|
steering: &events.SteeringMessage{Steering: 1.0, Confidence: 1.0},
|
||||||
rcThrottle: &events.ThrottleMessage{Throttle: 0.3, 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},
|
want: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
|
||||||
},
|
},
|
||||||
{
|
|
||||||
name: "On pilot drive mode, should brake on brutal change",
|
|
||||||
fields: fields{
|
|
||||||
driveMode: events.DriveMode_PILOT,
|
|
||||||
max: 1.0,
|
|
||||||
min: 0.3,
|
|
||||||
publishPilotFrequency: publishPilotFrequency,
|
|
||||||
brakeCtl: brake.NewCustomController(),
|
|
||||||
},
|
|
||||||
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},
|
|
||||||
throttleFeedback: &events.ThrottleMessage{Throttle: 1.0, Confidence: 1.0},
|
|
||||||
},
|
|
||||||
want: &events.ThrottleMessage{Throttle: -1.0, Confidence: 1.0},
|
|
||||||
},
|
|
||||||
}
|
}
|
||||||
|
|
||||||
for _, tt := range tests {
|
for _, tt := range tests {
|
||||||
t.Run(tt.name, func(t *testing.T) {
|
t.Run(tt.name, func(t *testing.T) {
|
||||||
c := New(nil,
|
c := New(nil,
|
||||||
throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
|
throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic,
|
||||||
tt.fields.min, tt.fields.max,
|
tt.fields.min, tt.fields.max,
|
||||||
tt.fields.publishPilotFrequency,
|
tt.fields.publishPilotFrequency,
|
||||||
WithBrakeController(tt.fields.brakeCtl),
|
|
||||||
)
|
)
|
||||||
|
|
||||||
go c.Start()
|
go c.Start()
|
||||||
@ -277,7 +241,6 @@ func TestController_Start(t *testing.T) {
|
|||||||
c.onDriveMode(nil, testtools.NewFakeMessageFromProtobuf(driveModeTopic, tt.msgEvents.driveMode))
|
c.onDriveMode(nil, testtools.NewFakeMessageFromProtobuf(driveModeTopic, tt.msgEvents.driveMode))
|
||||||
c.onRCThrottle(nil, testtools.NewFakeMessageFromProtobuf(rcThrottleTopic, tt.msgEvents.rcThrottle))
|
c.onRCThrottle(nil, testtools.NewFakeMessageFromProtobuf(rcThrottleTopic, tt.msgEvents.rcThrottle))
|
||||||
c.onSteering(nil, testtools.NewFakeMessageFromProtobuf(steeringTopic, tt.msgEvents.steering))
|
c.onSteering(nil, testtools.NewFakeMessageFromProtobuf(steeringTopic, tt.msgEvents.steering))
|
||||||
c.onThrottleFeedback(nil, testtools.NewFakeMessageFromProtobuf(throttleFeedbackTopic, tt.msgEvents.throttleFeedback))
|
|
||||||
waitPublish.Wait()
|
waitPublish.Wait()
|
||||||
|
|
||||||
var msg events.ThrottleMessage
|
var msg events.ThrottleMessage
|
||||||
|
@ -1,16 +1,13 @@
|
|||||||
package throttle
|
package throttle
|
||||||
|
|
||||||
import (
|
import "math"
|
||||||
"github.com/cyrilix/robocar-throttle/pkg/types"
|
|
||||||
"math"
|
|
||||||
)
|
|
||||||
|
|
||||||
type SteeringProcessor struct {
|
type SteeringProcessor struct {
|
||||||
minThrottle, maxThrottle types.Throttle
|
minThrottle, maxThrottle float32
|
||||||
}
|
}
|
||||||
|
|
||||||
// Process compute throttle from steering value
|
// Process compute throttle from steering value
|
||||||
func (sp *SteeringProcessor) Process(steering float32) types.Throttle {
|
func (sp *SteeringProcessor) Process(steering float32) float32 {
|
||||||
absSteering := math.Abs(float64(steering))
|
absSteering := math.Abs(float64(steering))
|
||||||
return sp.minThrottle + types.Throttle(float64(sp.maxThrottle-sp.minThrottle)*(1-absSteering))
|
return sp.minThrottle + float32(float64(sp.maxThrottle-sp.minThrottle)*(1-absSteering))
|
||||||
}
|
}
|
||||||
|
@ -1,14 +1,11 @@
|
|||||||
package throttle
|
package throttle
|
||||||
|
|
||||||
import (
|
import "testing"
|
||||||
"github.com/cyrilix/robocar-throttle/pkg/types"
|
|
||||||
"testing"
|
|
||||||
)
|
|
||||||
|
|
||||||
func TestSteeringProcessor_Process(t *testing.T) {
|
func TestSteeringProcessor_Process(t *testing.T) {
|
||||||
type fields struct {
|
type fields struct {
|
||||||
minThrottle types.Throttle
|
minThrottle float32
|
||||||
maxThrottle types.Throttle
|
maxThrottle float32
|
||||||
}
|
}
|
||||||
type args struct {
|
type args struct {
|
||||||
steering float32
|
steering float32
|
||||||
@ -17,7 +14,7 @@ func TestSteeringProcessor_Process(t *testing.T) {
|
|||||||
name string
|
name string
|
||||||
fields fields
|
fields fields
|
||||||
args args
|
args args
|
||||||
want types.Throttle
|
want float32
|
||||||
}{
|
}{
|
||||||
{
|
{
|
||||||
name: "steering straight",
|
name: "steering straight",
|
||||||
|
@ -1,3 +0,0 @@
|
|||||||
package types
|
|
||||||
|
|
||||||
type Throttle float32
|
|
Loading…
Reference in New Issue
Block a user