bool do_GetLoadedMeshNames(meshproc_msgs::GetLoadedMeshNames::Request &req,
meshproc_msgs::GetLoadedMeshNames::Response &res)
{
res.mesh_names.clear();
MeshMap::iterator it = loadedMeshes.begin();
for(it = loadedMeshes.begin(); it != loadedMeshes.end(); it++)
{
res.mesh_names.push_back(it->first);
}
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_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_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_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_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;
}