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