DeviceDriver.cpp
Go to the documentation of this file.
1 
17 #include <map>
18 #include <string>
19 #include "Driver/OniDriverAPI.h"
20 #include "libfreenect.hpp"
21 #include "DepthStream.hpp"
22 #include "ColorStream.hpp"
23 
24 
25 namespace FreenectDriver
26 {
28  {
29  private:
32 
33  // for Freenect::FreenectDevice
34  void DepthCallback(void* data, uint32_t timestamp) {
35  depth->buildFrame(data, timestamp);
36  }
37  void VideoCallback(void* data, uint32_t timestamp) {
38  color->buildFrame(data, timestamp);
39  }
40 
41  public:
42  Device(freenect_context* fn_ctx, int index) : Freenect::FreenectDevice(fn_ctx, index),
43  color(NULL),
44  depth(NULL) { }
46  {
47  destroyStream(color);
48  destroyStream(depth);
49  }
50 
51  // for DeviceBase
52 
54 
55  OniStatus getSensorInfoList(OniSensorInfo** pSensors, int* numSensors)
56  {
57  *numSensors = 2;
58  OniSensorInfo * sensors = new OniSensorInfo[*numSensors];
59  sensors[0] = depth->getSensorInfo();
60  sensors[1] = color->getSensorInfo();
61  *pSensors = sensors;
62  return ONI_STATUS_OK;
63  }
64 
66  {
67  switch (sensorType)
68  {
69  default:
70  LogError("Cannot create a stream of type " + to_string(sensorType));
71  return NULL;
72  case ONI_SENSOR_COLOR:
73  if (! color)
74  color = new ColorStream(this);
75  return color;
76  case ONI_SENSOR_DEPTH:
77  if (! depth)
78  depth = new DepthStream(this);
79  return depth;
80  // todo: IR
81  }
82  }
83 
85  {
86  if (pStream == NULL)
87  return;
88 
89  if (pStream == color)
90  {
92  delete color;
93  color = NULL;
94  }
95  if (pStream == depth)
96  {
98  delete depth;
99  depth = NULL;
100  }
101  }
102 
103  // todo: fill out properties
104  OniBool isPropertySupported(int propertyId)
105  {
106  if (propertyId == ONI_DEVICE_PROPERTY_IMAGE_REGISTRATION)
107  return true;
108  return false;
109  }
110 
111  OniStatus getProperty(int propertyId, void* data, int* pDataSize)
112  {
113  switch (propertyId)
114  {
115  default:
117  case ONI_DEVICE_PROPERTY_DRIVER_VERSION: // OniVersion
119  case ONI_DEVICE_PROPERTY_SERIAL_NUMBER: // string
121  // files
124  // xn
125  case XN_MODULE_PROPERTY_USB_INTERFACE: // XnSensorUsbInterface
126  case XN_MODULE_PROPERTY_MIRROR: // bool
127  case XN_MODULE_PROPERTY_RESET_SENSOR_ON_STARTUP: // unsigned long long
128  case XN_MODULE_PROPERTY_LEAN_INIT: // unsigned long long
129  case XN_MODULE_PROPERTY_SERIAL_NUMBER: // unsigned long long
130  case XN_MODULE_PROPERTY_VERSION: // XnVersions
132 
133  case ONI_DEVICE_PROPERTY_IMAGE_REGISTRATION: // OniImageRegistrationMode
134  if (*pDataSize != sizeof(OniImageRegistrationMode))
135  {
136  LogError("Unexpected size for ONI_DEVICE_PROPERTY_IMAGE_REGISTRATION");
137  return ONI_STATUS_ERROR;
138  }
139  *(static_cast<OniImageRegistrationMode*>(data)) = depth->getImageRegistrationMode();
140  return ONI_STATUS_OK;
141  }
142  }
143 
144  OniStatus setProperty(int propertyId, const void* data, int dataSize)
145  {
146  switch (propertyId)
147  {
148  default:
149  case ONI_DEVICE_PROPERTY_FIRMWARE_VERSION: // By implementation
150  case ONI_DEVICE_PROPERTY_DRIVER_VERSION: // OniVersion
152  case ONI_DEVICE_PROPERTY_SERIAL_NUMBER: // string
154  // files
157  // xn
158  case XN_MODULE_PROPERTY_USB_INTERFACE: // XnSensorUsbInterface
159  case XN_MODULE_PROPERTY_MIRROR: // bool
160  case XN_MODULE_PROPERTY_RESET_SENSOR_ON_STARTUP: // unsigned long long
161  case XN_MODULE_PROPERTY_LEAN_INIT: // unsigned long long
162  case XN_MODULE_PROPERTY_SERIAL_NUMBER: // unsigned long long
163  case XN_MODULE_PROPERTY_VERSION: // XnVersions
164  // xn commands
165  case XN_MODULE_PROPERTY_FIRMWARE_PARAM: // XnInnerParam
166  case XN_MODULE_PROPERTY_RESET: // unsigned long long
167  case XN_MODULE_PROPERTY_IMAGE_CONTROL: // XnControlProcessingData
168  case XN_MODULE_PROPERTY_DEPTH_CONTROL: // XnControlProcessingData
169  case XN_MODULE_PROPERTY_AHB: // XnAHBData
170  case XN_MODULE_PROPERTY_LED_STATE: // XnLedState
172 
173  case ONI_DEVICE_PROPERTY_IMAGE_REGISTRATION: // OniImageRegistrationMode
174  if (dataSize != sizeof(OniImageRegistrationMode))
175  {
176  LogError("Unexpected size for ONI_DEVICE_PROPERTY_IMAGE_REGISTRATION");
177  return ONI_STATUS_ERROR;
178  }
179  return depth->setImageRegistrationMode(*(static_cast<const OniImageRegistrationMode*>(data)));
180  }
181  }
182 
184  {
185  switch (commandId)
186  {
187  default:
189  return false;
190  }
191  }
192 
193  OniStatus invoke(int commandId, void* data, int dataSize)
194  {
195  switch (commandId)
196  {
197  default:
198  case ONI_DEVICE_COMMAND_SEEK: // OniSeek
200  }
201  }
202 
203 
204  /* todo: for DeviceBase
205  virtual OniStatus tryManualTrigger() {return ONI_STATUS_OK;}
206  */
207  };
208 
209 
211  {
212  private:
213  typedef std::map<OniDeviceInfo, oni::driver::DeviceBase*> OniDeviceMap;
214  OniDeviceMap devices;
215 
216  static std::string devid_to_uri(int id) {
217  return "freenect://" + to_string(id);
218  }
219 
220  static int uri_to_devid(const std::string uri) {
221  int id;
222  std::istringstream is(uri);
223  is.seekg(strlen("freenect://"));
224  is >> id;
225  return id;
226  }
227 
228  public:
229  Driver(OniDriverServices* pDriverServices) : DriverBase(pDriverServices)
230  {
231  WriteMessage("Using libfreenect v" + to_string(PROJECT_VER));
232 
234  freenect_select_subdevices(m_ctx, FREENECT_DEVICE_CAMERA); // OpenNI2 doesn't use MOTOR or AUDIO
235  DriverServices = &getServices();
236  }
237  ~Driver() { shutdown(); }
238 
239  // for DriverBase
240 
241  OniStatus initialize(oni::driver::DeviceConnectedCallback connectedCallback, oni::driver::DeviceDisconnectedCallback disconnectedCallback, oni::driver::DeviceStateChangedCallback deviceStateChangedCallback, void* pCookie)
242  {
243  DriverBase::initialize(connectedCallback, disconnectedCallback, deviceStateChangedCallback, pCookie);
244  for (int i = 0; i < Freenect::deviceCount(); i++)
245  {
246  std::string uri = devid_to_uri(i);
247 
248  WriteMessage("Found device " + uri);
249 
250  OniDeviceInfo info;
251  strncpy(info.uri, uri.c_str(), ONI_MAX_STR);
252  strncpy(info.vendor, "Microsoft", ONI_MAX_STR);
253  strncpy(info.name, "Kinect", ONI_MAX_STR);
254  devices[info] = NULL;
255  deviceConnected(&info);
256  deviceStateChanged(&info, 0);
257  }
258  return ONI_STATUS_OK;
259  }
260 
261  oni::driver::DeviceBase* deviceOpen(const char* uri, const char* mode = NULL)
262  {
263  for (OniDeviceMap::iterator iter = devices.begin(); iter != devices.end(); iter++)
264  {
265  if (strcmp(iter->first.uri, uri) == 0) // found
266  {
267  if (iter->second) // already open
268  {
269  return iter->second;
270  }
271  else
272  {
273  WriteMessage("Opening device " + std::string(uri));
274  int id = uri_to_devid(iter->first.uri);
275  Device* device = &createDevice<Device>(id);
276  iter->second = device;
277  return device;
278  }
279  }
280  }
281 
282  LogError("Could not find device " + std::string(uri));
283  return NULL;
284  }
285 
287  {
288  for (OniDeviceMap::iterator iter = devices.begin(); iter != devices.end(); iter++)
289  {
290  if (iter->second == pDevice)
291  {
292  WriteMessage("Closing device " + std::string(iter->first.uri));
293  int id = uri_to_devid(iter->first.uri);
294  devices.erase(iter);
295  deleteDevice(id);
296  return;
297  }
298  }
299 
300  LogError("Could not close unrecognized device");
301  }
302 
303  OniStatus tryDevice(const char* uri)
304  {
305  oni::driver::DeviceBase* device = deviceOpen(uri);
306  if (! device)
307  return ONI_STATUS_ERROR;
308  deviceClose(device);
309  return ONI_STATUS_OK;
310  }
311 
312  void shutdown()
313  {
314  for (OniDeviceMap::iterator iter = devices.begin(); iter != devices.end(); iter++)
315  {
316  WriteMessage("Closing device " + std::string(iter->first.uri));
317  int id = uri_to_devid(iter->first.uri);
318  deleteDevice(id);
319  }
320 
321  devices.clear();
322  }
323 
324 
325  /* todo: for DriverBase
326  virtual void* enableFrameSync(oni::driver::StreamBase** pStreams, int streamCount);
327  virtual void disableFrameSync(void* frameSyncGroup);
328  */
329  };
330 }
331 
332 
333 // macros defined in XnLib (not included) - workaround
334 #define XN_NEW(type, arg...) new type(arg)
335 #define XN_DELETE(p) delete(p)
337 #undef XN_NEW
338 #undef XN_DELETE
OniImageRegistrationMode
Definition: OniCEnums.h:72
void DepthCallback(void *data, uint32_t timestamp)
void freenect_select_subdevices(freenect_context *ctx, freenect_device_flags subdevs)
Definition: fakenect.c:338
static void LogError(std::string error)
Definition: Utility.hpp:66
OniImageRegistrationMode getImageRegistrationMode() const
Definition: DepthStream.hpp:56
Driver(OniDriverServices *pDriverServices)
OniStatus tryDevice(const char *uri)
OniStatus getProperty(int propertyId, void *data, int *pDataSize)
Device(freenect_context *fn_ctx, int index)
OniStatus setProperty(int propertyId, const void *data, int dataSize)
int OniBool
Definition: OniCTypes.h:28
void VideoCallback(void *data, uint32_t timestamp)
OniStatus invoke(int commandId, void *data, int dataSize)
OniStatus getSensorInfoList(OniSensorInfo **pSensors, int *numSensors)
OniBool isPropertySupported(int propertyId)
OniStatus setImageRegistrationMode(OniImageRegistrationMode mode)
Definition: DepthStream.hpp:57
ONI_EXPORT_DRIVER(FreenectDriver::Driver)
char vendor[ONI_MAX_STR]
Definition: OniCTypes.h:79
void freenect_set_log_level(freenect_context *ctx, freenect_loglevel level)
Definition: fakenect.c:401
OniBool isImageRegistrationModeSupported(OniImageRegistrationMode mode)
OniSensorType
Definition: OniCEnums.h:38
oni::driver::DeviceBase * deviceOpen(const char *uri, const char *mode=NULL)
std::map< OniDeviceInfo, oni::driver::DeviceBase * > OniDeviceMap
char uri[ONI_MAX_STR]
Definition: OniCTypes.h:78
void destroyStream(oni::driver::StreamBase *pStream)
MyFreenectDevice * device
static int uri_to_devid(const std::string uri)
OniStatus initialize(oni::driver::DeviceConnectedCallback connectedCallback, oni::driver::DeviceDisconnectedCallback disconnectedCallback, oni::driver::DeviceStateChangedCallback deviceStateChangedCallback, void *pCookie)
void buildFrame(void *data, uint32_t timestamp)
Definition: VideoStream.hpp:38
OniBool isCommandSupported(int commandId)
oni::driver::StreamBase * createStream(OniSensorType sensorType)
static std::string devid_to_uri(int id)
#define ONI_MAX_STR
Definition: OniCTypes.h:37
static OniSensorInfo getSensorInfo()
Definition: DepthStream.hpp:47
OniBool isImageRegistrationModeSupported(OniImageRegistrationMode mode)
Definition: DepthStream.hpp:66
static std::string to_string(const T &n)
Definition: Utility.hpp:48
unsigned int uint32_t
char name[ONI_MAX_STR]
Definition: OniCTypes.h:80
static void WriteMessage(std::string info)
Definition: Utility.hpp:59
static oni::driver::DriverServices * DriverServices
Definition: Utility.hpp:65
void(ONI_CALLBACK_TYPE * DeviceStateChangedCallback)(const OniDeviceInfo *deviceId, int errorState, void *pCookie)
Definition: OniDriverAPI.h:37
void deviceClose(oni::driver::DeviceBase *pDevice)
OniStatus
Definition: OniCEnums.h:25
void * pCookie
Definition: OniDriverAPI.h:35
FreenectDevice(freenect_context *_ctx, int _index)
Definition: libfreenect.hpp:65
void(ONI_CALLBACK_TYPE * DeviceDisconnectedCallback)(const OniDeviceInfo *, void *pCookie)
Definition: OniDriverAPI.h:36
static OniSensorInfo getSensorInfo()
Definition: ColorStream.hpp:35


libfreenect
Author(s): Hector Martin, Josh Blake, Kyle Machulis, OpenKinect community
autogenerated on Thu Jun 6 2019 19:25:38