func TestOpenCloseLib(t *testing.T) {
lib, rc := freenect.Initialize()
if rc != 0 {
t.Errorf("Initialize returned %d", rc)
}
lib.Shutdown()
}
func TestOpenCloseDevs(t *testing.T) {
lib, rc := freenect.Initialize()
var logger = func(level int, message string) {
fmt.Printf("LEVEL: %d MSG: %s", level, message)
}
if rc == 0 {
defer lib.Shutdown()
lib.Log(logger)
lib.LogLevel(freenect.LogFlood)
for i := 0; i < len(lib.Devices); i++ {
dev := lib.Devices[i]
rc = dev.Open()
if rc != 0 {
t.Errorf("Failed to open device %d. Returned %d", i, rc)
}
rc = dev.Close()
if rc != 0 {
t.Errorf("Failed to close device %d. Returned %d", i, rc)
}
}
}
}
// uses a single video frame buffer that gets set on the first sourcing
// the processing occurs in the sink and future calls to source return nil
// which skips reseting the video buffer pointer
func TestVideoOneBufferNoSwap(t *testing.T) {
t.Logf("TestVideoOneBufferNoSwap")
var logger = func(level int, message string) {
fmt.Printf("LEVEL: %d MSG: %s", level, message)
}
var buffer []byte = nil
var source = func(bytes int) []byte {
if buffer == nil {
fmt.Printf("Creating buffer\n")
buffer = make([]byte, bytes)
return buffer
}
return nil
}
var recvd = 0
var first, last int64 = 0, 0
var sink = func(frame []byte, stamp int32) {
if &frame[0] != &buffer[0] {
t.Errorf("Unknown frame buffer arrived")
}
if recvd == 0 {
first = time.Now().UnixNano()
}
if recvd == 29 {
last = time.Now().UnixNano()
}
crc := crc32.ChecksumIEEE(frame)
fmt.Printf("Got frame %d stamped: %d crc32: %x\n", recvd, stamp, crc)
recvd++
}
lib, rc := freenect.Initialize()
if rc == 0 {
defer lib.Shutdown()
lib.Log(logger)
lib.LogLevel(freenect.LogWarning)
for i := 0; i < len(lib.Devices); i++ {
dev := lib.Devices[i]
rc = dev.Open()
if rc != 0 {
t.Errorf("Failed to open device. Returned %d", rc)
}
defer dev.Close()
cam, rc := dev.VideoCamera(freenect.MEDIUM, freenect.RGB, source, sink)
if rc != 0 {
t.Errorf("No camera. Returned %d", rc)
}
cam.Start()
defer cam.Stop()
for recvd < 30 {
fmt.Printf("")
}
fmt.Printf("Got 30 video frames in %d ms\n", (last-first)/1e6)
}
}
}
func TestTilt(t *testing.T) {
lib, rc := freenect.Initialize()
if rc == 0 {
defer lib.Shutdown()
for i := 0; i < len(lib.Devices); i++ {
dev := lib.Devices[i]
rc = dev.Open()
if rc != 0 {
t.Errorf("Failed to open device. Returned %d", rc)
}
defer dev.Close()
dev.LED(freenect.OFF)
tilt := dev.GetTilt()
run := true
go func() {
for run {
tilt.Refresh()
}
}()
dump := func() {
motor := ""
switch tilt.Status {
case freenect.TILT_STOPPED:
motor = "stopped"
break
case freenect.TILT_AT_LIMIT:
motor = "at limit"
break
default:
motor = "moving"
}
fmt.Printf("Current settings -- angle %f degrees, motor %s, accel (%f, %f, %f)\n", tilt.Angle, motor, tilt.AccelX, tilt.AccelY, tilt.AccelZ)
}
dev.LED(freenect.BLINK_GREEN)
dump()
fmt.Printf("Leveling...\n")
tilt.SetAngle(0.0)
for tilt.Angle != 0.0 {
}
dump()
time.Sleep(1e9)
dev.LED(freenect.RED)
fmt.Printf("27 degrees\n")
tilt.SetAngle(27.0)
for tilt.Status == freenect.TILT_MOVING {
}
dump()
time.Sleep(1e9)
dev.LED(freenect.YELLOW)
fmt.Printf("-27 degrees\n")
tilt.SetAngle(-27.0)
for tilt.Status == freenect.TILT_MOVING {
}
dump()
time.Sleep(1e9)
fmt.Printf("Relevel\n")
dev.LED(freenect.BLINK_RED_YELLOW)
tilt.SetAngle(0.0)
for tilt.Angle != 0.0 {
}
dump()
time.Sleep(1e9)
run = false
}
}
}
func TestVideoAndDepth(t *testing.T) {
t.Logf("TestVideoAndDepth")
var logger = func(level int, message string) {
fmt.Printf("LEVEL: %d MSG: %s", level, message)
}
vframes := make(chan []byte)
dframes := make(chan []uint16)
run := 2
ready := true
recvd := 0
gamma := make([]float64, 2048)
for i := 0; i < 2048; i++ {
v := float64(i) / 2048.0
v = v * v * v * 6.0
gamma[i] = v * 6.0 * 256.0
}
go func() {
fmt.Printf("Waiting to process frames\n")
var video []byte = nil
var depth []uint16 = nil
var image = func() {
fname := fmt.Sprintf("bothtest-%d.png", recvd)
f, err := os.OpenFile(fname, os.O_CREATE|os.O_WRONLY, 0666)
if err == nil {
m := image.NewNRGBA(image.Rect(0, 0, 1280, 480))
for y := 0; y < 480; y++ {
for x := 0; x < 640; x++ {
m.Set(x, y, color.NRGBA{uint8(video[(y*640*3)+(3*x)]), uint8(video[(y*640*3)+((3*x)+1)]), uint8(video[(y*640*3)+((3*x)+2)]), 255})
d := depth[x+(y*640)]
mm := uint16(gamma[d])
var rgba color.NRGBA
lb := uint8(mm & 0xff)
switch mm >> 8 {
case 0:
rgba = color.NRGBA{255, 255 - lb, 255 - lb, 255}
break
case 1:
rgba = color.NRGBA{255, lb, 0, 255}
break
case 2:
rgba = color.NRGBA{255 - lb, 255, 0, 255}
break
case 3:
rgba = color.NRGBA{0, 255, lb, 255}
break
case 4:
rgba = color.NRGBA{0, 255 - lb, 255, 255}
break
case 5:
rgba = color.NRGBA{0, 0, 255 - lb, 255}
break
default:
rgba = color.NRGBA{0, 0, 0, 255}
break
}
m.Set(x+640, y, rgba)
}
}
png.Encode(f, m)
}
video = nil
depth = nil
recvd++
}
for run > 0 {
select {
case frame := <-vframes:
video = make([]byte, len(frame))
copy(video, frame)
if depth != nil {
image()
}
case frame := <-dframes:
depth = make([]uint16, len(frame))
copy(depth, frame)
if video != nil {
image()
}
default:
}
}
fmt.Printf("Done processing frames\n")
}()
var vbuffer []byte = nil
var vsource = func(bytes int) []byte {
if vbuffer == nil {
vbuffer = make([]byte, bytes)
return vbuffer
}
return nil
}
var vsink = func(frame []byte, stamp int32) {
if ready {
vframes <- frame
} else {
run--
}
}
var dbuffer []uint16 = nil
var dsource = func(bytes int) []uint16 {
if dbuffer == nil {
dbuffer = make([]uint16, bytes)
return dbuffer
}
return nil
}
var dsink = func(frame []uint16, stamp int32) {
if ready {
dframes <- frame
} else {
run--
}
}
lib, rc := freenect.Initialize()
if rc == 0 {
defer lib.Shutdown()
lib.Log(logger)
lib.LogLevel(freenect.LogWarning)
for i := 0; i < len(lib.Devices); i++ {
dev := lib.Devices[i]
rc = dev.Open()
if rc != 0 {
t.Errorf("Failed to open device. Returned %d", rc)
}
defer dev.Close()
vcam, rc := dev.VideoCamera(freenect.MEDIUM, freenect.RGB, vsource, vsink)
if rc != 0 {
t.Errorf("No video camera. Returned %d", rc)
}
dcam, rc := dev.DepthCamera(freenect.MEDIUM, freenect.D11BIT, dsource, dsink)
if rc != 0 {
t.Errorf("No depth camera. Returned %d", rc)
}
vcam.Start()
defer vcam.Stop()
dcam.Start()
defer dcam.Stop()
for recvd < 30 {
fmt.Printf("")
}
ready = false
}
}
}
func TestDepthOnly(t *testing.T) {
t.Logf("TestDepthOnly")
var logger = func(level int, message string) {
fmt.Printf("LEVEL: %d MSG: %s", level, message)
}
var buffer []uint16 = nil
var source = func(bytes int) []uint16 {
if buffer == nil {
buffer = make([]uint16, bytes)
return buffer
}
return nil
}
gamma := make([]float64, 2048)
for i := 0; i < 2048; i++ {
v := float64(i) / 2048.0
v = v * v * v * 6.0
gamma[i] = v * 6.0 * 256.0
//gamma[i] = 0.1236 * math.Tan((float64(i)/2842.5) + 1.1863)
}
fmt.Printf("%f %f\n", gamma[0], gamma[2047])
var recvd = 0
var sink = func(frame []uint16, stamp int32) {
if &frame[0] != &buffer[0] {
t.Errorf("Unknown depth buffer arrived")
}
// PNG each frame
fname := fmt.Sprintf("depthtest-%d.png", recvd)
f, err := os.OpenFile(fname, os.O_CREATE|os.O_WRONLY, 0666)
if err == nil {
m := image.NewNRGBA(image.Rect(0, 0, 640, 480))
for y := 0; y < 480; y++ {
for x := 0; x < 640; x++ {
d := buffer[x+(y*640)]
mm := uint16(gamma[d])
var rgba color.NRGBA
lb := uint8(mm & 0xff)
switch mm >> 8 {
case 0:
rgba = color.NRGBA{255, 255 - lb, 255 - lb, 255}
break
case 1:
rgba = color.NRGBA{255, lb, 0, 255}
break
case 2:
rgba = color.NRGBA{255 - lb, 255, 0, 255}
break
case 3:
rgba = color.NRGBA{0, 255, lb, 255}
break
case 4:
rgba = color.NRGBA{0, 255 - lb, 255, 255}
break
case 5:
rgba = color.NRGBA{0, 0, 255 - lb, 255}
break
default:
rgba = color.NRGBA{0, 0, 0, 255}
break
}
m.Set(x, y, rgba)
}
}
png.Encode(f, m)
}
fmt.Printf("Got frame %d stamped: %d\n", recvd, stamp)
recvd++
}
lib, rc := freenect.Initialize()
if rc == 0 {
defer lib.Shutdown()
lib.Log(logger)
lib.LogLevel(freenect.LogWarning)
for i := 0; i < len(lib.Devices); i++ {
dev := lib.Devices[i]
rc = dev.Open()
if rc != 0 {
t.Errorf("Failed to open device. Returned %d", rc)
}
defer dev.Close()
cam, rc := dev.DepthCamera(freenect.MEDIUM, freenect.D11BIT, source, sink)
if rc != 0 {
t.Errorf("No camera. Returned %d", rc)
}
cam.Start()
defer cam.Stop()
for recvd < 30 {
fmt.Printf("")
}
}
}
}
// capture and process using two buffers
func TestVideoDoubleBuffer(t *testing.T) {
t.Logf("TestVideoDoubleBuffer")
var logger = func(level int, message string) {
fmt.Printf("LEVEL: %d MSG: %s", level, message)
}
buffers := make(chan []byte, 2)
frames := make(chan []byte, 2)
run := true
recvd := 0
go func() {
fmt.Printf("Waiting to process frames\n")
for run {
select {
// wait for the next frame
case frame := <-frames:
if recvd == 30 {
return
}
// PNG each frame
fname := fmt.Sprintf("vdbtest-%d.png", recvd)
f, err := os.OpenFile(fname, os.O_CREATE|os.O_WRONLY, 0666)
if err == nil {
m := image.NewNRGBA(image.Rect(0, 0, 640, 480))
for y := 0; y < 480; y++ {
for x := 0; x < 640; x++ {
m.Set(x, y, color.NRGBA{uint8(frame[(y*640*3)+(3*x)]), uint8(frame[(y*640*3)+((3*x)+1)]), uint8(frame[(y*640*3)+((3*x)+2)]), 255})
}
}
png.Encode(f, m)
}
// return the buffer
buffers <- frame
recvd++
default:
}
}
fmt.Printf("Done processing frames\n")
}()
var once = true
var source = func(bytes int) []byte {
if once {
once = false
fmt.Printf("Allocating buffers\n")
buffers <- make([]byte, bytes)
buffers <- make([]byte, bytes)
fmt.Printf("Allocated buffers\n")
}
return <-buffers
}
var sink = func(frame []byte, stamp int32) {
frames <- frame
}
lib, rc := freenect.Initialize()
if rc == 0 {
defer lib.Shutdown()
lib.Log(logger)
lib.LogLevel(freenect.LogWarning)
for i := 0; i < len(lib.Devices); i++ {
dev := lib.Devices[i]
rc = dev.Open()
if rc != 0 {
t.Errorf("Failed to open device. Returned %d", rc)
}
defer dev.Close()
cam, rc := dev.VideoCamera(freenect.MEDIUM, freenect.RGB, source, sink)
if rc != 0 {
t.Errorf("No camera. Returned %d", rc)
}
cam.Start()
defer cam.Stop()
for recvd < 30 {
fmt.Printf("")
}
run = false
}
}
}