Exemple #1
0
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;
}
Exemple #2
0
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;
}
Exemple #3
0
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;
}
Exemple #4
0
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;
}
Exemple #5
0
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;
}
Exemple #6
0
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;
}
Exemple #7
0
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;
}
Exemple #8
0
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;
}