Display road

README.md

@@ -9,7 +9,7 @@ Tool to display camera frame and metrics
   -mqtt-broker string
         Broker Uri, use MQTT_BROKER env if arg not set (default "tcp://127.0.0.1:1883")
   -mqtt-client-id string
-        Mqtt client id, use MQTT_CLIENT_ID env if args not set (default "robocar-frame-display")
+        Mqtt client id, use MQTT_CLIENT_ID env if args not set (default "robocar-display")
   -mqtt-password string
         Broker Password, MQTT_PASSWORD env if args not set
   -mqtt-qos int
@@ -18,7 +18,15 @@ Tool to display camera frame and metrics
         Retain mqtt message, if not set, true if MQTT_RETAIN env variable is set
   -mqtt-topic-frame string
         Mqtt topic that contains frame to display, use MQTT_TOPIC_FRAME if args not set
+  -mqtt-topic-objects string
+        Mqtt topic that contains detected objects, use MQTT_TOPIC_OBJECTS if args not set
+  -mqtt-topic-road string
+        Mqtt topic that contains road description, use MQTT_TOPIC_ROAD if args not set
   -mqtt-username string
         Broker Username, use MQTT_USERNAME env if arg not set
+  -with-objects
+        Display detected objects
+  -with-road
+        Display detected road
 ```
 

cmd/rc-display/rc-display.go

@@ -14,8 +14,8 @@ const (
 
 func main() {
 	var mqttBroker, username, password, clientId string
-	var frameTopic, objectsTopic string
+	var frameTopic, objectsTopic, roadTopic string
-	var withObjects bool
+	var withObjects, withRoad bool
 
 	mqttQos := cli.InitIntFlag("MQTT_QOS", 0)
 	_, mqttRetain := os.LookupEnv("MQTT_RETAIN")
@@ -23,9 +23,13 @@ func main() {
 	cli.InitMqttFlags(DefaultClientId, &mqttBroker, &username, &password, &clientId, &mqttQos, &mqttRetain)
 
 	flag.StringVar(&frameTopic, "mqtt-topic-frame", os.Getenv("MQTT_TOPIC_FRAME"), "Mqtt topic that contains frame to display, use MQTT_TOPIC_FRAME if args not set")
+
 	flag.StringVar(&objectsTopic, "mqtt-topic-objects", os.Getenv("MQTT_TOPIC_OBJECTS"), "Mqtt topic that contains detected objects, use MQTT_TOPIC_OBJECTS if args not set")
 	flag.BoolVar(&withObjects, "with-objects", false, "Display detected objects")
 
+	flag.StringVar(&roadTopic, "mqtt-topic-road", os.Getenv("MQTT_TOPIC_ROAD"), "Mqtt topic that contains road description, use MQTT_TOPIC_ROAD if args not set")
+	flag.BoolVar(&withRoad, "with-road", false, "Display detected road")
+
 	flag.Parse()
 	if len(os.Args) <= 1 {
 		flag.PrintDefaults()
@@ -38,7 +42,9 @@ func main() {
 	}
 	defer client.Disconnect(50)
 
-	p := part.NewPart(client, frameTopic, objectsTopic, withObjects)
+	p := part.NewPart(client, frameTopic,
+		objectsTopic, roadTopic,
+		withObjects, withRoad )
 	defer p.Stop()
 
 	cli.HandleExit(p)

part/part.go

@@ -12,15 +12,18 @@ import (
 	"image/color"
 )
 
-func NewPart(client mqtt.Client, frameTopic, objectsTopic string, withObjects bool) *FramePart {
+func NewPart(client mqtt.Client, frameTopic, objectsTopic, roadTopic string, withObjects, withRoad bool) *FramePart {
 	return &FramePart{
 		client:       client,
 		frameTopic:   frameTopic,
 		objectsTopic: objectsTopic,
+		roadTopic:    roadTopic,
 		window:       gocv.NewWindow(frameTopic),
 		withObjects:  withObjects,
+		withRoad:     withRoad,
 		imgChan:      make(chan gocv.Mat),
 		objectsChan:  make(chan events.ObjectsMessage),
+		roadChan:     make(chan events.RoadMessage),
 		cancel:       make(chan interface{}),
 	}
 
@@ -28,13 +31,15 @@ func NewPart(client mqtt.Client, frameTopic, objectsTopic string, withObjects bo
 
 type FramePart struct {
 	client                   mqtt.Client
-	frameTopic, objectsTopic string
+	frameTopic, objectsTopic, roadTopic string
 
 	window      *gocv.Window
 	withObjects bool
+	withRoad    bool
 
 	imgChan     chan gocv.Mat
 	objectsChan chan events.ObjectsMessage
+	roadChan    chan events.RoadMessage
 	cancel      chan interface{}
 }
 
@@ -45,6 +50,8 @@ func (p *FramePart) Start() error {
 
 	var img = gocv.NewMat()
 	var objectsMsg events.ObjectsMessage
+	var roadMsg events.RoadMessage
+
 	for {
 		select {
 		case newImg := <-p.imgChan:
@@ -52,11 +59,13 @@ func (p *FramePart) Start() error {
 			img = newImg
 		case objects := <-p.objectsChan:
 			objectsMsg = objects
+		case road := <-p.roadChan:
+			roadMsg = road
 		case <-p.cancel:
 			img.Close()
 			return nil
 		}
-		p.drawFrame(&img, &objectsMsg)
+		p.drawFrame(&img, &objectsMsg, &roadMsg)
 	}
 }
 
@@ -65,7 +74,7 @@ func (p *FramePart) Stop() {
 
 	close(p.cancel)
 
-	StopService("frame-display", p.client, p.frameTopic)
+	StopService("frame-display", p.client, p.frameTopic, p.roadTopic)
 }
 
 func (p *FramePart) onFrame(_ mqtt.Client, message mqtt.Message) {
@@ -86,15 +95,27 @@ func (p *FramePart) onFrame(_ mqtt.Client, message mqtt.Message) {
 }
 
 func (p *FramePart) onObjects(_ mqtt.Client, message mqtt.Message) {
-	var objectsMsg events.ObjectsMessage
+	var msg events.ObjectsMessage
 
-	err := proto.Unmarshal(message.Payload(), &objectsMsg)
+	err := proto.Unmarshal(message.Payload(), &msg)
 	if err != nil {
-		log.Errorf("unable to unmarshal detected objects: %v", err)
+		log.Errorf("unable to unmarshal msg %T: %v", msg, err)
 		return
 	}
 
-	p.objectsChan <- objectsMsg
+	p.objectsChan <- msg
+}
+
+func (p *FramePart) onRoad(_ mqtt.Client, message mqtt.Message) {
+	var msg events.RoadMessage
+
+	err := proto.Unmarshal(message.Payload(), &msg)
+	if err != nil {
+		log.Errorf("unable to unmarshal msg %T: %v", msg, err)
+		return
+	}
+
+	p.roadChan <- msg
 }
 
 func (p *FramePart) registerCallbacks() error {
@@ -110,14 +131,23 @@ func (p *FramePart) registerCallbacks() error {
 		}
 	}
 
+	if p.withRoad {
+		err := service.RegisterCallback(p.client, p.roadTopic, p.onRoad)
+		if err != nil {
+			return err
+		}
+	}
 	return nil
 }
 
-func (p *FramePart) drawFrame(img *gocv.Mat, objects *events.ObjectsMessage) {
+func (p *FramePart) drawFrame(img *gocv.Mat, objects *events.ObjectsMessage, road *events.RoadMessage) {
 
 	if p.withObjects {
 		p.drawObjects(img, objects)
 	}
+	if p.withRoad {
+		p.drawRoad(img, road)
+	}
 
 	p.window.IMShow(*img)
 	p.window.WaitKey(1)
@@ -133,6 +163,20 @@ func (p *FramePart) drawObjects(img *gocv.Mat, objects *events.ObjectsMessage) {
 	}
 }
 
+func (p *FramePart) drawRoad(img *gocv.Mat, road *events.RoadMessage) {
+	cntr := make([]image.Point, 0, len(road.GetContour()))
+	for _, pt := range road.GetContour() {
+		cntr = append(cntr, image.Point{X: int(pt.GetX()), Y: int(pt.GetY())})
+	}
+
+	gocv.DrawContours(
+		img,
+		[][]image.Point{cntr},
+		0,
+		color.RGBA{R: 255, G: 0, B: 0, A: 128,},
+		-1)
+}
+
 func StopService(name string, client mqtt.Client, topics ...string) {
 	log.Printf("Stop %s service", name)
 	token := client.Unsubscribe(topics...)