XnStatus XnSensorIO::EnumerateSensors(XnConnectionString* aConnectionStrings, XnUInt32* pnCount)
{
XnStatus nRetVal = XN_STATUS_OK;
nRetVal = xnUSBInit();
if (nRetVal != XN_STATUS_OK && nRetVal != XN_STATUS_USB_ALREADY_INIT)
return nRetVal;
// Temporary patch: "Cache" the devices since running USB enum on the MacOSX platform takes several seconds due to problems in libusb!
#if (XN_PLATFORM == XN_PLATFORM_MACOSX)
static XnStringsHash devicesSet;
if (devicesSet.Size() == 0)
{
// search for a v6.0.1 device
nRetVal = Enumerate(XN_SENSOR_6_0_1_PRODUCT_ID, devicesSet);
XN_IS_STATUS_OK(nRetVal);
// search for a v6.0 device
nRetVal = Enumerate(XN_SENSOR_6_0_PRODUCT_ID, devicesSet);
XN_IS_STATUS_OK(nRetVal);
}
#else
XnStringsHash devicesSet;
// search for a v6.0.1 device
nRetVal = Enumerate(XN_SENSOR_6_0_1_PRODUCT_ID, devicesSet);
XN_IS_STATUS_OK(nRetVal);
// search for a v6.0 device
nRetVal = Enumerate(XN_SENSOR_6_0_PRODUCT_ID, devicesSet);
XN_IS_STATUS_OK(nRetVal);
// search for a v5.0 device
nRetVal = Enumerate(XN_SENSOR_5_0_PRODUCT_ID, devicesSet);
XN_IS_STATUS_OK(nRetVal);
#endif
// now copy back
XnUInt32 nCount = 0;
for (XnStringsHash::ConstIterator it = devicesSet.begin(); it != devicesSet.end(); ++it, ++nCount)
{
if (nCount < *pnCount)
{
strcpy(aConnectionStrings[nCount], it.Key());
}
}
if (nCount > *pnCount)
{
*pnCount = nCount;
return XN_STATUS_OUTPUT_BUFFER_OVERFLOW;
}
// All is good...
*pnCount = nCount;
return (XN_STATUS_OK);
}
XnStatus OpenNIMotorController::Create()
{
const XnUSBConnectionString *paths;
XnUInt32 count;
XnStatus res;
// Init OpenNI USB
res = xnUSBInit();
//std::cout << xnGetStatusString(res) << " " << res << std::endl;
if( res != XN_STATUS_OK && res != 131142 /** USB alreay initialized **/ )
return res;
// list all "Kinect motor" USB devices
res = xnUSBEnumerateDevices( 0x45e, 0x02b0, &paths, &count );
if( res != XN_STATUS_OK )
return res;
const XnChar* pUSBtoUse = paths[0];
if( count > 0 )
{
res = xnUSBOpenDeviceByPath( pUSBtoUse, &m_xDevice );
for( int i=0; i < count; ++i )
{
unsigned short vendor_id;
unsigned short product_id;
unsigned char bus;
unsigned char address;
sscanf(paths[i], "%hx/%hx@%hhu/%hhu", &vendor_id, &product_id, &bus, &address);
//printf("MotorInfo vendor_id %i product_id %i bus %i address %i \n", vendor_id, product_id, bus, address );
//std::cout << "MI: " << paths[i] << std::endl;
}
if( res != XN_STATUS_OK )
return res;
// Init motors
XnUChar buf;
res = xnUSBSendControl( m_xDevice, (XnUSBControlType) 0xc0, 0x10, 0x00, 0x00, &buf, sizeof(buf), 0 );
if( res != XN_STATUS_OK )
{
Release();
return res;
}
res = xnUSBSendControl( m_xDevice, XN_USB_CONTROL_TYPE_VENDOR, 0x06, 0x01, 0x00, NULL, 0, 0);
if( res != XN_STATUS_OK )
{
Release();
return res;
}
m_bOpened = true;
std::cout << "\n_2Real: The motor controller was initialized correctly" << std::endl;
return XN_STATUS_OK;
}
return XN_STATUS_OS_FILE_OPEN_FAILED;
}
XnStatus XnSensorIO::EnumerateSensors(XnConnectionString* aConnectionStrings, XnUInt32* pnCount)
{
XnStatus nRetVal = XN_STATUS_OK;
nRetVal = xnUSBInit();
if (nRetVal != XN_STATUS_OK && nRetVal != XN_STATUS_USB_ALREADY_INIT)
return nRetVal;
// Temporary patch: "Cache" the devices since running USB enum on the MacOSX platform takes several seconds due to problems in libusb!
#if (XN_PLATFORM == XN_PLATFORM_MACOSX)
static XnStringsSet devicesSet;
if (devicesSet.Size() == 0)
{
// search for supported devices
for (XnUInt32 i = 0; i < ms_supportedProductsCount; ++i)
{
nRetVal = Enumerate(ms_supportedProducts[i], devicesSet);
XN_IS_STATUS_OK(nRetVal);
}
}
#else
XnStringsSet devicesSet;
// search for supported devices
for (XnUInt32 i = 0; i < ms_supportedProductsCount; ++i)
{
nRetVal = Enumerate(ms_supportedProducts[i], devicesSet);
XN_IS_STATUS_OK(nRetVal);
}
#endif
// now copy back
XnUInt32 nCount = 0;
for (XnStringsSet::ConstIterator it = devicesSet.Begin(); it != devicesSet.End(); ++it, ++nCount)
{
if (nCount < *pnCount)
{
strcpy(aConnectionStrings[nCount], it->Key());
}
}
if (nCount > *pnCount)
{
*pnCount = nCount;
return XN_STATUS_OUTPUT_BUFFER_OVERFLOW;
}
// All is good...
*pnCount = nCount;
return (XN_STATUS_OK);
}
XnStatus XnSensorIO::EnumerateSensors(XnConnectionString* aConnectionStrings, XnUInt32* pnCount)
{
XnStatus nRetVal = XN_STATUS_OK;
XnBool bIsPresent = FALSE;
nRetVal = xnUSBInit();
if (nRetVal != XN_STATUS_OK && nRetVal != XN_STATUS_USB_ALREADY_INIT)
return nRetVal;
XnStringsHash devicesSet;
// --avin mod--
// search for a kinect device
nRetVal = Enumerate(XN_SENSOR_PRODUCT_ID_KINECT, devicesSet);
XN_IS_STATUS_OK(nRetVal);
// now copy back
XnUInt32 nCount = 0;
for (XnStringsHash::ConstIterator it = devicesSet.begin(); it != devicesSet.end(); ++it, ++nCount)
{
if (nCount < *pnCount)
{
strcpy(aConnectionStrings[nCount], it.Key());
}
}
if (nCount > *pnCount)
{
*pnCount = nCount;
return XN_STATUS_OUTPUT_BUFFER_OVERFLOW;
}
// All is good...
*pnCount = nCount;
return (XN_STATUS_OK);
}
XnStatus XnSensorIO::OpenDevice(const XnChar* strPath)
{
XnStatus nRetVal;
XnUSBDeviceSpeed DevSpeed;
nRetVal = xnUSBInit();
if (nRetVal != XN_STATUS_OK && nRetVal != XN_STATUS_USB_ALREADY_INIT)
return nRetVal;
xnLogVerbose(XN_MASK_DEVICE_IO, "Connecting to USB device...");
if (strPath == NULL || strcmp(strPath, "*:0") == 0)
{
// support old style API
XnConnectionString aConnections[1];
XnUInt32 nCount = 1;
nRetVal = EnumerateSensors(aConnections, &nCount);
if (nRetVal != XN_STATUS_OK && nRetVal != XN_STATUS_OUTPUT_BUFFER_OVERFLOW)
{
return nRetVal;
}
strPath = aConnections[0];
}
// try to open the device
xnLogVerbose(XN_MASK_DEVICE_IO, "Trying to open sensor '%s'...", strPath);
nRetVal = xnUSBOpenDeviceByPath(strPath, &m_pSensorHandle->USBDevice);
XN_IS_STATUS_OK(nRetVal);
nRetVal = xnUSBGetDeviceSpeed(m_pSensorHandle->USBDevice, &DevSpeed);
XN_IS_STATUS_OK(nRetVal);
if (DevSpeed != XN_USB_DEVICE_HIGH_SPEED)
{
XN_LOG_WARNING_RETURN(XN_STATUS_USB_UNKNOWN_DEVICE_SPEED, XN_MASK_DEVICE_IO, "Device is not high speed!");
}
// on older firmwares, control was sent over BULK endpoints. Check if this is the case
xnLogVerbose(XN_MASK_DEVICE_IO, "Trying to open endpoint 0x4 for control out (for old firmwares)...");
nRetVal = xnUSBOpenEndPoint(m_pSensorHandle->USBDevice, 0x4, XN_USB_EP_BULK, XN_USB_DIRECTION_OUT, &m_pSensorHandle->ControlConnection.ControlOutConnectionEp);
if (nRetVal == XN_STATUS_USB_ENDPOINT_NOT_FOUND || nRetVal == XN_STATUS_USB_WRONG_ENDPOINT_TYPE || nRetVal == XN_STATUS_USB_WRONG_ENDPOINT_DIRECTION)
{
// this is not the case. use regular control endpoint (0)
m_pSensorHandle->ControlConnection.bIsBulk = FALSE;
}
else
{
XN_IS_STATUS_OK(nRetVal);
xnLogVerbose(XN_MASK_DEVICE_IO, "Opening endpoint 0x85 for control in...");
nRetVal = xnUSBOpenEndPoint(m_pSensorHandle->USBDevice, 0x85, XN_USB_EP_BULK, XN_USB_DIRECTION_IN, &m_pSensorHandle->ControlConnection.ControlInConnectionEp);
XN_IS_STATUS_OK(nRetVal);
m_pSensorHandle->ControlConnection.bIsBulk = TRUE;
}
nRetVal = IsSensorLowBandwidth(strPath, &m_bIsLowBandwidth);
XN_IS_STATUS_OK(nRetVal);
xnLogInfo(XN_MASK_DEVICE_IO, "Connected to USB device%s", m_bIsLowBandwidth ? " (LowBand)" : "");
strcpy(m_strDeviceName, strPath);
return XN_STATUS_OK;
}
