Ejemplo n.º 1
0
    void RosSchunk::tactileCallback(const iis_robot_dep::TactileSensor& state) {

        tactileMutex.lock();

            currentTactileReadings.clear();

            for(int i = 0; i < state.tactile_matrix.size(); ++i) {

                 TactileMatrix tactMat = state.tactile_matrix.at(i);
                 int xSize = tactMat.cells_x;
                 int ySize = tactMat.cells_y;
                 mat currentMat(xSize, ySize);

                 for(int j = 0, run = 0; j < xSize; ++j)
                     for(int k = 0; k < ySize; ++k, ++run)
                         currentMat(j, k) = tactMat.tactile_array.at(run);

                 currentTactileReadings.push_back(currentMat.t());

            }

        tactileMutex.unlock();

    }
Ejemplo n.º 2
0
    /** Save this MtlLib as a *.mtl - using the path, fileName and the provided asset-out library */
    void Obj::saveAs(const AssetOutputLibrary &assetOutputLibrary, const char* path, const char* fileName, bool saveAsMtlToo, ObjSaveModeFlags saveMode) {
	OMLOGI("Opening (obj) output stream for %s%s", path, fileName);
	std::unique_ptr<std::ostream> output = assetOutputLibrary.getAssetOutputStream(path, fileName);

	// saveAs our mtlLib if the caller wants that too and there is anything to write
	// Also the mtl lib is not saved if the saveMode does not use materials at all
	if(saveAsMtlToo && (((int)saveMode & ObjSaveModeFlags::MATERIALS_GEOMETRY) != 0)) {
		// Generate proper *.mtl file name using the provided *.obj file name
		// If there is .*obj extension, we change that to *.mtl if there were none, we just add...
		std::string fn(fileName);
		size_t lastIndex = fn.find_last_of("."); 
		std::string rawName;
		if(lastIndex == std::string::npos) {
			// There were no *.obj extension or any extension
			rawName = fn;
		} else {
			// There were an extension - remove that
			rawName = fn.substr(0, lastIndex); 
		}
		// And *.mtl ending for getting the extension
		std::string mtlFileName = rawName + ".mtl";

		// Save the *.mtl file near the *.obj
		mtlLib.saveAs(assetOutputLibrary, path, mtlFileName.c_str());
	}

	// Reference the (already existing or generated) *.mtl file(s) - if save mode saves materials
	if(((int)saveMode & ObjSaveModeFlags::MATERIALS_GEOMETRY) != 0) {
		auto mtlLibLine = mtlLib.asText();
		output->write(mtlLibLine.c_str(), mtlLibLine.length())<<'\n';
	}

	// Write out all vertex data as 'v's
	for(auto v : vs) {
		auto line = v.asText();
		output->write(line.c_str(), line.length())<<'\n';
	}
	// Write out all vertex data as 'vt's
	for(auto vt : vts) {
		auto line = vt.asText();
		output->write(line.c_str(), line.length())<<'\n';
	}
	// Write out all vertex data as 'vn's
	for(auto vn : vns) {
		auto line = vn.asText();
		output->write(line.c_str(), line.length())<<'\n';
	}


	// Write out faces, groups, materials
	// ----------------------------------

	if((int)saveMode == ObjSaveModeFlags::ONLY_UNGROUPED_GEOMETRY) {
		// Simplest case: only geometry - just write out faces
		for(auto f : fs) {
			auto line = f.asText();
			output->write(line.c_str(), line.length())<<'\n';
		}
	} else {
		// For proper compact output generation we need the objMatFaceGroups sorted
		std::map<std::string, ObjectMaterialFaceGroup> sortedObjectMaterialGroups;
		bool gbit = false;

		// The sorting key should be either matName:err:groupName or groupName:mtl:matName according to mode!
		// This ensures that those face elements we can compact together are near each other!
		for(auto kv : objectMaterialGroups) {
			if(((int)saveMode == ObjSaveModeFlags::GROUPS_GEOMETRY) ||
			   ((int)saveMode == ObjSaveModeFlags::MATERIALS_AND_GROUPS)) {
				// set the gbit here
				if(((int)saveMode & ObjSaveModeFlags::G) != 0){
					gbit = true; // indicate 'g' usage
				}

				// groupName:mtl:matName - it is already available
				// Rem.: Both above cases we need to sort by groups basically!
				std::string key = kv.first;
				sortedObjectMaterialGroups[key] = kv.second;
			} else if((int)saveMode == ObjSaveModeFlags::MATERIALS_GEOMETRY) {
				// matName:err:groupName
				// Rem.: We use :err: deliberately to force ourself handling things as it should!
				//       We cannot just save out these keys when generating real output - only used for sorting!
				std::string key = kv.second.textureDataHoldingMaterial.name + ":err:" + kv.second.objectGroupName;
				sortedObjectMaterialGroups[key] = kv.second;
			} else {
				// This should never happen - unless someone breaks ObjMaster!
				OMLOGE("Code is utterly broken! Unkown savemode!");
				exit(0);
			}
		}

		// Print out stuff that have the group or material for it
		std::string currentMat("");	// material now used - USING EMPTY STRING IS NECESSARY! SEE BELOW LOOP FOR HANDLING UNGROUPED DATA!
		std::string currentGrp("");	// group now used - USING EMPTY STRING IS NECESSARY HERE TOO!
		int minStartFaceIndex = INT_MAX; // we do a min-search to see if there are faces that does not belong to any materials or groups!
		constexpr int NO_MAT_FACE_GRP = INT_MAX; // If the minStartFaceIndex stays the INT_MAX it means there were no matFace groups at all!
		for(auto skv : sortedObjectMaterialGroups) {
			std::string &matName = skv.second.textureDataHoldingMaterial.name;
			std::string &grpName = skv.second.objectGroupName;


			// See if we need to print out groups or not - and if we need: then see if group has changed or not!
			if((((int)saveMode & ObjSaveModeFlags::GROUPS_GEOMETRY) != 0) && (currentGrp != grpName)) {
				// Write out the group
				std::string line = ObjectGroupElement::asTextO(grpName);
				if(gbit) {
					line = ObjectGroupElement::asTextG(grpName);
				}
				output->write(line.c_str(), line.length())<<'\n';

				// Erase current material (as we better always restart when the groups have changed!)
				currentMat = "";

				// Update current group
				currentGrp = grpName;
			}

			// See if we need to print out materials or not - and if we need: then see if material has changed or not!
			if((((int)saveMode & ObjSaveModeFlags::MATERIALS_GEOMETRY) != 0) && (currentMat != matName)) {
				// Write out the group
				std::string line = UseMtl::asText(matName);
				output->write(line.c_str(), line.length())<<'\n';

				// Update current group
				currentMat = matName;
			}

			// Get where is this mat-face group - in which slice
			int startFaceIndex = skv.second.faceIndex;
			// We save the minimal start index we find
			if(minStartFaceIndex > startFaceIndex) {
				minStartFaceIndex = startFaceIndex;
			}
			int faceCount = skv.second.meshFaceCount;

			// Print out the faces for this material-face group
			for(int i = 0; i < faceCount; ++i) {
				auto f = fs[startFaceIndex + i];
				auto line = f.asText();
				output->write(line.c_str(), line.length())<<'\n';
			}
		}

		// (!)There might be faces not belonging to any group or material.
		// We need to write them out somehow - and we add them to a new technical group...
		// Rem.: we could add these before any 'o', 'g' or 'usemtl' as it should be - but that would slow us down considerably...
		// Rem.: This problematic case never happens if the file is read-in with objmaster. Then the objMatFaceGroups are always
		//       generated for the non-grouped and non-materialized faces too - with an empty name. Because the empty name is
		//       the default empty setup above, that means that their faces are written out "automagically" well as it should
		//       (before any 'o', 'g', 'usemtl')! This is quite tricky, but visibly working - see the above code!
		//       The below code only handles the cases when the Obj object is manually constructed in-memory and has a bad layout.

		// The ungrouped faces start from faceNo zero, and lasts until the minimum start face of the matFace groups
		int ungroupmatFaceEndIndex = minStartFaceIndex;
		if((ungroupmatFaceEndIndex != NO_MAT_FACE_GRP) && (ungroupmatFaceEndIndex > 0)) {
			// Write out a technical group for these elements
			std::string grpName(TECHNICAL_UNKNOWN_GROUP);
			std::string line = ObjectGroupElement::asTextO(grpName);
			if(gbit) {
				line = ObjectGroupElement::asTextG(grpName);
			}
			output->write(line.c_str(), line.length())<<'\n';

			// Simples case: only geometry - just write out faces until that point
			for(int i = 0; i < ungroupmatFaceEndIndex; ++i) {
				auto f = fs[i];
				auto line = f.asText();
				output->write(line.c_str(), line.length())<<'\n';
			}
		}
	}
    }