static void DrawPointSet(PointSetNode *pointSet)
{
CoordinateNode *coordinate = pointSet->getCoordinateNodes();
if (!coordinate)
return;
NormalNode *normal = pointSet->getNormalNodes();
ColorNode *color = pointSet->getColorNodes();
float vpoint[3];
float pcolor[3];
glColor3f(1.0f, 1.0f, 1.0f);
glBegin(GL_POINTS);
int nCoordinatePoint = coordinate->getNPoints();
for (int n=0; n<nCoordinatePoint; n++) {
if (color) {
color->getColor(n, pcolor);
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, pcolor);
// glColor3fv(pcolor);
}
coordinate->getPoint(n, vpoint);
glVertex3fv(vpoint);
}
glEnd();
}
static void DrawIdxLineSet(IndexedLineSetNode *idxLineSet)
{
CoordinateNode *coordinate = idxLineSet->getCoordinateNodes();
if (!coordinate)
return;
NormalNode *normal = idxLineSet->getNormalNodes();
ColorNode *color = idxLineSet->getColorNodes();
int bColorPerVertex =idxLineSet->getColorPerVertex();
bool bLineBegin = true;
bool bLineClose = true;
int nLine = 0;
float vpoint[3];
float pcolor[3];
glColor3f(1.0f, 1.0f, 1.0f);
int nCoordIndexes = idxLineSet->getNCoordIndexes();
for (int nCoordIndex=0; nCoordIndex<nCoordIndexes; nCoordIndex++) {
int coordIndex = idxLineSet->getCoordIndex(nCoordIndex);
if (bLineBegin) {
glBegin(GL_LINE_STRIP);
bLineBegin = false;
bLineClose = false;
if (color && !bColorPerVertex) {
color->getColor(nLine, pcolor);
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, pcolor);
// glColor3fv(pcolor);
}
nLine++;
}
if (coordIndex != -1) {
coordinate->getPoint(coordIndex, vpoint);
glVertex3fv(vpoint);
if (color && bColorPerVertex) {
color->getColor(coordIndex, pcolor);
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, pcolor);
// glColor3fv(pcolor);
}
}
else {
glEnd();
bLineBegin = true;
bLineClose = true;
}
}
if (bLineClose == false)
glEnd();
}
ColorNode *SceneGraph::findColorNode(char *name) {
if (!name || strlen(name) <= 0)
return NULL;
for (ColorNode *node = findColorNode(); node; node = node->nextTraversal()) {
const char *nodeName = node->getName();
if (nodeName && strlen(nodeName)) {
if (!strcmp(name, nodeName))
return node;
}
}
return NULL;
}
/*----------------------------------------------------------------------------*/
void SoftKineticCamera::configureColorNode()
{
// connect new color sample handler
m_cnode.newSampleReceivedEvent().connect(&onNewColorSample);
ColorNode::Configuration config = m_cnode.getConfiguration();
config.frameFormat = FRAME_FORMAT_VGA;
config.compression = COMPRESSION_TYPE_MJPEG;
config.powerLineFrequency = POWER_LINE_FREQUENCY_50HZ;
config.framerate = 25;
m_cnode.setEnableColorMap(true);
try
{
m_context.requestControl(m_cnode,0);
m_cnode.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");
}
static ColorNode getFirstAvailableColorNode(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
ColorNode colorNode = node.as<ColorNode>();
if (colorNode.isSet())
return colorNode;
}
}
// return an unset color node
return ColorNode();
}
void PointSetNode::outputContext(std::ostream &printStream, const char *indentString) const
{
ColorNode *color = getColorNodes();
if (color != NULL) {
if (color->isInstanceNode() == false) {
if (color->getName() != NULL && strlen(color->getName()))
printStream << indentString << "\t" << "color " << "DEF " << color->getName() << " Color {" << std::endl;
else
printStream << indentString << "\t" << "color Color {" << std::endl;
color->Node::outputContext(printStream, indentString, "\t");
printStream << indentString << "\t" << "}" << std::endl;
}
else
printStream << indentString << "\t" << "color USE " << color->getName() << std::endl;
}
CoordinateNode *coord = getCoordinateNodes();
if (coord != NULL) {
if (coord->isInstanceNode() == false) {
if (coord->getName() != NULL && strlen(coord->getName()))
printStream << indentString << "\t" << "coord " << "DEF " << coord->getName() << " Coordinate {" << std::endl;
else
printStream << indentString << "\t" << "coord Coordinate {" << std::endl;
coord->Node::outputContext(printStream, indentString, "\t");
printStream << indentString << "\t" << "}" << std::endl;
}
else
printStream << indentString << "\t" << "coord USE " << coord->getName() << std::endl;
}
}
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 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;
}
static void DrawIdxFaceSet(IndexedFaceSetNode *idxFaceSet)
{
CoordinateNode *coordinateNode = idxFaceSet->getCoordinateNodes();
if (!coordinateNode)
return;
TextureCoordinateNode *texCoordNode = idxFaceSet->getTextureCoordinateNodes();
NormalNode *normalNode = idxFaceSet->getNormalNodes();
ColorNode *colorNode = idxFaceSet->getColorNodes();
bool colorPerVertex =idxFaceSet->getColorPerVertex();
bool normalPerVertex =idxFaceSet->getNormalPerVertex();
bool ccw = idxFaceSet->getCCW();
if (ccw == true)
glFrontFace(GL_CCW);
else
glFrontFace(GL_CW);
bool solid = idxFaceSet->getSolid();
if (solid == false)
glDisable(GL_CULL_FACE);
else
glEnable(GL_CULL_FACE);
bool convex = idxFaceSet->getConvex();
GLUtriangulatorObj *tessObj = NULL;
if (convex == false) {
tessObj = gluNewTess();
gluTessCallback(tessObj, GLU_BEGIN, (GLUtessCallBackFunc)glBegin);
gluTessCallback(tessObj, GLU_VERTEX, (GLUtessCallBackFunc)glVertex3dv);
gluTessCallback(tessObj, GLU_END, (GLUtessCallBackFunc)glEnd);
}
bool bPolygonBegin = true;
bool bPolygonClose = true;
int nPolygon = 0;
int nVertex = 0;
float point[3];
float vector[3];
float color[4]; color[3] = 1.0f;
float coord[2];
double (*tessPoint)[3];
if ((idxFaceSet->getColorPerVertex() && idxFaceSet->getColorNodes()) || (idxFaceSet->getNormalPerVertex() && idxFaceSet->getNormalNodes()))
glShadeModel (GL_SMOOTH);
else
glShadeModel (GL_FLAT);
int nColorIndexes = idxFaceSet->getNColorIndexes();
int nNormalIndexes = idxFaceSet->getNNormalIndexes();
int nTexCoordIndexes = idxFaceSet->getNTexCoordIndexes();
int nCoordIndexes = idxFaceSet->getNCoordIndexes();
for (int nCoordIndex=0; nCoordIndex<nCoordIndexes; nCoordIndex++) {
int coordIndex = idxFaceSet->getCoordIndex(nCoordIndex);
if (bPolygonBegin) {
if (convex == false)
gluBeginPolygon(tessObj);
else
glBegin(GL_POLYGON);
bPolygonBegin = false;
bPolygonClose = false;
int nVertices = 0;
int index = coordIndex;
while (index != -1) {
nVertices++;
int nIndex = nCoordIndex + nVertices;
if (nIndex < nCoordIndexes)
index = idxFaceSet->getCoordIndex(nIndex);
else
break;
}
if (convex == false)
tessPoint = new double[nVertices][3];
// dafault color
//glColor3f(1.0f, 1.0f, 1.0f);
// default normal
if ((nCoordIndex + 2) < nCoordIndexes) {
float point[3][3];
float normal[3];
for (int n=0; n<3; n++) {
int index = idxFaceSet->getCoordIndex(nCoordIndex+n);
coordinateNode->getPoint(index, point[n]);
}
GetNormalFromVertices(point, normal);
glNormal3fv(normal);
}
else
glNormal3f(0.0f, 0.0f, 1.0f);
if (colorNode && !colorPerVertex) {
if (0 < nColorIndexes)
colorNode->getColor(idxFaceSet->getColorIndex(nPolygon), color);
else
colorNode->getColor(nPolygon, color);
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color);
// glColor3fv(color);
}
if (normalNode && !normalPerVertex) {
if (0 < nNormalIndexes)
normalNode->getVector(idxFaceSet->getNormalIndex(nPolygon), vector);
else
normalNode->getVector(nPolygon, vector);
glNormal3fv(vector);
}
nPolygon++;
nVertex = 0;
}
if (coordIndex != -1) {
if (colorNode && colorPerVertex) {
if (0 < nColorIndexes)
colorNode->getColor(idxFaceSet->getColorIndex(nCoordIndex), color);
else
colorNode->getColor(coordIndex, color);
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color);
// glColor3fv(color);
}
if (normalNode && normalPerVertex) {
if (0 < nNormalIndexes)
normalNode->getVector(idxFaceSet->getNormalIndex(nCoordIndex), vector);
else
normalNode->getVector(coordIndex, vector);
glNormal3fv(vector);
}
if (texCoordNode) {
if (0 < nTexCoordIndexes)
texCoordNode->getPoint(idxFaceSet->getTexCoordIndex(nCoordIndex), coord);
else
texCoordNode->getPoint(coordIndex, coord);
coord[1] = 1.0f - coord[1];
glTexCoord2fv(coord);
}
coordinateNode->getPoint(coordIndex, point);
if (convex == false) {
tessPoint[nVertex][0] = point[0];
tessPoint[nVertex][1] = point[1];
tessPoint[nVertex][2] = point[2];
gluTessVertex(tessObj, tessPoint[nVertex], tessPoint[nVertex]);
}
else
glVertex3fv(point);
nVertex++;
}
else {
if (convex == false) {
gluEndPolygon(tessObj);
delete[] tessPoint;
}
else
glEnd();
bPolygonBegin = true;
bPolygonClose = true;
}
}
if (bPolygonClose == false) {
if (convex == false) {
gluEndPolygon(tessObj);
delete[] tessPoint;
}
else
glEnd();
}
if (ccw == false)
glFrontFace(GL_CCW);
if (solid == false)
glEnable(GL_CULL_FACE);
if (convex == false)
gluDeleteTess(tessObj);
glShadeModel(GL_SMOOTH);
}
static void DrawElevationGrid(ElevationGridNode *eg)
{
int xDimension = eg->getXDimension();
int zDimension = eg->getZDimension();
int nPoints = xDimension * zDimension;
float xSpacing = eg->getXSpacing();
float zSpacing = eg->getZSpacing();
int x, z;
SFVec3f *point = new SFVec3f[nPoints];
for (x=0; x<xDimension; x++) {
for (z=0; z<zDimension; z++) {
float xpos = xSpacing * x;
float ypos = eg->getHeight(x + z*zDimension);
float zpos = zSpacing * z;
point[x + z*xDimension].setValue(xpos, ypos, zpos);
}
}
ColorNode *color = eg->getColorNodes();
NormalNode *normal = eg->getNormalNodes();
TextureCoordinateNode *texCoord = eg->getTextureCoordinateNodes();
bool bColorPerVertex = eg->getColorPerVertex();
bool bNormalPerVertex = eg->getNormalPerVertex();
bool ccw = eg->getCCW();
if (ccw == true)
glFrontFace(GL_CCW);
else
glFrontFace(GL_CW);
bool solid = eg->getSolid();
if (solid == false)
glDisable(GL_CULL_FACE);
else
glEnable(GL_CULL_FACE);
glNormal3f(0.0f, 1.0f, 0.0f);
for (x=0; x<xDimension-1; x++) {
for (z=0; z<zDimension-1; z++) {
int n;
if (bColorPerVertex == false && color) {
float egColor[4]; egColor[3] = 1.0f;
color->getColor(x + z*zDimension, egColor);
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, egColor);
// glColor3fv(egColor);
}
if (bNormalPerVertex == false && normal) {
float egNormal[3];
normal->getVector(x + z*zDimension, egNormal);
glNormal3fv(egNormal);
}
float egPoint[4][3];
float egColor[4][4];
float egNormal[4][3];
float egTexCoord[4][2];
egColor[0][3] = egColor[1][3] = egColor[2][3] = egColor[3][3] = 1.0f;
for (n=0; n<4; n++) {
int xIndex = x + ((n < 2) ? 0 : 1);
int zIndex = z + (n%2);
int index = xIndex + zIndex*xDimension;
if (bColorPerVertex == true && color)
color->getColor(index, egColor[n]);
if (bNormalPerVertex == true && normal)
normal->getVector(index, egNormal[n]);
if (texCoord)
texCoord->getPoint(index, egTexCoord[n]);
else {
egTexCoord[n][0] = (float)xIndex / (float)(xDimension-1);
egTexCoord[n][1] = (float)zIndex / (float)(zDimension-1);
}
point[index].getValue(egPoint[n]);
}
glBegin(GL_POLYGON);
// Tanks for Joerg Scheurich aka MUFTI
if (!normal) {
float point[3][3];
float normal[3];
for (n=0; n<3; n++)
memcpy(point[n] , egPoint[n], sizeof(float)*3);
GetNormalFromVertices(point, normal);
glNormal3fv(normal);
}
for (n=0; n<3; n++) {
if (bColorPerVertex == true && color) {
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, egColor[n]);
// glColor3fv(egColor);
}
if (bNormalPerVertex == true && normal)
glNormal3fv(egNormal[n]);
glTexCoord2fv(egTexCoord[n]);
glVertex3fv(egPoint[n]);
}
glEnd();
glBegin(GL_POLYGON);
// Tanks for Joerg Scheurich aka MUFTI
if (!normal) {
float point[3][3];
float normal[3];
for (n=3; 0<n; n--)
memcpy(point[3-n] , egPoint[n], sizeof(float)*3);
GetNormalFromVertices(point, normal);
glNormal3fv(normal);
}
for (n=3; 0<n; n--) {
if (bColorPerVertex == true && color) {
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, egColor[n]);
// glColor3fv(egColor);
}
if (bNormalPerVertex == true && normal)
glNormal3fv(egNormal[n]);
glTexCoord2fv(egTexCoord[n]);
glVertex3fv(egPoint[n]);
}
glEnd();
}
}
if (ccw == false)
glFrontFace(GL_CCW);
if (solid == false)
glEnable(GL_CULL_FACE);
delete []point;
}
