XnStatus LinkContInputStream::Init(LinkControlEndpoint* pLinkControlEndpoint,
XnStreamType streamType,
XnUInt16 nStreamID,
IConnection* pConnection)
{
XnStatus nRetVal = XN_STATUS_OK;
if (m_hCriticalSection == NULL)
{
xnLogError(XN_MASK_INPUT_STREAM, "Cannot initialize - critical section was not created successfully");
XN_ASSERT(FALSE);
return XN_STATUS_ERROR;
}
xnl::AutoCSLocker csLock(m_hCriticalSection);
if (m_bInitialized)
{
//We shutdown first so we can re-initialize.
Shutdown();
}
nRetVal = LinkInputStream::Init(pLinkControlEndpoint, streamType, nStreamID, pConnection);
XN_IS_STATUS_OK_LOG_ERROR("Init base input stream", nRetVal);
m_nStreamID = nStreamID;
m_nUserBufferMaxSize = CONT_STREAM_PREDEFINED_BUFFER_SIZE;
m_nUserBufferCurrentSize = m_nWorkingBufferCurrentSize = 0;
//Allocate buffers
m_pUserBuffer = reinterpret_cast<XnUInt8*>(xnOSCallocAligned(1, m_nUserBufferMaxSize, XN_DEFAULT_MEM_ALIGN));
if (m_pUserBuffer == NULL)
{
Shutdown();
xnLogError(XN_MASK_INPUT_STREAM, "Failed to allocate buffer of size %u", m_nUserBufferMaxSize);
XN_ASSERT(FALSE);
return XN_STATUS_ALLOC_FAILED;
}
m_pWorkingBuffer = reinterpret_cast<XnUInt8*>(xnOSCallocAligned(1, CONT_STREAM_PREDEFINED_BUFFER_SIZE, XN_DEFAULT_MEM_ALIGN));
if (m_pWorkingBuffer == NULL)
{
Shutdown();
xnLogError(XN_MASK_INPUT_STREAM, "Failed to allocate buffer of size %u", m_nUserBufferMaxSize);
XN_ASSERT(FALSE);
return XN_STATUS_ALLOC_FAILED;
}
nRetVal = xnLinkGetStreamDumpName(m_nStreamID, m_strDumpName, sizeof(m_strDumpName));
if (nRetVal != XN_STATUS_OK)
{
xnLogWarning(XN_MASK_INPUT_STREAM, "Failed to get stream dump name: %s", xnGetStatusString(nRetVal));
XN_ASSERT(FALSE);
}
m_bInitialized = TRUE;
return XN_STATUS_OK;
}
XnStatus XnDeviceSensorAllocateBuffers(XnDevicePrivateData* pDevicePrivateData)
{
pDevicePrivateData->SensorHandle.DepthConnection.pUSBBuffer = (XnUInt8*)xnOSCallocAligned(XN_SENSOR_PROTOCOL_USB_BUFFER_SIZE, sizeof(XnUInt8), XN_DEFAULT_MEM_ALIGN);
pDevicePrivateData->SensorHandle.DepthConnection.nUSBBufferReadOffset = 0;
pDevicePrivateData->SensorHandle.DepthConnection.nUSBBufferWriteOffset = 0;
pDevicePrivateData->SensorHandle.ImageConnection.pUSBBuffer = (XnUInt8*)xnOSCallocAligned(XN_SENSOR_PROTOCOL_USB_BUFFER_SIZE, sizeof(XnUInt8), XN_DEFAULT_MEM_ALIGN);
pDevicePrivateData->SensorHandle.ImageConnection.nUSBBufferReadOffset = 0;
pDevicePrivateData->SensorHandle.ImageConnection.nUSBBufferWriteOffset = 0;
if (pDevicePrivateData->pSensor->IsMiscSupported())
{
pDevicePrivateData->SensorHandle.MiscConnection.pUSBBuffer = (XnUInt8*)xnOSCallocAligned(XN_SENSOR_PROTOCOL_USB_BUFFER_SIZE, sizeof(XnUInt8), XN_DEFAULT_MEM_ALIGN);
pDevicePrivateData->SensorHandle.MiscConnection.nUSBBufferReadOffset = 0;
pDevicePrivateData->SensorHandle.MiscConnection.nUSBBufferWriteOffset = 0;
}
else
{
pDevicePrivateData->SensorHandle.MiscConnection.pUSBBuffer = NULL;
}
return (XN_STATUS_OK);
}
int main(int argc, char **argv)
{
XnStatus nRetVal = XN_STATUS_OK;
memset(COM_tracker,0,15*100*sizeof(float));
if (argc > 1)
{
nRetVal = g_Context.Init();
CHECK_RC(nRetVal, "Init");
nRetVal = g_Context.OpenFileRecording(argv[1], g_Player);
if (nRetVal != XN_STATUS_OK)
{
printf("Can't open recording %s: %s\n", argv[1], xnGetStatusString(nRetVal));
return 1;
}
}
else
{
xn::EnumerationErrors errors;
nRetVal = g_Context.InitFromXmlFile(SAMPLE_XML_PATH, g_scriptNode, &errors);
if (nRetVal == XN_STATUS_NO_NODE_PRESENT)
{
XnChar strError[1024];
errors.ToString(strError, 1024);
printf("%s\n", strError);
return (nRetVal);
}
else if (nRetVal != XN_STATUS_OK)
{
printf("Open failed: %s\n", xnGetStatusString(nRetVal));
return (nRetVal);
}
}
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator);
if (nRetVal != XN_STATUS_OK)
{
printf("No depth generator found. Using a default one...");
xn::MockDepthGenerator mockDepth;
nRetVal = mockDepth.Create(g_Context);
CHECK_RC(nRetVal, "Create mock depth");
// set some defaults
XnMapOutputMode defaultMode;
defaultMode.nXRes = 320;
defaultMode.nYRes = 240;
defaultMode.nFPS = 30;
nRetVal = mockDepth.SetMapOutputMode(defaultMode);
CHECK_RC(nRetVal, "set default mode");
// set FOV
XnFieldOfView fov;
fov.fHFOV = 1.0225999419141749;
fov.fVFOV = 0.79661567681716894;
nRetVal = mockDepth.SetGeneralProperty(XN_PROP_FIELD_OF_VIEW, sizeof(fov), &fov);
CHECK_RC(nRetVal, "set FOV");
XnUInt32 nDataSize = defaultMode.nXRes * defaultMode.nYRes * sizeof(XnDepthPixel);
XnDepthPixel* pData = (XnDepthPixel*)xnOSCallocAligned(nDataSize, 1, XN_DEFAULT_MEM_ALIGN);
nRetVal = mockDepth.SetData(1, 0, nDataSize, pData);
CHECK_RC(nRetVal, "set empty depth map");
g_DepthGenerator = mockDepth;
}
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_IMAGE, g_ImageGenerator);
if (nRetVal != XN_STATUS_OK)
{
printf("No image node exists! check your XML.");
return 1;
}
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator);
if (nRetVal != XN_STATUS_OK)
{
nRetVal = g_UserGenerator.Create(g_Context);
CHECK_RC(nRetVal, "Find user generator");
}
XnCallbackHandle hUserCallbacks;
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON))
{
printf("Supplied user generator doesn't support skeleton\n");
return 1;
}
nRetVal = g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks);
CHECK_RC(nRetVal, "Register to user callbacks");
nRetVal = g_Context.StartGeneratingAll();
CHECK_RC(nRetVal, "StartGenerating");
#ifndef USE_GLES
glInit(&argc, argv);
glutMainLoop();
#else
if (!opengles_init(GL_WIN_SIZE_X, GL_WIN_SIZE_Y, &display, &surface, &context))
{
printf("Error initializing opengles\n");
CleanupExit();
}
glDisable(GL_DEPTH_TEST);
glEnable(GL_TEXTURE_2D);
glEnableClientState(GL_VERTEX_ARRAY);
glDisableClientState(GL_COLOR_ARRAY);
while (!g_bQuit)
{
glutDisplay();
eglSwapBuffers(display, surface);
}
opengles_shutdown(display, surface, context);
CleanupExit();
#endif
}
OpenNITrackerLoopback (ros::NodeHandle &nh, int argc, char **argv)
: g_bNeedPose (false)
, nh_ (nh)
, argc_ (argc), argv_(argv)
{
g_strPose[0] = 0x0;
XnStatus nRetVal = XN_STATUS_OK;
xn::EnumerationErrors errors;
// TODO: Matteo had: "/opt/ros/diamondback/ros/BodyTracker/SamplesConfig.xml"
std::string configFilename = ros::package::getPath("realtime_urdf_filter") + "/include/SamplesConfig.xml";
nRetVal = g_Context.InitFromXmlFile(configFilename.c_str(), g_scriptNode, &errors);
if (nRetVal == XN_STATUS_NO_NODE_PRESENT)
{
XnChar strError[1024];
errors.ToString(strError, 1024);
printf("%s\n", strError);
// TODO: return (nRetVal);
}
else if (nRetVal != XN_STATUS_OK)
{
printf("Open failed: %s\n", xnGetStatusString(nRetVal));
// TODO: return (nRetVal);
}
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator);
// TODO: can we remove this?
if (nRetVal != XN_STATUS_OK)
{
printf("No depth generator found. Using a default one...");
xn::MockDepthGenerator _mockDepth;
nRetVal = mockDepth.Create(g_Context);
CHECK_RC(nRetVal, "Create mock depth");
// set some defaults
XnMapOutputMode defaultMode;
defaultMode.nXRes = 320;
defaultMode.nYRes = 240;
defaultMode.nFPS = 30;
nRetVal = mockDepth.SetMapOutputMode(defaultMode);
CHECK_RC(nRetVal, "set default mode");
// set FOV
XnFieldOfView fov;
fov.fHFOV = 1.0225999419141749;
fov.fVFOV = 0.79661567681716894;
nRetVal = mockDepth.SetGeneralProperty(XN_PROP_FIELD_OF_VIEW, sizeof(fov), &fov);
CHECK_RC(nRetVal, "set FOV");
XnUInt32 nDataSize = defaultMode.nXRes * defaultMode.nYRes * sizeof(XnDepthPixel);
XnDepthPixel* pData = (XnDepthPixel*)xnOSCallocAligned(nDataSize, 1, XN_DEFAULT_MEM_ALIGN);
nRetVal = mockDepth.SetData(1, 0, nDataSize, pData);
CHECK_RC(nRetVal, "set empty depth map");
g_DepthGenerator = mockDepth;
}
// by Heresy, create mock node
mockDepth.CreateBasedOn( g_DepthGenerator, "mock-depth" );
g_DepthGenerator.GetMetaData(depthMD);
// by Heresy, create an user generator fot mock depth node
xn::Query xQuery;
xQuery.AddNeededNode( "mock-depth" );
nRetVal = g_UserGenerator.Create( g_Context, &xQuery );
CHECK_RC(nRetVal, "Find user generator");
XnCallbackHandle hUserCallbacks, hCalibrationComplete, hPoseDetected;
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON))
{
printf("Supplied user generator doesn't support skeleton\n");
// TODO: return 1;
}
nRetVal = g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, this, hUserCallbacks);
CHECK_RC(nRetVal, "Register to user callbacks");
nRetVal = g_UserGenerator.GetSkeletonCap().RegisterToCalibrationComplete(UserCalibration_CalibrationComplete, this, hCalibrationComplete);
CHECK_RC(nRetVal, "Register to calibration complete");
if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration())
{
g_bNeedPose = TRUE;
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION))
{
printf("Pose required, but not supported\n");
// TODO: return 1;
}
nRetVal = g_UserGenerator.GetPoseDetectionCap().RegisterToPoseDetected(UserPose_PoseDetected, this, hPoseDetected);
CHECK_RC(nRetVal, "Register to Pose Detected");
g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose);
}
g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
nRetVal = g_Context.StartGeneratingAll();
CHECK_RC(nRetVal, "StartGenerating");
tf_pub_ = nh_.advertise<tf::tfMessage> ("/tf_human", 1);
event_pub_ = nh_.advertise<std_msgs::String> ("user_event", 1);
setupURDFSelfFilter ();
}
UserTracker::UserTracker(int argc, char **argv, XnUInt64 timeSpanForExitPose) : m_bValid(FALSE),
m_timeSpanForExitPose(timeSpanForExitPose),
m_pExitPoseDetector(NULL)
{
m_bRecord=FALSE;
XnStatus nRetVal = XN_STATUS_OK;
XnBool OpenedRecording=FALSE;
if(argc>1)
{
for(int i=1; i<argc-1; i++)
{
if(xnOSStrCmp(argv[i],"-RecordingFilename")==0)
{
// initialize the context from scratch
nRetVal = m_Context.Init();
CHECK_RC(nRetVal, "Context Initialization");
// try to open the recording as the source for sensor data
nRetVal = m_Context.OpenFileRecording(argv[i+1], m_Player);
CHECK_RC(nRetVal, "Couldn't open recording with command line argument");
OpenedRecording=TRUE;
break; // we only support one recording at a time..
}
}
}
if(OpenedRecording==FALSE)
{
xn::EnumerationErrors errors;
nRetVal = m_Context.InitFromXmlFile(SAMPLE_XML_PATH, m_ScriptNode, &errors);
if (nRetVal == XN_STATUS_NO_NODE_PRESENT)
{
XnChar strError[1024];
errors.ToString(strError, 1024);
printf("%s\n", strError);
return;
}
CHECK_RC(nRetVal, "Open XML failed");
}
nRetVal = m_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, m_DepthGenerator);
if (nRetVal != XN_STATUS_OK)
{
printf("No depth generator found. Using a default one...");
xn::MockDepthGenerator mockDepth;
nRetVal = mockDepth.Create(m_Context);
CHECK_RC(nRetVal, "Create mock depth");
// set some defaults
XnMapOutputMode defaultMode;
defaultMode.nXRes = 320;
defaultMode.nYRes = 240;
defaultMode.nFPS = 30;
nRetVal = mockDepth.SetMapOutputMode(defaultMode);
CHECK_RC(nRetVal, "set default mode");
// set FOV
XnFieldOfView fov;
fov.fHFOV = 1.0225999419141749;
fov.fVFOV = 0.79661567681716894;
nRetVal = mockDepth.SetGeneralProperty(XN_PROP_FIELD_OF_VIEW, sizeof(fov), &fov);
CHECK_RC(nRetVal, "set FOV");
XnUInt32 nDataSize = defaultMode.nXRes * defaultMode.nYRes * sizeof(XnDepthPixel);
XnDepthPixel* pData = (XnDepthPixel*)xnOSCallocAligned(nDataSize, 1, XN_DEFAULT_MEM_ALIGN);
nRetVal = mockDepth.SetData(1, 0, nDataSize, pData);
CHECK_RC(nRetVal, "set empty depth map");
m_DepthGenerator = mockDepth;
}
nRetVal = m_Context.FindExistingNode(XN_NODE_TYPE_USER, m_UserGenerator);
if (nRetVal != XN_STATUS_OK)
{
nRetVal = m_UserGenerator.Create(m_Context);
CHECK_RC(nRetVal, "Find user generator");
}
if (!m_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON))
{
printf("Supplied user generator doesn't support skeleton\n");
return;
}
m_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
nRetVal = m_Context.StartGeneratingAll();
CHECK_RC(nRetVal, "StartGenerating");
m_pExitPoseDetector=XN_NEW(ExitPoseDetector,m_UserGenerator);
if(!(m_pExitPoseDetector->Valid()))
{
printf("Exit pose is not supported by user node. Therefore will continue to run without support for exit pose\n");
XN_DELETE(m_pExitPoseDetector);
m_pExitPoseDetector=NULL;
}
m_bValid=TRUE;
}
int main(int argc, char **argv)
{
setup(argc, argv, &g_UserGenerator);
XnStatus nRetVal = XN_STATUS_OK;
std::string configFilename = ros::package::getPath("user_tracker") + "/config/SamplesConfig.xml";
xn::EnumerationErrors errors;
nRetVal = g_Context.InitFromXmlFile(configFilename.c_str(), g_scriptNode, &errors);
if (nRetVal == XN_STATUS_NO_NODE_PRESENT)
{
XnChar strError[1024];
errors.ToString(strError, 1024);
ROS_INFO("%s\n", strError);
return (nRetVal);
}
else if (nRetVal != XN_STATUS_OK)
{
ROS_INFO("Open failed: %s\n", xnGetStatusString(nRetVal));
return (nRetVal);
}
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator);
if (nRetVal != XN_STATUS_OK)
{
ROS_INFO("No depth generator found. Using a default one...");
xn::MockDepthGenerator mockDepth;
nRetVal = mockDepth.Create(g_Context);
CHECK_RC(nRetVal, "Create mock depth");
// set some defaults
XnMapOutputMode defaultMode;
defaultMode.nXRes = 320;
defaultMode.nYRes = 240;
defaultMode.nFPS = 30;
nRetVal = mockDepth.SetMapOutputMode(defaultMode);
CHECK_RC(nRetVal, "set default mode");
// set FOV
XnFieldOfView fov;
fov.fHFOV = 1.0225999419141749;
fov.fVFOV = 0.79661567681716894;
nRetVal = mockDepth.SetGeneralProperty(XN_PROP_FIELD_OF_VIEW, sizeof(fov), &fov);
CHECK_RC(nRetVal, "set FOV");
XnUInt32 nDataSize = defaultMode.nXRes * defaultMode.nYRes * sizeof(XnDepthPixel);
XnDepthPixel* pData = (XnDepthPixel*)xnOSCallocAligned(nDataSize, 1, XN_DEFAULT_MEM_ALIGN);
nRetVal = mockDepth.SetData(1, 0, nDataSize, pData);
CHECK_RC(nRetVal, "set empty depth map");
g_DepthGenerator = mockDepth;
}
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator);
if (nRetVal != XN_STATUS_OK)
{
nRetVal = g_UserGenerator.Create(g_Context);
CHECK_RC(nRetVal, "Find user generator");
}
XnCallbackHandle hUserCallbacks, hCalibrationStart, hCalibrationComplete, hPoseDetected, hCalibrationInProgress, hPoseInProgress;
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON))
{
ROS_INFO("Supplied user generator doesn't support skeleton\n");
return 1;
}
nRetVal = g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks);
CHECK_RC(nRetVal, "Register to user callbacks");
nRetVal = g_UserGenerator.GetSkeletonCap().RegisterToCalibrationStart(UserCalibration_CalibrationStart, NULL, hCalibrationStart);
CHECK_RC(nRetVal, "Register to calibration start");
nRetVal = g_UserGenerator.GetSkeletonCap().RegisterToCalibrationComplete(UserCalibration_CalibrationComplete, NULL, hCalibrationComplete);
CHECK_RC(nRetVal, "Register to calibration complete");
if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration())
{
g_bNeedPose = TRUE;
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION))
{
ROS_INFO("Pose required, but not supported\n");
return 1;
}
nRetVal = g_UserGenerator.GetPoseDetectionCap().RegisterToPoseDetected(UserPose_PoseDetected, NULL, hPoseDetected);
CHECK_RC(nRetVal, "Register to Pose Detected");
g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose);
}
g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
nRetVal = g_UserGenerator.GetSkeletonCap().RegisterToCalibrationInProgress(MyCalibrationInProgress, NULL, hCalibrationInProgress);
CHECK_RC(nRetVal, "Register to calibration in progress");
nRetVal = g_UserGenerator.GetPoseDetectionCap().RegisterToPoseInProgress(MyPoseInProgress, NULL, hPoseInProgress);
CHECK_RC(nRetVal, "Register to pose in progress");
nRetVal = g_Context.StartGeneratingAll();
CHECK_RC(nRetVal, "StartGenerating");
#ifndef USE_GLES
glInit(&argc, argv);
glutMainLoop();
#else
if (!opengles_init(GL_WIN_SIZE_X, GL_WIN_SIZE_Y, &display, &surface, &context))
{
ROS_INFO("Error initializing opengles\n");
CleanupExit();
}
glDisable(GL_DEPTH_TEST);
glEnable(GL_TEXTURE_2D);
glEnableClientState(GL_VERTEX_ARRAY);
glDisableClientState(GL_COLOR_ARRAY);
while (!g_bQuit)
{
glutDisplay();
eglSwapBuffers(display, surface);
}
opengles_shutdown(display, surface, context);
CleanupExit();
#endif
}
