This repository has been archived by the owner on Nov 28, 2018. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 5
/
Freenect.hs
218 lines (189 loc) · 7.48 KB
/
Freenect.hs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
{-# LANGUAGE ViewPatterns #-}
{-# OPTIONS -fno-warn-name-shadowing #-}
{-# LANGUAGE DeriveDataTypeable #-}
-- | Interface to the Kinect device.
module Freenect
(initialize
,newContext
,shutdown
,countDevices
,withContext
,processEvents
,selectSubdevices
,newDevice
,openDevice
,closeDevice
,withDevice
,setLogLevel
,setDepthCallback
,startDepth
,setTiltDegrees
,Context
,FreenectException(..)
,Subdevice(..)
,LogLevel(..))
where
import Freenect.FFI
import Control.Monad
import Data.Bits
import Data.List
import Control.Exception
import Data.Typeable
import Foreign
import Foreign.C
import Data.IORef
-- | An acquireable resource. This abstracts the notion of C-level
-- pointers that may or may not refer to something in memory. Avoids
-- segmentation faults and other nasties. Nobody wants segmentation
-- faults in their Haskell code.
data Resource a = Initialized a | Uninitialized a
deriving Show
-- | A Freenect context.
newtype Context = CPtr (IORef (Resource (Ptr (Ptr ContextStruct))))
-- | A Freenect device.
newtype Device = DPtr (IORef (Resource (Ptr (Ptr DeviceStruct))))
-- | Freenect exception type.
data FreenectException
= InitFail -- ^ There was a problem initializing.
| ShutdownFail -- ^ There was a problem shutting down.
| CloseDeviceFail -- ^ There was a problem closing the device.
| AlreadyInitializedContext -- ^ Trying to initialize a context that
-- was already initialized.
| AlreadyOpenedDevice -- ^ Trying to open a device that was
-- already opened.
| UseOfUninitializedContext -- ^ Attempt to use an uninitialized
-- context.
| UseOfUninitializedDevice -- ^ Attempt to use an uninitialized
-- device.
| ProcessEvents -- ^ Call to process events failed.
| OpenDeviceFailed Integer -- ^ Opening a device failed.
| StartDepthProblem -- ^ Problem starting the depth stream.
| UnableToSetTilt -- ^ Unable to set the tilt.
deriving (Show,Typeable)
instance Exception FreenectException
-- | Initialize a Freenect context. Throws exception if already
-- initialized.
initialize :: Context -> IO ()
initialize (CPtr ptrRef) = do
ptr <- readIORef ptrRef
case ptr of
Initialized{} -> throw AlreadyInitializedContext
Uninitialized ptr -> do
succeed InitFail (writeIORef ptrRef (Initialized ptr)) $
freenect_init ptr 0
-- | Create a new Freenect context. Must be initialized before use.
newContext :: IO Context
newContext = new_freenect_context >>= fmap CPtr . newIORef . Uninitialized
-- | Shutdown a Freenect context.
shutdown :: Context -> IO ()
shutdown cptr@(CPtr ptrRef) = flip withC cptr $ \ptr ->
succeed ShutdownFail
(writeIORef ptrRef (Uninitialized ptr))
(peek ptr >>= freenect_shutdown)
-- | Count the number of devices on a Freenect context.
countDevices :: Context -> IO Integer
countDevices =
withC $ \ptr ->
fmap fromIntegral (peek ptr >>= freenect_num_devices)
-- | Do something with an initialized context, and free the context at
-- the end of the comutation, or on exception.
withContext :: (Context -> IO a) -> IO a
withContext f = bracket newContext shutdown (\c -> do initialize c; f c)
-- | Process events.
processEvents :: Context -> IO ()
processEvents = withC (succeed ProcessEvents (return ())
. (peek >=> freenect_process_events))
-- | Run a computation for which the CInt result is zero (in C this is
-- success), and thrown an exception if the result is non-zero.
succeed :: Exception e => e -> IO () -> IO CInt -> IO ()
succeed e ok m = do
result <- m
if result == 0
then ok
else throw e
-- | A sub-device (motor, camera and audio), if supported on the
-- platform.
data Subdevice = Motor | Camera | Auto
deriving (Show,Eq)
-- | Set which subdevices any subsequent calls to openDevice should
-- open. This will not affect devices which have already been
-- opened. The default behavior, should you choose not to call this
-- function at all, is to open all supported subdevices - motor,
-- cameras, and audio, if supported on the platform.
selectSubdevices :: Context -> [Subdevice] -> IO ()
selectSubdevices c (nub -> subdevices) = flip withC c $ \ptr -> do
ptr <- peek ptr
freenect_select_subdevices ptr (foldl1 (.|.) (map toDeviceId subdevices))
where toDeviceId Motor = 1
toDeviceId Camera = 2
toDeviceId Auto = 4
-- | Create a new device.
newDevice :: IO Device
newDevice = new_freenect_device >>= fmap DPtr . newIORef . Uninitialized
-- | Open a Kinect device.
openDevice :: Context -> Device -> Integer -> IO ()
openDevice c (DPtr devptr) index = flip withC c $ \cptr -> do
dptr <- readIORef devptr
case dptr of
Initialized{} -> throw AlreadyOpenedDevice
Uninitialized dptr -> do
succeed (OpenDeviceFailed index) (writeIORef devptr (Initialized dptr)) $ do
cptr <- peek cptr
freenect_open_device cptr dptr (fromIntegral index)
-- | Close a device.
closeDevice :: Device -> IO ()
closeDevice dptr@(DPtr ptrRef) = do
flip withD dptr $ \ptr -> do
succeed CloseDeviceFail
(writeIORef ptrRef (Uninitialized ptr))
(peek ptr >>= freenect_close_device)
-- | Do something with an initialized context, and free the context at
-- the end of the comutation, or on exception.
withDevice :: Context -> Integer -> (Device -> IO a) -> IO a
withDevice ctx i f = bracket newDevice closeDevice (\d -> do openDevice ctx d i; f d)
-- | Do something with a device pointer. Unexported.
withD :: (Ptr (Ptr DeviceStruct) -> IO a) -> Device -> IO a
withD cons (DPtr ptr) = do
ptr <- readIORef ptr
case ptr of
Uninitialized{} -> throw UseOfUninitializedDevice
Initialized ptr -> cons ptr
-- | Do something with a context pointer. Unexported.
withC :: (Ptr (Ptr ContextStruct) -> IO a) -> Context -> IO a
withC cons (CPtr ptr) = do
ptr <- readIORef ptr
case ptr of
Uninitialized{} -> throw UseOfUninitializedContext
Initialized ptr -> cons ptr
-- | Message logging levels.
data LogLevel
= LogFatal -- ^ Crashing/non-recoverable errors
| LogError -- ^ Major errors
| LogWarning -- ^ Warning messages
| LogNotice -- ^ Important messages
| LogInfo -- ^ Normal messages
| LogDebug -- ^ Useful development messages
| LogSpew -- ^ Slightly less useful messages
| LogFlood -- ^ EVERYTHING. May slow performance.
deriving (Show,Eq,Enum)
-- | Set the logging level for the specified context.
setLogLevel :: LogLevel -> Context -> IO ()
setLogLevel level = withC $ \ptr -> do
ptr <- peek ptr
freenect_set_log_level ptr (fromIntegral (fromEnum level))
-- | Set callback for depth information received event.
setDepthCallback :: Device -> (Ptr DeviceStruct -> Ptr () -> Word32 -> IO ()) -> IO ()
setDepthCallback d callback = flip withD d $ \dptr -> do
dptr <- peek dptr
callbackPtr <- wrapDepthCallback callback
freenect_set_depth_callback dptr callbackPtr
-- | Start the depth information stream for a device.
startDepth :: Device -> IO ()
startDepth = withD $ \ptr -> succeed StartDepthProblem (return ()) $ do
ptr <- peek ptr
freenect_start_depth ptr
-- | Start the depth information stream for a device.
setTiltDegrees :: Double -> Device -> IO ()
setTiltDegrees angle = withD $ \ptr -> succeed UnableToSetTilt (return ()) $ do
ptr <- peek ptr
freenect_set_tilt_degs ptr (realToFrac angle)