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();
}
/** 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';
}
}
}
}