bool do_ConvexHull(meshproc_msgs::ConvexHull::Request &req,
meshproc_msgs::ConvexHull::Response &res)
{
MeshMap::iterator itA = loadedMeshes.find(req.mesh_A);
res.mesh_A_loaded = true;
res.file_written = false;
res.result = shape_msgs::Mesh();
if(itA == loadedMeshes.end())
{
res.mesh_A_loaded = false;
return true;
}
MeshMap::iterator itR = loadedMeshes.find(req.mesh_R);
if(itR == loadedMeshes.end())
{
itR = loadedMeshes.insert(loadedMeshes.begin(),
std::pair<std::string, MeshEntry*>(req.mesh_R, new MeshEntry()));
}
MeshEntry *A, *R;
A = itA->second;
R = itR->second;
R->setFromConvexHull(*A);
if(req.return_result)
R->writeToMsg(res.result);
else
res.result = shape_msgs::Mesh();
if(req.result_to_file)
{
res.file_written = R->writeToFile(req.result_filename);
}
return true;
}
bool do_SolidifyMesh(meshproc_msgs::SolidifyMesh::Request &req,
meshproc_msgs::SolidifyMesh::Response &res)
{
bool already_had_R = true;
MeshMap::iterator itA = loadedMeshes.find(req.mesh_A);
res.mesh_A_loaded = true;
res.file_written = false;
res.result = shape_msgs::Mesh();
if(itA == loadedMeshes.end())
{
res.mesh_A_loaded = false;
return true;
}
MeshMap::iterator itR = loadedMeshes.find(req.mesh_R);
if(itR == loadedMeshes.end())
{
already_had_R = false;
itR = loadedMeshes.insert(loadedMeshes.begin(),
std::pair<std::string, MeshEntry*>(req.mesh_R, new MeshEntry()));
}
MeshEntry *A, *R;
A = itA->second;
R = itR->second;
R->setFromSolidification(*A, req.thickness);
if(req.return_result)
R->writeToMsg(res.result);
else
res.result = shape_msgs::Mesh();
if(req.result_to_file)
{
res.file_written = R->writeToFile(req.result_filename);
}
return true;
}
bool do_ProjectMesh(meshproc_msgs::ProjectMesh::Request &req,
meshproc_msgs::ProjectMesh::Response &res)
{
bool already_had_R = true;
MeshMap::iterator itA = loadedMeshes.find(req.mesh_A);
res.mesh_A_loaded = true;
res.operation_performed = false;
res.file_written = false;
res.result = shape_msgs::Mesh();
if(itA == loadedMeshes.end())
{
res.mesh_A_loaded = false;
return true;
}
MeshMap::iterator itR = loadedMeshes.find(req.mesh_R);
if(itR == loadedMeshes.end())
{
already_had_R = false;
itR = loadedMeshes.insert(loadedMeshes.begin(),
std::pair<std::string, MeshEntry*>(req.mesh_R, new MeshEntry()));
}
MeshEntry *A, *R;
A = itA->second;
R = itR->second;
res.operation_performed = R->setFromProjection(*A, req.normal.x, req.normal.y, req.normal.z, req.fill_holes);
if(!res.operation_performed)
{
if(!already_had_R)
{
delete R;
loadedMeshes.erase(req.mesh_R);
}
return true;
}
if(req.return_result_as_mesh)
R->writeToMsg(res.result);
else
res.result = shape_msgs::Mesh();
if(req.return_result_as_polygon)
{
std::vector<double> x, y, z;
R->getBoundaryPolygon(x, y, z, res.edge_L, res.edge_R);
int maxK = x.size();
for(int k = 0; k < maxK; k++)
{
geometry_msgs::Point aux;
aux.x = x[k];
aux.y = y[k];
aux.z = z[k];
res.points.push_back(aux);
}
}
if(req.result_to_file)
{
res.file_written = R->writeToFile(req.result_filename);
}
return true;
}
bool do_ConvexDecomposition(meshproc_msgs::ConvexDecomposition::Request &req,
meshproc_msgs::ConvexDecomposition::Response &res)
{
MeshMap::iterator itA = loadedMeshes.find(req.mesh_A);
res.mesh_loaded = true;
res.nr_components = 0;
res.mesh_names.clear();
res.file_written.clear();
res.components.clear();
if(itA == loadedMeshes.end())
{
res.mesh_loaded = false;
return true;
}
std::vector<MeshEntry*> components;
components.clear();
std::cerr << "Getting components" << std::endl;
itA->second->getConvexComponents(components);
std::cerr << "Components obtained" << std::endl;
int maxK = res.nr_components = components.size();
for(int k = 0; k < maxK; k++)
{
char nr_str[20];
std::sprintf(nr_str, "%d", k);
std::string cName = req.mesh_R + nr_str;
MeshMap::iterator itR = loadedMeshes.find(cName);
if(itR == loadedMeshes.end())
{
itR = loadedMeshes.insert(loadedMeshes.begin(),
std::pair<std::string, MeshEntry*>(cName, components[k]));
}
else
{
delete itR->second;
itR->second = components[k];
}
MeshEntry* R = itR->second;
res.mesh_names.push_back(cName);
if(req.return_result)
{
res.components.push_back(shape_msgs::Mesh());
R->writeToMsg(res.components[k]);
}
if(req.result_to_file)
{
res.file_written.push_back(R->writeToFile(req.result_filename + nr_str + req.result_filename_extension));
res.file_names.push_back(req.result_filename + nr_str + req.result_filename_extension);
}
}
return true;
}
bool do_GetMeshSkeleton(meshproc_msgs::GetMeshSkeleton::Request &req,
meshproc_msgs::GetMeshSkeleton::Response &res)
{
MeshMap::iterator itA = loadedMeshes.find(req.mesh_name);
res.mesh_loaded = true;
res.vertices.clear();
res.edge_ends_L.clear();
res.edge_ends_R.clear();
if(itA == loadedMeshes.end())
{
res.mesh_loaded = false;
return true;
}
std::vector<double> x, y, z;
x.clear();
y.clear();
z.clear();
itA->second->getMeshSkeleton(req.approximate, req.duplicate_distance, x, y, z, res.edge_ends_L, res.edge_ends_R);
int maxK = x.size();
res.vertices.reserve(maxK);
for(int k = 0; k < maxK; k++)
{
geometry_msgs::Point aux;
aux.x = x[k];
aux.y = y[k];
aux.z = z[k];
res.vertices.push_back(aux);
}
return true;
}
int gr_mesh_cmd(lua_State* L)
{
GRLUA_DEBUG_CALL;
gr_node_ud* data = (gr_node_ud*)lua_newuserdata(L, sizeof(gr_node_ud));
data->node = 0;
const char* name = luaL_checkstring(L, 1);
const char* obj_fname = luaL_checkstring(L, 2);
std::string sfname( obj_fname );
// Use a dictionary structure to make sure every mesh is loaded
// at most once.
auto i = mesh_map.find( sfname );
Mesh *mesh = nullptr;
if( i == mesh_map.end() ) {
mesh = new Mesh( obj_fname );
} else {
mesh = i->second;
}
data->node = new GeometryNode( name, mesh );
luaL_getmetatable(L, "gr.node");
lua_setmetatable(L, -2);
return 1;
}
bool do_GetNearMeshVertices(meshproc_msgs::GetNearMeshVertices::Request &req,
meshproc_msgs::GetNearMeshVertices::Response &res)
{
MeshMap::iterator itA = loadedMeshes.find(req.mesh_name);
res.mesh_loaded = true;
res.neighbors.clear();
if(itA == loadedMeshes.end())
{
res.mesh_loaded = false;
return true;
}
std::vector<MeshEntry::XYZTriplet> points;
points.clear();
itA->second->getNearVertices(req.point.x, req.point.y, req.point.z, req.distance, points);
int maxK = points.size();
for(int k = 0; k < maxK; k++)
{
geometry_msgs::Point aux;
aux.x = points[k].x;
aux.y = points[k].y;
aux.z = points[k].z;
res.neighbors.push_back(aux);
}
return true;
}
bool do_UnloadMesh(meshproc_msgs::UnloadMesh::Request &req, meshproc_msgs::UnloadMesh::Response &res)
{
res.unloaded_mesh = 0;
MeshMap::iterator it = loadedMeshes.find(req.mesh_name);
if(loadedMeshes.end() != it)
{
delete it->second;
res.unloaded_mesh = 1;
loadedMeshes.erase(it);
}
return true;
}
MeshEntry* checkMeshAvailability(std::string const& meshName,
meshproc_msgs::CSGRequest::Response::_mesh_A_loaded_type &isLoaded,
meshproc_msgs::CSGRequest::Response::_mesh_A_csg_safe_type &isCSGSafe)
{
isLoaded = isCSGSafe = false;
MeshMap::iterator it = loadedMeshes.find(meshName);
if(it != loadedMeshes.end())
{
isLoaded = true;
isCSGSafe = it->second->isCSGSafe();
return it->second;
}
return NULL;
}
bool do_GetMeshAABB(meshproc_msgs::GetMeshAABB::Request &req,
meshproc_msgs::GetMeshAABB::Response &res)
{
MeshMap::iterator itA = loadedMeshes.find(req.mesh_name);
res.mesh_loaded = true;
if(itA == loadedMeshes.end())
{
res.mesh_loaded = false;
return true;
}
double maxX, minX, maxY, minY, maxZ, minZ;
itA->second->getBoundingBox(maxX, minX, maxY, minY, maxZ, minZ);
res.max_x = maxX;
res.min_x = minX;
res.max_y = maxY;
res.min_y = minY;
res.max_z = maxZ;
res.min_z = minZ;
return true;
}
bool do_PathOnMesh(meshproc_msgs::PathOnMesh::Request &req,
meshproc_msgs::PathOnMesh::Response &res)
{
MeshMap::iterator itA = loadedMeshes.find(req.mesh_name);
res.mesh_loaded = true;
res.operation_performed = false;
if(itA == loadedMeshes.end())
{
res.mesh_loaded = false;
return true;
}
MeshEntry *A;
A = itA->second;
res.operation_performed = A->getGeodesicPath(req.lina_start, req.lina_end, req.linb_start, req.linb_end,
res.path);
return true;
}
bool do_GetMesh(meshproc_msgs::GetMesh::Request &req,
meshproc_msgs::GetMesh::Response &res)
{
MeshMap::iterator itA = loadedMeshes.find(req.mesh_name);
res.mesh_loaded = true;
res.file_written = false;
if(itA == loadedMeshes.end())
{
res.mesh_loaded = false;
return true;
}
if(req.return_result)
itA->second->writeToMsg(res.result);
else
res.result = shape_msgs::Mesh();
if(req.result_to_file)
{
res.file_written = itA->second->writeToFile(req.result_filename);
}
return true;
}
bool do_LoadMesh(meshproc_msgs::LoadMesh::Request &req, meshproc_msgs::LoadMesh::Response &res)
{
ROS_INFO("Loading mesh %s", req.mesh_name.c_str());
res.loaded_mesh = res.io_error = res.mesh_already_loaded = false;
MeshMap::iterator it = loadedMeshes.find(req.mesh_name);
if(it != loadedMeshes.end())
res.mesh_already_loaded = true;
else
{
if((0 == req.mesh_filenames.size()) && (0 == req.mesh_msgs.size()))
return true;
it = loadedMeshes.insert(loadedMeshes.begin(),
std::pair<std::string, MeshEntry*>(req.mesh_name, new MeshEntry()));
MeshEntry *R = it->second;
int maxK = req.mesh_filenames.size();
bool goOn = true;
for(int k = 0; (k < maxK) && goOn; k++)
{
ROS_INFO(" loading part from file %s", req.mesh_filenames[k].c_str());
goOn = R->loadFromFile(req.mesh_filenames[k], req.duplicate_dist);
}
if(!goOn)
{
ROS_INFO(" Encountered I/O error (file might not exist or is inaccessible), cancelling load.");
res.io_error = true;
delete R;
loadedMeshes.erase(it);
return true;
}
res.loaded_mesh = true;
maxK = req.mesh_msgs.size();
for(int k = 0; k < maxK; k++)
{
ROS_INFO(" loading part from message");
R->loadFromMsg(req.mesh_msgs[k], req.duplicate_dist);
}
}
ROS_INFO(" Loading done.");
return true;
}
bool do_AffineTransformMesh(meshproc_msgs::AffineTransformMesh::Request &req,
meshproc_msgs::AffineTransformMesh::Response &res)
{
MeshEntry *A, *R;
MeshMap::iterator itA = loadedMeshes.find(req.mesh_A);
MeshMap::iterator itR = loadedMeshes.find(req.mesh_R);
res.mesh_A_loaded = true;
res.file_written = false;
if(itA == loadedMeshes.end())
{
res.mesh_A_loaded = false;
return true;
}
if(itR == loadedMeshes.end())
{
itR = loadedMeshes.insert(loadedMeshes.begin(),
std::pair<std::string, MeshEntry*>(req.mesh_R, new MeshEntry()));
}
A = itA->second;
R = itR->second;
R->setFromMeshEntry(*A);
Eigen::Affine3d M = Eigen::Translation3d(req.transform.translation.x,
req.transform.translation.y,
req.transform.translation.z)*
Eigen::Quaterniond(req.transform.rotation.w,
req.transform.rotation.x,
req.transform.rotation.y,
req.transform.rotation.z);
R->applyTransform(M);
if(req.return_result)
R->writeToMsg(res.result);
else
res.result = shape_msgs::Mesh();
if(req.result_to_file)
{
res.file_written = R->writeToFile(req.result_filename);
}
return true;
}
bool do_GetMeshProps(meshproc_msgs::GetMeshProps::Request &req, meshproc_msgs::GetMeshProps::Response &res)
{
MeshMap::iterator it = loadedMeshes.find(req.mesh_name);
if(it != loadedMeshes.end())
{
res.is_loaded = true;
res.is_closed = it->second->isClosed();
res.is_manifold = it->second->isManifold();
res.is_orientable = it->second->isOrientable();
res.is_safe_for_csg = it->second->isCSGSafe();
res.num_connected_components = it->second->getNrConnectedComponents();
res.num_vertices = it->second->getNrVertices();
res.num_edges = it->second->getNrEdges();
res.num_faces = it->second->getNrFaces();
res.Euler_characteristic = it->second->getEulerCharacteristic();
res.volume = it->second->getVolume();
}
else
{
res.is_loaded = false;
}
return true;
}
bool do_SelectByNormal(meshproc_msgs::SelectByNormal::Request &req,
meshproc_msgs::SelectByNormal::Response &res)
{
MeshMap::iterator itA = loadedMeshes.find(req.mesh_A);
res.mesh_A_loaded = true;
res.file_written = false;
res.operation_performed = false;
res.result = shape_msgs::Mesh();
if(itA == loadedMeshes.end())
{
res.mesh_A_loaded = false;
return true;
}
MeshEntry *A;
A = itA->second;
res.operation_performed = A->filterByNormal(req.result_filename,
req.normal.x, req.normal.y, req.normal.z,
req.tolerated_angle);
res.file_written = res.operation_performed;
return true;
}
cv::Mat projectWithEigen()
{
// Transform meshes into camera frame
// For each frame in vector
for (int frame = 0; frame < mMeshFrameIDs.size(); frame++)
{
// Lookup current transform
Eigen::Isometry3 transform;
transform = transforms.at(mMeshFrameIDs[frame]);
// Get copy of mesh for each frame
ap::Mesh* sourceMesh;
ap::Mesh* transformedMesh;
//std::cerr << "Getting frame " << frame << " : " << mMeshFrameIDs[frame] << std::endl;
MeshMap::iterator scene_i = scenes.find(mMeshFrameIDs[frame]);
if (scenes.end() == scene_i) { continue; }
sourceMesh = scene_i->second;
MeshMap::iterator scene_t = transformedScenes.find(mMeshFrameIDs[frame]);
if (transformedScenes.end() == scene_t) { continue; }
transformedMesh = scene_t->second;
// Transform mesh into camera frame
for (int i = 0; i < sourceMesh->vertices.size(); i++)
{
Eigen::Vector3 newVertex = transform * sourceMesh->vertices[i];
//std::cerr << mesh->vertices[i].transpose() << "\t->\t" << newVertex.transpose() << std::endl;
transformedMesh->vertices[i] = newVertex;
}
}
// For each pixel in camera image
cv::Mat robotImage(mCameraModel.cameraInfo().height, mCameraModel.cameraInfo().width, CV_32F);
float* pixelPtr = (float*)robotImage.data;
float maxDepth = 0;
for (int v = 0; v < robotImage.rows; v++)
{
for (int u = 0; u < robotImage.cols; u++)
{
// Create a ray through the pixel
int pixelIdx = u + (v * robotImage.cols);
//std::cerr << "Pixel (" << u << "," << v << ")" << std::endl;
cv::Point2d pixel = cv::Point2d(u, v);
cv::Point3d cvRay = mCameraModel.projectPixelTo3dRay(pixel);
// Convert cvRay to ap::Ray
ap::Ray ray;
ray.point = Eigen::Vector3::Zero();
ray.vector.x() = cvRay.x; ray.vector.y() = cvRay.y; ray.vector.z() = cvRay.z;
ray.vector.normalize();
//std::cerr << ray.vector.transpose() << std::endl;
// For each frame in vector
for (int frame = 0; frame < mMeshFrameIDs.size(); frame++)
{
MeshMap::iterator scene_i = transformedScenes.find(mMeshFrameIDs[frame]);
if (transformedScenes.end() == scene_i)
{
continue;
}
ap::Mesh* mesh = scene_i->second;
// For each triangle in mesh
for (int i = 0; i < mesh->faces.size(); i++)
{
// Check for intersection. If finite, set distance
ap::Triangle triangle(mesh->vertices[mesh->faces[i].vertices[0]],
mesh->vertices[mesh->faces[i].vertices[1]],
mesh->vertices[mesh->faces[i].vertices[2]]);
Eigen::Vector3 intersection = ap::intersectRayTriangle(ray, triangle);
if (std::isfinite(intersection.x()))
{
float d = intersection.norm();
float val = pixelPtr[pixelIdx];
if (val == 0 || val > d)
{
pixelPtr[pixelIdx] = d;
}
if (d > maxDepth)
{
maxDepth = d;
}
}
}
}
}
}
// Return the matrix
if (maxDepth == 0) { maxDepth = 1;}
return robotImage/maxDepth;
}
bool do_CSGRequest(meshproc_msgs::CSGRequest::Request &req, meshproc_msgs::CSGRequest::Response &res)
{
res.operation_performed = false;
res.mesh_A_csg_safe = res.mesh_A_loaded = res.mesh_B_csg_safe = res.mesh_B_loaded = false;
res.result = shape_msgs::Mesh();
bool already_had_R = true;
MeshEntry *A, *B, *R;
A = checkMeshAvailability(req.mesh_A, res.mesh_A_loaded, res.mesh_A_csg_safe);
B = checkMeshAvailability(req.mesh_B, res.mesh_B_loaded, res.mesh_B_csg_safe);
MeshMap::iterator it = loadedMeshes.find(req.mesh_R);
if(it == loadedMeshes.end())
{
already_had_R = false;
it = loadedMeshes.insert(loadedMeshes.begin(),
std::pair<std::string, MeshEntry*>(req.mesh_R, new MeshEntry()));
}
R = it->second;
if(!canPerform(res.mesh_A_loaded, res.mesh_B_loaded, res.mesh_A_csg_safe, res.mesh_B_csg_safe, req.operation))
{
//Operation can't be performed, so let's not leave a result mesh (if created just now) loaded as a side-effect.
if(!already_had_R)
{
delete R;
loadedMeshes.erase(req.mesh_R);
}
return true;
}
switch(req.operation)
{
case 0:
res.operation_performed = R->setFromUnion(*A, *B);
break;
case 1:
res.operation_performed = R->setFromIntersection(*A, *B);
break;
case 2:
res.operation_performed = R->setFromDifference(*A, *B);
break;
case 3:
res.operation_performed = R->setFromSymmetricDifference(*A, *B);
break;
case 4:
res.operation_performed = R->setFromMinkowskiSum(*A, *B);
break;
case 5:
res.operation_performed = R->setFromMinkowskiErosion(*A, *B);
break;
}
if(!res.operation_performed)
{
//Operation failed, so let's not leave a result mesh (if created just now) loaded as a side-effect.
if(!already_had_R)
{
delete R;
loadedMeshes.erase(req.mesh_R);
}
return true;
}
if(req.return_result)
R->writeToMsg(res.result);
else
res.result = shape_msgs::Mesh();
if(req.result_to_file)
{
res.file_written = R->writeToFile(req.result_filename);
}
return true;
}