robocars/robocar-tools
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Display road

Browse Source
This commit is contained in:
Cyrille Nofficial 2020-01-04 15:01:10 +01:00
parent c724e9e8af
commit 3bfed77e43
3 changed files with 71 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
README.md
View File

@ -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
```

12
cmd/rc-display/rc-display.go
View File

@ -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)

62
part/part.go
View File

@ -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...)