90 lines
2.0 KiB
Go
90 lines
2.0 KiB
Go
//go:build example
|
|
// +build example
|
|
|
|
//
|
|
// Do not build by default.
|
|
|
|
package main
|
|
|
|
import (
|
|
"fmt"
|
|
"image"
|
|
"math"
|
|
"path"
|
|
"runtime"
|
|
"time"
|
|
|
|
"gobot.io/x/gobot/v2"
|
|
"gobot.io/x/gobot/v2/platforms/opencv"
|
|
"gobot.io/x/gobot/v2/platforms/parrot/ardrone"
|
|
"gocv.io/x/gocv"
|
|
)
|
|
|
|
func main() {
|
|
runtime.GOMAXPROCS(runtime.NumCPU())
|
|
|
|
_, currentfile, _, _ := runtime.Caller(0)
|
|
cascade := path.Join(path.Dir(currentfile), "haarcascade_frontalface_alt.xml")
|
|
window := opencv.NewWindowDriver()
|
|
camera := opencv.NewCameraDriver("tcp://192.168.1.1:5555")
|
|
ardroneAdaptor := ardrone.NewAdaptor()
|
|
drone := ardrone.NewDriver(ardroneAdaptor)
|
|
|
|
work := func() {
|
|
detect := false
|
|
drone.TakeOff()
|
|
var img gocv.Mat
|
|
camera.On(opencv.Frame, func(data interface{}) {
|
|
img = data.(gocv.Mat)
|
|
if !detect {
|
|
window.IMShow(img)
|
|
window.WaitKey(1)
|
|
}
|
|
})
|
|
_ = drone.On(ardrone.Flying, func(data interface{}) {
|
|
gobot.After(1*time.Second, func() { drone.Up(0.2) })
|
|
gobot.After(2*time.Second, func() { drone.Hover() })
|
|
gobot.After(5*time.Second, func() {
|
|
detect = true
|
|
gobot.Every(300*time.Millisecond, func() {
|
|
drone.Hover()
|
|
i := img
|
|
faces := opencv.DetectObjects(cascade, i)
|
|
biggest := 0
|
|
var face image.Rectangle
|
|
for _, f := range faces {
|
|
if f.Width() > biggest {
|
|
biggest = f.Width()
|
|
face = f
|
|
}
|
|
}
|
|
if face != nil {
|
|
opencv.DrawRectangles(i, []img.Rectangle{face}, 0, 255, 0, 5)
|
|
centerX := float64(img.Size()).X * 0.5
|
|
turn := -(float64(face.Min.X - centerX)) / centerX
|
|
fmt.Println("turning:", turn)
|
|
if turn < 0 {
|
|
drone.Clockwise(math.Abs(turn * 0.4))
|
|
} else {
|
|
drone.CounterClockwise(math.Abs(turn * 0.4))
|
|
}
|
|
}
|
|
window.IMShow(i)
|
|
window.WaitKey(1)
|
|
})
|
|
gobot.After(20*time.Second, func() { drone.Land() })
|
|
})
|
|
})
|
|
}
|
|
|
|
robot := gobot.NewRobot("face",
|
|
[]gobot.Connection{ardroneAdaptor},
|
|
[]gobot.Device{window, camera, drone},
|
|
work,
|
|
)
|
|
|
|
if err := robot.Start(); err != nil {
|
|
panic(err)
|
|
}
|
|
}
|