static DepthNode getFirstAvailableDepthNode(Context context)
{
// obtain the list of devices attached to the host
vector<Device> devices = context.getDevices();
for (vector<Device>::const_iterator iter = devices.begin(); iter != devices.end(); iter++)
{
Device device = *iter;
// obtain the list of nodes of the current device
vector<Node> nodes = device.getNodes();
for (vector<Node>::const_iterator nodeIter = nodes.begin(); nodeIter != nodes.end(); nodeIter++)
{
Node node = *nodeIter;
// if the node is a DepthSense::ColorNode, return it
DepthNode colorNode = node.as<DepthNode>();
if (colorNode.isSet())
return colorNode;
}
}
// return an unset color node
return DepthNode();
}
/*----------------------------------------------------------------------------*/
void configureDepthNode()
{
g_dnode.newSampleReceivedEvent().connect(&onNewDepthSample);
DepthNode::Configuration config = g_dnode.getConfiguration();
config.frameFormat = FRAME_FORMAT_QVGA;
config.framerate = 25;
config.mode = DepthNode::CAMERA_MODE_CLOSE_MODE;
config.saturation = true;
g_dnode.setEnableVertices(true);
g_dnode.setEnableDepthMapFloatingPoint(true);
try
{
g_context.requestControl(g_dnode,0);
g_dnode.setConfiguration(config);
}
catch (ArgumentException& e)
{
printf("Argument Exception: %s\n",e.what());
}
catch (UnauthorizedAccessException& e)
{
printf("Unauthorized Access Exception: %s\n",e.what());
}
catch (IOException& e)
{
printf("IO Exception: %s\n",e.what());
}
catch (InvalidOperationException& e)
{
printf("Invalid Operation Exception: %s\n",e.what());
}
catch (ConfigurationException& e)
{
printf("Configuration Exception: %s\n",e.what());
}
catch (StreamingException& e)
{
printf("Streaming Exception: %s\n",e.what());
}
catch (TimeoutException&)
{
printf("TimeoutException\n");
}
}
/*----------------------------------------------------------------------------*/
void pcl::SoftKineticDevice::onNodeDisconnected(Device device, Device::NodeRemovedData data)
{
// if (data.node.is<AudioNode>() && (data.node.as<AudioNode>() == g_anode))
// g_anode.unset();
if (data.node.is<ColorNode>() && (data.node.as<ColorNode>() == g_cnode))
g_cnode.unset();
if (data.node.is<DepthNode>() && (data.node.as<DepthNode>() == g_dnode))
g_dnode.unset();
printf("Node disconnected\n");
}
/*----------------------------------------------------------------------------*/
void SoftKineticCamera::onNodeDisconnected(Device device, Device::NodeRemovedData data)
{
if (data.node.is<AudioNode>() && (data.node.as<AudioNode>() == m_anode))
m_anode.unset();
if (data.node.is<ColorNode>() && (data.node.as<ColorNode>() == m_cnode))
m_cnode.unset();
if (data.node.is<DepthNode>() && (data.node.as<DepthNode>() == m_dnode))
m_dnode.unset();
printf("Node disconnected\n");
}
void
pcl::SoftKineticDevice::close ()
{
//pp_.Close ();
if (context_.isSet())
{
context_.quit();
skrun_thread_.join();
context_.stopNodes();
if (g_cnode.isSet()) { context_.unregisterNode(g_cnode); g_cnode.unset(); }
if (g_dnode.isSet()) { context_.unregisterNode(g_dnode); g_dnode.unset(); }
//if (g_anode.isSet()) context_.unregisterNode(g_anode);
if (g_pProjHelper) {
delete g_pProjHelper;
g_pProjHelper = NULL;
}
context_.unset();
}
}
/*----------------------------------------------------------------------------*/
int main(int argc, char* argv[])
{
ros::init (argc, argv, "pub_pcl");
ros::NodeHandle nh;
pub = nh.advertise<PointCloud> ("points2", 1);
point_cloud::PointCloud detector("pcl_class1"); //creates PointCloudLab class object
int user_input = 1;
g_context = Context::create("localhost");
g_context.deviceAddedEvent().connect(&onDeviceConnected);
g_context.deviceRemovedEvent().connect(&onDeviceDisconnected);
// Get the list of currently connected devices
vector<Device> da = g_context.getDevices();
// We are only interested in the first device
if (da.size() >= 1)
{
g_bDeviceFound = true;
da[0].nodeAddedEvent().connect(&onNodeConnected);
da[0].nodeRemovedEvent().connect(&onNodeDisconnected);
vector<Node> na = da[0].getNodes();
printf("Found %u nodes\n",na.size());
for (int n = 0; n < (int)na.size(); n++)
configureNode(na[n]);
}
while(user_input==1) {
g_context.startNodes();
g_context.run();
g_context.stopNodes();
cout << "Please enter an 1 or 0: ";
cin >> user_input;
}
if (g_cnode.isSet()) g_context.unregisterNode(g_cnode);
if (g_dnode.isSet()) g_context.unregisterNode(g_dnode);
if (g_anode.isSet()) g_context.unregisterNode(g_anode);
if (g_pProjHelper)
delete g_pProjHelper;
return 0;
}
/*----------------------------------------------------------------------------*/
void SoftKineticCamera::configureDepthNode()
{
m_dnode.newSampleReceivedEvent().connect(&onNewDepthSample);
DepthNode::Configuration config = m_dnode.getConfiguration();
config.frameFormat = FRAME_FORMAT_QVGA;
config.framerate = 25;
config.mode = DepthNode::CAMERA_MODE_CLOSE_MODE;
config.saturation = true;
m_dnode.setEnableDepthMap(true);
try
{
m_context.requestControl(m_dnode,0);
m_dnode.setEnableConfidenceMap(true);
m_dnode.setConfidenceThreshold(120);
m_dnode.setConfiguration(config);
}
catch (ArgumentException& e)
{
printf("Argument Exception: %s\n",e.what());
}
catch (UnauthorizedAccessException& e)
{
printf("Unauthorized Access Exception: %s\n",e.what());
}
catch (IOException& e)
{
printf("IO Exception: %s\n",e.what());
}
catch (InvalidOperationException& e)
{
printf("Invalid Operation Exception: %s\n",e.what());
}
catch (ConfigurationException& e)
{
printf("Configuration Exception: %s\n",e.what());
}
catch (StreamingException& e)
{
printf("Streaming Exception: %s\n",e.what());
}
catch (TimeoutException&)
{
printf("TimeoutException\n");
}
}
/*----------------------------------------------------------------------------*/
void pcl::SoftKineticDevice::configureNode(Node node)
{
if ((node.is<DepthNode>())&&(!g_dnode.isSet()))
{
g_dnode = node.as<DepthNode>();
configureDepthNode();
context_.registerNode(node);
}
if ((node.is<ColorNode>())&&(!g_cnode.isSet()))
{
g_cnode = node.as<ColorNode>();
configureColorNode();
context_.registerNode(node);
}
// if ((node.is<AudioNode>())&&(!g_anode.isSet()))
// {
// g_anode = node.as<AudioNode>();
// configureAudioNode();
// context_.registerNode(node);
// }
}
/*----------------------------------------------------------------------------*/
void SoftKineticCamera::configureNode(Node node)
{
if ((node.is<DepthNode>())&&(!m_dnode.isSet()))
{
m_dnode = node.as<DepthNode>();
configureDepthNode();
m_context.registerNode(node);
}
if ((node.is<ColorNode>())&&(!m_cnode.isSet()))
{
m_cnode = node.as<ColorNode>();
configureColorNode();
m_context.registerNode(node);
}
if ((node.is<AudioNode>())&&(!m_anode.isSet()))
{
m_anode = node.as<AudioNode>();
configureAudioNode();
m_context.registerNode(node);
}
}
/*----------------------------------------------------------------------------*/
void configureNode(Node node)
{
if ((node.is<DepthNode>())&&(!g_dnode.isSet()))
{
g_dnode = node.as<DepthNode>();
configureDepthNode();
g_context.registerNode(node);
}
if ((node.is<ColorNode>())&&(!g_cnode.isSet()))
{
g_cnode = node.as<ColorNode>();
configureColorNode();
g_context.registerNode(node);
}
if ((node.is<AudioNode>())&&(!g_anode.isSet()))
{
g_anode = node.as<AudioNode>();
configureAudioNode();
g_context.registerNode(node);
}
}
bool SoftKineticCamera::Init() {
m_context = Context::create("localhost");
m_context.deviceAddedEvent().connect(&SoftKineticCamera::onDeviceConnected);
m_context.deviceRemovedEvent().connect(&SoftKineticCamera::onDeviceDisconnected);
// Get the list of currently connected devices
vector<Device> devices = m_context.getDevices();
if (devices.size()==0)
{
return false;
}
//Assume that there is only one device
m_bDeviceFound=true;
m_device=devices[0];
m_device.nodeAddedEvent().connect(&SoftKineticCamera::onNodeConnected);
m_device.nodeRemovedEvent().connect(&SoftKineticCamera::onNodeDisconnected);
vector<Node> nodes = m_device.getNodes();
for (int n = 0; n < (int)nodes.size();n++) {
configureNode(nodes[n]);
}
m_context.startNodes();
//Allocate memory to depth and color frames.
int32_t colorWidth,colorHeight;
int32_t depthWidth,depthHeight;
FrameFormat_toResolution(m_cnode.getConfiguration().frameFormat,&colorWidth,&colorHeight);
FrameFormat_toResolution(m_dnode.getConfiguration().frameFormat,&depthWidth,&depthHeight);
sizeOfDepthMap=depthWidth*depthHeight*sizeof(CAMERA_DEPTH_TYPE);
m_depthMap=new CAMERA_DEPTH_TYPE[sizeOfDepthMap];
sizeOfRGBMap=colorWidth*colorHeight*sizeof(CAMERA_RGB_TYPE);
m_rgbMap=new uint8_t[sizeOfRGBMap];
this->colorWidth=colorWidth;
this->colorHeight=colorHeight;
cutilSafeCall(cudaMalloc((void**)&m_cudaRGBMap, sizeOfRGBMap));
cutilSafeCall(cudaMalloc((void**)&m_cudaDepthMap, sizeOfDepthMap));
tthread::thread eventThread(&runEvents,0);
eventThread.detach();
SetParams(SOFTKINETIC_FOH_RADIANS, SOFTKINETIC_FOV_RADIANS,depthWidth,depthHeight);
m_params.m_z_offset = 0.f;
m_params.m_min_depth = SOFTKINETIC_MIN_DEPTH_MM;
m_params.m_max_depth = SOFTKINETIC_MAX_DEPTH_MM;
m_params.k1 = SOFTKINETIC_K1;
m_params.k2 = SOFTKINETIC_K2;
m_params.p1 = SOFTKINETIC_P1;
m_params.p2 = SOFTKINETIC_P2;
m_params.depthNullValue = SOFTKINETIC_DEPTH_NULL_VALUE;
CameraAbstract::Init();
return true;
}