C++ ColorNode类代码示例

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::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);
    }
}

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);
			}
//.........这里部分代码省略.........

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) {
//.........这里部分代码省略.........

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;
}