feat(dsiplay): add option to display throttle feedback

README.md

@@ -65,3 +65,10 @@ Usage of record:
   -record-json-path string
         Path where to write json files, use RECORD_JSON_PATH if args not set
 ```
+
+## Useful
+
+Debug record:
+
+    go run ./cmd/rc-tools display record -mqtt-broker tcp://diabolo.local:1883 -mqtt-username satanas -mqtt-password satanas -mqtt-client-id display-record -mqtt-topic-records car/satanas/part/records
+

cmd/rc-tools/rc-tools.go

@@ -28,8 +28,8 @@ func main() {
 	var mqttBroker, username, password, clientId string
 	var framePath string
 	var fps int
-	var frameTopic, objectsTopic, roadTopic, recordTopic string
+	var frameTopic, objectsTopic, roadTopic, recordTopic, throttleFeedbackTopic string
-	var withObjects, withRoad bool
+	var withObjects, withRoad, withThrottleFeedback bool
 	var recordsPath string
 	var trainArchiveName string
 	var trainSliceSize int
@@ -55,7 +55,6 @@ func main() {
 	cli.SetDefaultValueFromEnv(&ociImage, "TRAIN_OCI_IMAGE", "")
 	cli.SetDefaultValueFromEnv(&bucket, "TRAIN_BUCKET", "")
 
-
 	flag.BoolVar(&debug, "debug", false, "Display debug logs")
 
 	displayFlags := flag.NewFlagSet("display", flag.ExitOnError)
@@ -82,18 +81,19 @@ func main() {
 	displayCameraFlags.StringVar(&roadTopic, "mqtt-topic-road", os.Getenv("MQTT_TOPIC_ROAD"), "Mqtt topic that contains road description, use MQTT_TOPIC_ROAD if args not set")
 	displayCameraFlags.BoolVar(&withRoad, "with-road", false, "Display detected road")
 
+	displayCameraFlags.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")
+	displayCameraFlags.BoolVar(&withThrottleFeedback, "with-throttle-feedback", false, "Display throttle feedback")
+
 	recordFlags := flag.NewFlagSet("record", flag.ExitOnError)
 	cli.InitMqttFlagSet(recordFlags, DefaultClientId, &mqttBroker, &username, &password, &clientId, &mqttQos, &mqttRetain)
 	recordFlags.StringVar(&recordTopic, "mqtt-topic-records", os.Getenv("MQTT_TOPIC_RECORDS"), "Mqtt topic that contains record data for training, use MQTT_TOPIC_RECORDS if args not set")
 	recordFlags.StringVar(&recordsPath, "record-path", os.Getenv("RECORD_PATH"), "Path where to write records files, use RECORD_PATH if args not set")
 
-
 	var basedir, destdir string
 	impdkFlags := flag.NewFlagSet("import-donkey-records", flag.ExitOnError)
 	impdkFlags.StringVar(&basedir, "from", "", "source directory")
 	impdkFlags.StringVar(&destdir, "to", "", "destination directory")
 
-
 	trainingFlags := flag.NewFlagSet("training", flag.ExitOnError)
 	trainingFlags.Usage = func() {
 		fmt.Printf("Usage of %s %s:\n", os.Args[0], trainingFlags.Name())
@@ -133,7 +133,6 @@ func main() {
 	trainArchiveFlags.IntVar(&horizon, "horizon", 0, "Upper zone image to crop (in pixels)")
 	trainArchiveFlags.BoolVar(&withFlipImage, "with-flip-image", withFlipImage, "Flip horiontal image and reverse steering to increase data into training archive")
 
-
 	modelsFlags := flag.NewFlagSet("models", flag.ExitOnError)
 	modelsFlags.Usage = func() {
 		fmt.Printf("Usage of %s %s:\n", os.Args[0], modelsFlags.Name())
@@ -199,7 +198,7 @@ func main() {
 				zap.S().Fatalf("unable to connect to mqtt bus: %v", err)
 			}
 			defer client.Disconnect(50)
-			runDisplay(client, framePath, frameTopic, fps, objectsTopic, roadTopic, withObjects, withRoad)
+			runDisplay(client, framePath, frameTopic, fps, objectsTopic, roadTopic, throttleFeedbackTopic, withObjects, withRoad, withThrottleFeedback)
 		default:
 			displayFlags.PrintDefaults()
 			os.Exit(0)
@@ -249,7 +248,6 @@ func main() {
 			trainingFlags.PrintDefaults()
 			os.Exit(0)
 
-
 		}
 	case modelsFlags.Name():
 
@@ -330,7 +328,8 @@ func runDisplayRecord(client mqtt.Client, recordTopic string){
 		zap.S().Fatalf("unable to start service: %v", err)
 	}
 }
-func runDisplay(client mqtt.Client, framePath string, frameTopic string, fps int, objectsTopic string, roadTopic string, withObjects bool, withRoad bool) {
+func runDisplay(client mqtt.Client, framePath string, frameTopic string, fps int, objectsTopic, roadTopic, throttleFeedbackTopic string,
+	withObjects, withRoad, withThrottleFeedback bool) {
 
 	if framePath != "" {
 		camera, err := video.NewCameraFake(client, frameTopic, framePath, fps)
@@ -344,8 +343,8 @@ func runDisplay(client mqtt.Client, framePath string, frameTopic string, fps int
 	}
 
 	p := part.NewPart(client, frameTopic,
-		objectsTopic, roadTopic,
+		objectsTopic, roadTopic, throttleFeedbackTopic,
-		withObjects, withRoad)
+		withObjects, withRoad, withThrottleFeedback)
 	defer p.Stop()
 
 	cli.HandleExit(p)

go.mod

@@ -8,7 +8,7 @@ require (
 	github.com/aws/aws-sdk-go-v2/service/s3 v1.26.11
 	github.com/aws/aws-sdk-go-v2/service/sagemaker v1.32.1
 	github.com/cyrilix/robocar-base v0.1.7
-	github.com/cyrilix/robocar-protobuf/go v1.0.5
+	github.com/cyrilix/robocar-protobuf/go v1.1.0
 	github.com/disintegration/imaging v1.6.2
 	github.com/eclipse/paho.mqtt.golang v1.4.1
 	github.com/golang/protobuf v1.5.2

go.sum

@@ -38,8 +38,8 @@ github.com/benbjohnson/clock v1.1.0 h1:Q92kusRqC1XV2MjkWETPvjJVqKetz1OzxZB7mHJLj
 github.com/benbjohnson/clock v1.1.0/go.mod h1:J11/hYXuz8f4ySSvYwY0FKfm+ezbsZBKZxNJlLklBHA=
 github.com/cyrilix/robocar-base v0.1.7 h1:EVzZ0KjigSFpke5f3A/PybEH3WFUEIrYSc3z/dhOZ48=
 github.com/cyrilix/robocar-base v0.1.7/go.mod h1:4E11HQSNy2NT8e7MW188y6ST9C0RzarKyn7sK/3V/Lk=
-github.com/cyrilix/robocar-protobuf/go v1.0.5 h1:PX1At+pf6G7gJwT4LzJLQu3/LPFTTNNlZmZSYtnSELY=
+github.com/cyrilix/robocar-protobuf/go v1.1.0 h1:txIjGnnCF3UzedpsWu+sL7nMA+pNjSnX6HZlAmuReH4=
-github.com/cyrilix/robocar-protobuf/go v1.0.5/go.mod h1:Y3AE28K5V7EZxMXp/6A8RhkRz15VOfFy4CjST35FbtQ=
+github.com/cyrilix/robocar-protobuf/go v1.1.0/go.mod h1:Y3AE28K5V7EZxMXp/6A8RhkRz15VOfFy4CjST35FbtQ=
 github.com/davecgh/go-spew v1.1.0/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSsI+c5H38=
 github.com/davecgh/go-spew v1.1.1 h1:vj9j/u1bqnvCEfJOwUhtlOARqs3+rkHYY13jYWTU97c=
 github.com/davecgh/go-spew v1.1.1/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSsI+c5H38=

part/part.go

@@ -11,20 +11,25 @@ import (
 	"image"
 	"image/color"
 	"log"
+	"time"
 )
 
-func NewPart(client mqtt.Client, frameTopic, objectsTopic, roadTopic string, withObjects, withRoad bool) *FramePart {
+func NewPart(client mqtt.Client, frameTopic, objectsTopic, roadTopic, throttleFeedbackTopic string,
+	withObjects, withRoad, withThrottleFeedback bool) *FramePart {
 	return &FramePart{
 		client:                client,
 		frameTopic:            frameTopic,
 		objectsTopic:          objectsTopic,
 		roadTopic:             roadTopic,
+		throttleFeedbackTopic: throttleFeedbackTopic,
 		window:                gocv.NewWindow("frameTopic"),
 		withObjects:           withObjects,
 		withRoad:              withRoad,
+		withThrottleFeedback:  withThrottleFeedback,
 		imgChan:               make(chan gocv.Mat),
 		objectsChan:           make(chan events.ObjectsMessage),
 		roadChan:              make(chan events.RoadMessage),
+		throttleFeedbackChan:  make(chan events.ThrottleMessage),
 		cancel:                make(chan interface{}),
 	}
 
@@ -32,15 +37,17 @@ func NewPart(client mqtt.Client, frameTopic, objectsTopic, roadTopic string, wit
 
 type FramePart struct {
 	client                                                     mqtt.Client
-	frameTopic, objectsTopic, roadTopic string
+	frameTopic, objectsTopic, roadTopic, throttleFeedbackTopic string
 
 	window               *gocv.Window
 	withObjects          bool
 	withRoad             bool
+	withThrottleFeedback bool
 
 	imgChan              chan gocv.Mat
 	objectsChan          chan events.ObjectsMessage
 	roadChan             chan events.RoadMessage
+	throttleFeedbackChan chan events.ThrottleMessage
 	cancel               chan interface{}
 }
 
@@ -52,9 +59,13 @@ func (p *FramePart) Start() error {
 	var img = gocv.NewMat()
 	var objectsMsg events.ObjectsMessage
 	var roadMsg events.RoadMessage
+	var throttleFeedbackMsg events.ThrottleMessage
 
+	ticker := time.NewTicker(1 * time.Second)
 	for {
 		select {
+		case <-ticker.C:
+			img = gocv.NewMatWithSize(120, 120, gocv.MatTypeCV8S)
 		case newImg := <-p.imgChan:
 			img.Close()
 			img = newImg
@@ -62,11 +73,14 @@ func (p *FramePart) Start() error {
 			objectsMsg = objects
 		case road := <-p.roadChan:
 			roadMsg = road
+		case throttleFeedback := <-p.throttleFeedbackChan:
+			throttleFeedbackMsg = throttleFeedback
 		case <-p.cancel:
 			img.Close()
 			return nil
 		}
-		p.drawFrame(&img, &objectsMsg, &roadMsg)
+		p.drawFrame(&img, &objectsMsg, &roadMsg, &throttleFeedbackMsg)
+		ticker.Reset(1 * time.Second)
 	}
 }
 
@@ -105,6 +119,7 @@ func (p *FramePart) onObjects(_ mqtt.Client, message mqtt.Message) {
 		return
 	}
 
+	zap.S().Infow("new objects", zap.String("topic", message.Topic()), zap.String("object", msg.Objects[0].String()))
 	p.objectsChan <- msg
 }
 
@@ -120,6 +135,18 @@ func (p *FramePart) onRoad(_ mqtt.Client, message mqtt.Message) {
 	p.roadChan <- msg
 }
 
+func (p *FramePart) 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 msg %T: %v", msg, err)
+		return
+	}
+
+	p.throttleFeedbackChan <- msg
+}
+
 func (p *FramePart) registerCallbacks() error {
 	err := RegisterCallback(p.client, p.frameTopic, p.onFrame)
 	if err != nil {
@@ -139,10 +166,16 @@ func (p *FramePart) registerCallbacks() error {
 			return err
 		}
 	}
+	if p.withThrottleFeedback {
+		err := service.RegisterCallback(p.client, p.throttleFeedbackTopic, p.onThrottleFeedback)
+		if err != nil {
+			return err
+		}
+	}
 	return nil
 }
 
-func (p *FramePart) drawFrame(img *gocv.Mat, objects *events.ObjectsMessage, road *events.RoadMessage) {
+func (p *FramePart) drawFrame(img *gocv.Mat, objects *events.ObjectsMessage, road *events.RoadMessage, tf *events.ThrottleMessage) {
 
 	if p.withObjects {
 		p.drawObjects(img, objects)
@@ -150,17 +183,26 @@ func (p *FramePart) drawFrame(img *gocv.Mat, objects *events.ObjectsMessage, roa
 	if p.withRoad {
 		p.drawRoad(img, road)
 	}
+	if p.withThrottleFeedback {
+		p.drawThrottleFeedbackText(img, tf)
+	}
 
 	p.window.IMShow(*img)
 	p.window.WaitKey(1)
 }
 
 func (p *FramePart) drawObjects(img *gocv.Mat, objects *events.ObjectsMessage) {
+	zap.S().Debugf("draw object %v", objects)
 	for _, obj := range objects.GetObjects() {
 		gocv.Rectangle(
 			img,
-			image.Rect(int(obj.GetLeft()), int(obj.GetTop()), int(obj.GetRight()), int(obj.GetBottom())),
+			image.Rect(
-			color.RGBA{0, 255, 0, 0},
+				int(obj.GetLeft()*float32(img.Cols())),
+				int(obj.GetTop()*float32(img.Rows())),
+				int(obj.GetRight()*float32(img.Cols())),
+				int(obj.GetBottom()*float32(img.Rows())),
+			),
+			color.RGBA{R: 0, G: 255, B: 0, A: 0},
 			2)
 	}
 }
@@ -205,6 +247,18 @@ func (p *FramePart) drawRoadText(img *gocv.Mat, road *events.RoadMessage) {
 	)
 }
 
+func (p *FramePart) drawThrottleFeedbackText(img *gocv.Mat, tf *events.ThrottleMessage) {
+	gocv.PutText(
+		img,
+		fmt.Sprintf("Throttle feedback: %.3f", tf.Throttle),
+		image.Point{X: 5, Y: 20},
+		gocv.FontHersheyPlain,
+		0.6,
+		color.RGBA{R: 0, G: 255, B: 255, A: 255},
+		1,
+	)
+}
+
 func StopService(name string, client mqtt.Client, topics ...string) {
 	zap.S().Infof("Stop %s service", name)
 	token := client.Unsubscribe(topics...)

pkg/display/record.go

@@ -96,7 +96,6 @@ func (r *Record) drawRecord(rec *events.RecordMessage, objects *events.ObjectsMe
 	r.window.WaitKey(1)
 }
 
-
 func (r *Record) drawSteering(img *gocv.Mat, steering float32) {
 	gocv.PutText(
 		img,

pkg/train/config.go

@@ -3,4 +3,3 @@ package train
 const (
 	prefixInput = "input/data/train/"
 )
-

run_training.sh

@@ -6,8 +6,8 @@ set +x
 RECORDS_PATH=~/robocar/record-sim4-2
 #TRAINING_OPTS="--horizon=20"
 TRAINING_OPTS=""
-#MODEL_TYPE="categorical"
+MODEL_TYPE="categorical"
-MODEL_TYPE="linear"
+#MODEL_TYPE="linear"
 IMG_WIDTH=160
 IMG_HEIGHT=120
 HORIZON=20

vendor/github.com/cyrilix/robocar-protobuf/go/events/events.pb.go

@@ -1,7 +1,7 @@
 // Code generated by protoc-gen-go. DO NOT EDIT.
 // versions:
-// 	protoc-gen-go v1.27.1
+// 	protoc-gen-go v1.28.1
-// 	protoc        v3.12.4
+// 	protoc        v3.21.4
 // source: events/events.proto
 
 package events
@@ -468,17 +468,17 @@ func (x *ObjectsMessage) GetFrameRef() *FrameRef {
 	return nil
 }
 
-// BoundingBox that contains an object
+// BoundingBox that contains an object, coordinates as percent
 type Object struct {
 	state         protoimpl.MessageState
 	sizeCache     protoimpl.SizeCache
 	unknownFields protoimpl.UnknownFields
 
 	Type       TypeObject `protobuf:"varint,1,opt,name=type,proto3,enum=robocar.events.TypeObject" json:"type,omitempty"`
-	Left       int32      `protobuf:"varint,2,opt,name=left,proto3" json:"left,omitempty"`
+	Left       float32    `protobuf:"fixed32,2,opt,name=left,proto3" json:"left,omitempty"`
-	Top        int32      `protobuf:"varint,3,opt,name=top,proto3" json:"top,omitempty"`
+	Top        float32    `protobuf:"fixed32,3,opt,name=top,proto3" json:"top,omitempty"`
-	Right      int32      `protobuf:"varint,4,opt,name=right,proto3" json:"right,omitempty"`
+	Right      float32    `protobuf:"fixed32,4,opt,name=right,proto3" json:"right,omitempty"`
-	Bottom     int32      `protobuf:"varint,5,opt,name=bottom,proto3" json:"bottom,omitempty"`
+	Bottom     float32    `protobuf:"fixed32,5,opt,name=bottom,proto3" json:"bottom,omitempty"`
 	Confidence float32    `protobuf:"fixed32,6,opt,name=confidence,proto3" json:"confidence,omitempty"`
 }
 
@@ -521,28 +521,28 @@ func (x *Object) GetType() TypeObject {
 	return TypeObject_ANY
 }
 
-func (x *Object) GetLeft() int32 {
+func (x *Object) GetLeft() float32 {
 	if x != nil {
 		return x.Left
 	}
 	return 0
 }
 
-func (x *Object) GetTop() int32 {
+func (x *Object) GetTop() float32 {
 	if x != nil {
 		return x.Top
 	}
 	return 0
 }
 
-func (x *Object) GetRight() int32 {
+func (x *Object) GetRight() float32 {
 	if x != nil {
 		return x.Right
 	}
 	return 0
 }
 
-func (x *Object) GetBottom() int32 {
+func (x *Object) GetBottom() float32 {
 	if x != nil {
 		return x.Bottom
 	}
@@ -918,10 +918,10 @@ var file_events_events_proto_rawDesc = []byte{
 	0x0e, 0x32, 0x1a, 0x2e, 0x72, 0x6f, 0x62, 0x6f, 0x63, 0x61, 0x72, 0x2e, 0x65, 0x76, 0x65, 0x6e,
 	0x74, 0x73, 0x2e, 0x54, 0x79, 0x70, 0x65, 0x4f, 0x62, 0x6a, 0x65, 0x63, 0x74, 0x52, 0x04, 0x74,
 	0x79, 0x70, 0x65, 0x12, 0x12, 0x0a, 0x04, 0x6c, 0x65, 0x66, 0x74, 0x18, 0x02, 0x20, 0x01, 0x28,
-	0x05, 0x52, 0x04, 0x6c, 0x65, 0x66, 0x74, 0x12, 0x10, 0x0a, 0x03, 0x74, 0x6f, 0x70, 0x18, 0x03,
+	0x02, 0x52, 0x04, 0x6c, 0x65, 0x66, 0x74, 0x12, 0x10, 0x0a, 0x03, 0x74, 0x6f, 0x70, 0x18, 0x03,
-	0x20, 0x01, 0x28, 0x05, 0x52, 0x03, 0x74, 0x6f, 0x70, 0x12, 0x14, 0x0a, 0x05, 0x72, 0x69, 0x67,
+	0x20, 0x01, 0x28, 0x02, 0x52, 0x03, 0x74, 0x6f, 0x70, 0x12, 0x14, 0x0a, 0x05, 0x72, 0x69, 0x67,
-	0x68, 0x74, 0x18, 0x04, 0x20, 0x01, 0x28, 0x05, 0x52, 0x05, 0x72, 0x69, 0x67, 0x68, 0x74, 0x12,
+	0x68, 0x74, 0x18, 0x04, 0x20, 0x01, 0x28, 0x02, 0x52, 0x05, 0x72, 0x69, 0x67, 0x68, 0x74, 0x12,
-	0x16, 0x0a, 0x06, 0x62, 0x6f, 0x74, 0x74, 0x6f, 0x6d, 0x18, 0x05, 0x20, 0x01, 0x28, 0x05, 0x52,
+	0x16, 0x0a, 0x06, 0x62, 0x6f, 0x74, 0x74, 0x6f, 0x6d, 0x18, 0x05, 0x20, 0x01, 0x28, 0x02, 0x52,
 	0x06, 0x62, 0x6f, 0x74, 0x74, 0x6f, 0x6d, 0x12, 0x1e, 0x0a, 0x0a, 0x63, 0x6f, 0x6e, 0x66, 0x69,
 	0x64, 0x65, 0x6e, 0x63, 0x65, 0x18, 0x06, 0x20, 0x01, 0x28, 0x02, 0x52, 0x0a, 0x63, 0x6f, 0x6e,
 	0x66, 0x69, 0x64, 0x65, 0x6e, 0x63, 0x65, 0x22, 0x2f, 0x0a, 0x13, 0x53, 0x77, 0x69, 0x74, 0x63,

vendor/modules.txt

@@ -111,7 +111,7 @@ github.com/aws/smithy-go/waiter
 ## explicit; go 1.18
 github.com/cyrilix/robocar-base/cli
 github.com/cyrilix/robocar-base/service
-# github.com/cyrilix/robocar-protobuf/go v1.0.5
+# github.com/cyrilix/robocar-protobuf/go v1.1.0
 ## explicit; go 1.18
 github.com/cyrilix/robocar-protobuf/go/events
 # github.com/disintegration/imaging v1.6.2