From 3bfed77e43ff77397618ff712da8f4086f3cdd9a Mon Sep 17 00:00:00 2001 From: Cyrille Nofficial Date: Sat, 4 Jan 2020 15:01:10 +0100 Subject: [PATCH] Display road --- README.md | 10 +++++- cmd/rc-display/rc-display.go | 12 +++++-- part/part.go | 62 ++++++++++++++++++++++++++++++------ 3 files changed, 71 insertions(+), 13 deletions(-) diff --git a/README.md b/README.md index a52cb9b..1ae74b3 100644 --- a/README.md +++ b/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 ``` diff --git a/cmd/rc-display/rc-display.go b/cmd/rc-display/rc-display.go index 4304c87..dc90648 100644 --- a/cmd/rc-display/rc-display.go +++ b/cmd/rc-display/rc-display.go @@ -14,8 +14,8 @@ const ( func main() { var mqttBroker, username, password, clientId string - var frameTopic, objectsTopic string - var withObjects bool + var frameTopic, objectsTopic, roadTopic string + 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) diff --git a/part/part.go b/part/part.go index 52bda2d..c167471 100644 --- a/part/part.go +++ b/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...)