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;
}
void init()
{
// Assimp::Importer importer;
const aiScene* scene = aiImportFile("/home/arprice/catkin_workspace/src/ap_robot_utils/test/resources/cube.stla",
aiProcess_ValidateDataStructure |
aiProcess_JoinIdenticalVertices |
aiProcess_ImproveCacheLocality |
aiProcess_Triangulate |
aiProcess_OptimizeGraph |
aiProcess_OptimizeMeshes |
aiProcess_FindDegenerates |
aiProcess_SortByPType);
// std::cerr << importer.GetErrorString() << std::endl;
if (scene == NULL) { return; }
scenes.insert(MeshMap::value_type("cube", meshAItoAP(scene)));
transformedScenes.insert(MeshMap::value_type("cube", meshAItoAP(scene)));
}
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 load_scene( Scene* scene, const char* filename )
{
TiXmlDocument doc( filename );
const TiXmlElement* root = 0;
const TiXmlElement* elem = 0;
MaterialMap materials;
MeshMap meshes;
TriVertMap triverts;
assert( scene );
// load the document
if ( !doc.LoadFile() ) {
std::cout << "ERROR, " << doc.ErrorRow() << ":" << doc.ErrorCol() << "; "
<< "parse error: " << doc.ErrorDesc() << "\n";
return false;
}
// check for root element
root = doc.RootElement();
if ( !root ) {
std::cout << "No root element.\n";
return false;
}
// reset the scene
scene->reset();
try {
// parse the camera
elem = get_unique_child( root, true, STR_CAMERA );
parse_camera( elem, &scene->camera );
// parse background color
parse_elem( root, true, STR_BACKGROUND, &scene->background_color );
// parse refractive index
parse_elem( root, true, STR_REFRACT, &scene->refractive_index );
// parse ambient light
parse_elem( root, false, STR_AMLIGHT, &scene->ambient_light );
// parse the lights
elem = root->FirstChildElement( STR_PLIGHT );
while ( elem ) {
PointLight pl;
parse_point_light( elem, &pl );
scene->add_light( pl );
elem = elem->NextSiblingElement( STR_PLIGHT );
}
// parse the materials
elem = root->FirstChildElement( STR_MATERIAL );
while ( elem ) {
Material* mat = new Material();
check_mem( mat );
scene->add_material( mat );
const char* name = parse_material( elem, mat );
assert( name );
// place each material in map by it's name, so we can associate geometries
// with them when loading geometries
// check for repeat name
if ( !materials.insert( std::make_pair( name, mat ) ).second ) {
print_error_header( elem );
std::cout << "Material '" << name << "' multiply defined.\n";
throw std::exception();
}
elem = elem->NextSiblingElement( STR_MATERIAL );
}
// parse the meshes
elem = root->FirstChildElement( STR_MESH );
while ( elem ) {
Mesh* mesh = new Mesh();
check_mem( mesh );
scene->add_mesh( mesh );
const char* name = parse_mesh( elem, mesh );
assert( name );
// place each mesh in map by it's name, so we can associate geometries
// with them when loading geometries
if ( !meshes.insert( std::make_pair( name, mesh ) ).second ) {
print_error_header( elem );
std::cout << "Mesh '" << name << "' multiply defined.\n";
throw std::exception();
}
elem = elem->NextSiblingElement( STR_MESH );
}
// parse vertices (used by triangles)
elem = root->FirstChildElement( STR_VERTEX );
while ( elem ) {
Triangle::Vertex v;
const char* name = parse_triangle_vertex( materials, elem, &v );
assert( name );
// place each vertex in map by it's name, so we can associate triangles
// with them when loading geometries
if ( !triverts.insert( std::make_pair( name, v ) ).second ) {
print_error_header( elem );
std::cout << "Triangle vertex '" << name << "' multiply defined.\n";
throw std::exception();
}
elem = elem->NextSiblingElement( STR_VERTEX );
}
// parse the geometries
// spheres
elem = root->FirstChildElement( STR_SPHERE );
while ( elem ) {
Sphere* geom = new Sphere();
check_mem( geom );
scene->add_geometry( geom );
parse_geom_sphere( materials, elem, geom );
elem = elem->NextSiblingElement( STR_SPHERE );
}
// triangles
elem = root->FirstChildElement( STR_TRIANGLE );
while ( elem ) {
Triangle* geom = new Triangle();
check_mem( geom );
scene->add_geometry( geom );
parse_geom_triangle( materials, triverts, elem, geom );
elem = elem->NextSiblingElement( STR_TRIANGLE );
}
// models
elem = root->FirstChildElement( STR_MODEL );
while ( elem ) {
Model* geom = new Model();
check_mem( geom );
scene->add_geometry( geom );
parse_geom_model( materials, meshes, elem, geom );
elem = elem->NextSiblingElement( STR_MODEL );
}
// TODO add you own geometries here
} catch ( std::bad_alloc const& ) {
std::cout << "Out of memory error while loading scene\n.";
scene->reset();
return false;
} catch ( ... ) {
scene->reset();
return false;
}
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;
}