ros-industrial / noether

Tool path planning and surface segmenter
113 stars 44 forks source link

Issues generating paths for a subset of mesh faces #149

Open rr-tom-noble opened 2 years ago

rr-tom-noble commented 2 years ago

Hi,

I'm currently trying to use noether to generate paths over a subset of faces on a mesh. The faces are chosen through a tool I've developed in rviz, which then extracts those faces from the Mesh (represented as a vtkPolyData object) and converts them into a shape_msgs/Mesh message. this mesh selection is eventually passed to noether's plane slicer and edge path planners. This is the code I'm using to generate the message.

shape_msgs::Mesh Mesh::toShapeMsgsMesh(std::vector<int> faces)
{
    std::vector<int> processed_points;
    shape_msgs::Mesh mesh;
    for (int i : faces)
    {
        shape_msgs::MeshTriangle triangle;
        vtkIdList* face = vtkIdList::New();
        mesh_->GetCellPoints(i, face);
        for (int j = 0; j < 3; j++)
        {
            int vertex_id = face->GetId(j);
            if (std::find(processed_points.begin(), processed_points.end(), vertex_id) != processed_points.end())
                continue;
            double* point = mesh_->GetPoint(vertex_id);
            geometry_msgs::Point vertex;
            vertex.x = point[0];
            vertex.y = point[1];
            vertex.z = point[2];
            mesh.vertices.push_back(vertex);
            processed_points.push_back(vertex_id);
        }
        triangle.vertex_indices = {face->GetId(0), face->GetId(1), face->GetId(2)};
        mesh.triangles.push_back(triangle);
    }
    return mesh;
}

mesh_ is the vtkPolyData object containing all faces, and faces is the vector of selected face indexes.

When I attempt to generate a plane slice path over a selection, the message is passed to noether which then crashes. I don't think it's an issue with the message building or noether calling code as, when I select every face on the mesh, the planning works fine. It's just when using a subset of faces that it breaks.

Any help would be much appreciated!

marip8 commented 2 years ago

At face value, it seems like the vertex indices of your triangles may not be correct. Those indices need to correspond to the index of the vertex in mesh.vertices not to the ID that VTK decided to assign the vertex

If that isn't the case, you'll need to provide more details about the crash you encountered; there's not much we can say about the root cause with the information in the post. I would suggest attaching a debugger to the process and finding the specific line at which the crash occurs and what the state of the variables are there

rr-tom-noble commented 2 years ago

This was the dodgy line of code:

triangle.vertex_indices = {face->GetId(0), face->GetId(1), face->GetId(2)};

Since face->GetId(x) is the index into vtkPolyData vertex list, which contains all vertices. Since the selection contains a subset of vertices, the indices needed to be updated to point to the correct vertex in mesh.vertices

This fixes the crashing, however, I'm still unable to get noether to successfully generate a path for the selection:

[ INFO] [1627480014.856222657] [/toolpath_planner]: Handling toolpath plan request
[ INFO] [1627480014.857773656] [/toolpath_planner]: Loading mesh
[ INFO] [1627480014.857796108] [/toolpath_planner]: Loading mesh from service request
[ INFO] [1627480014.857847673] [/toolpath_planner]: Loading parameters
[ INFO] [1627480014.857946465] [/toolpath_planner]: Generating surface paths
[ INFO] [1627480014.858397418] [/tool_path_planner_server]: Starting path generation
[ INFO] [1627480014.858957040] [/tool_path_planner_server]: Planning path
Warning: tool_path_planner::PlaneSlicerRasterGenerator generating normal data
         at line 400 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
Warning: tool_path_planner::PlaneSlicerRasterGenerator FindPointsWithinRadius found no points for normal averaging, using closests
         at line 669 in /home/rob/checkout/external/src/noether/tool_path_planner/src/plane_slicer_raster_generator.cpp
[ INFO] [1627480014.889264177] [/tool_path_planner_server]: Surface 1 processed successfully
[ INFO] [1627480014.889615626] [/toolpath_planner]: Generating edge paths
[ INFO] [1627480014.889929894] [/tool_path_planner_server]: Starting path generation
[ INFO] [1627480014.889960505] [/tool_path_planner_server]: Planning path
Error:   No valid edge segments were found, original mesh may have duplicate vertices
         at line 579 in /home/rob/checkout/external/src/noether/tool_path_planner/src/halfedge_edge_generator.cpp
[ERROR] [1627480014.903492662] [/tool_path_planner_server]: Path planning on surface 1 failed
[ERROR] [1627480014.903696790] [/toolpath_planner]: Failed to generate edges from mesh

Below is the selection of the mesh I'm using

selection

For reference, the radius of the selection above is around 10 units, and the point spacing for edge generation I'm using is 1 unit

marip8 commented 2 years ago

What is the value of the search_radius parameter that you're using? The default value is 0.01 m which is much smaller than 10 and 1 "units". For reference, mesh units are always interpreted in meters in this repository because the supported mesh files do not encode unit specification (as far as I know)

rr-tom-noble commented 2 years ago

Hey and thanks for the response. Search radius was set to 2 for the plane slice planner, which is working fine. Does the half edge generator also use one? (my code uses both planners)

I've exported and uploaded the selection here too: https://filebin.net/cq69pc3dyuueyzcw

rr-tom-noble commented 2 years ago

Will check the mesh for duplicate vertices. Don't think there should be any. This is the updated code generating the mesh message:

shape_msgs::Mesh Mesh::toShapeMsgsMesh(std::vector<int> faces)
{
    std::map<int, int> vertex_id_mapping;
    shape_msgs::Mesh mesh;
    for (int i : faces)
    {
        shape_msgs::MeshTriangle triangle;
        vtkIdList* face = vtkIdList::New();
        mesh_->GetCellPoints(i, face);
        for (int j = 0; j < 3; j++)
        {
            int vertex_id = face->GetId(j);
            if (vertex_id_mapping.find(vertex_id) != vertex_id_mapping.end())
            {
                triangle.vertex_indices[j] = vertex_id_mapping[vertex_id];
                continue;
            }
            double* point = mesh_->GetPoint(vertex_id);
            geometry_msgs::Point vertex;
            vertex.x = point[0];
            vertex.y = point[1];
            vertex.z = point[2];
            mesh.vertices.push_back(vertex);
            vertex_id_mapping[vertex_id] = mesh.vertices.size() - 1;
            triangle.vertex_indices[j] = vertex_id_mapping[vertex_id];
        }
        mesh.triangles.push_back(triangle);
    }
    return mesh;
}

These lines should prevent any vertices from being added twice

if (vertex_id_mapping.find(vertex_id) != vertex_id_mapping.end())
{
    triangle.vertex_indices[j] = vertex_id_mapping[vertex_id];
    continue;
}
rr-tom-noble commented 2 years ago

As in the above issue, I've verified that the selection does not contain duplicate vertices or faces using the following code

shape_msgs::Mesh Mesh::toShapeMsgsMesh(std::vector<int> faces)
{
    std::map<int, int> vertex_id_mapping;
    shape_msgs::Mesh mesh;
    for (int i : faces)
    {
        shape_msgs::MeshTriangle triangle;
        vtkIdList* face = vtkIdList::New();
        mesh_->GetCellPoints(i, face);
        for (int j = 0; j < 3; j++)
        {
            int vertex_id = face->GetId(j);
            if (vertex_id_mapping.find(vertex_id) != vertex_id_mapping.end())
            {
                triangle.vertex_indices[j] = vertex_id_mapping[vertex_id];
                continue;
            }
            double* point = mesh_->GetPoint(vertex_id);
            geometry_msgs::Point vertex;
            vertex.x = point[0];
            vertex.y = point[1];
            vertex.z = point[2];
            mesh.vertices.push_back(vertex);
            vertex_id_mapping[vertex_id] = mesh.vertices.size() - 1;
            triangle.vertex_indices[j] = vertex_id_mapping[vertex_id];
        }
        mesh.triangles.push_back(triangle);
    }
    for (int i = 0; i < mesh.vertices.size(); i++)
    {
        for (int j = 0; j < mesh.vertices.size(); j++)
        {
            if (i == j)
                continue;
            geometry_msgs::Point p1 = mesh.vertices[i];
            geometry_msgs::Point p2 = mesh.vertices[j];
            if (p1.x == p2.x && p1.y == p2.y && p1.z == p2.z)
            {
                ROS_WARN("Mesh contains duplicate vertex!");
                return mesh;
            }
        }
    }
    ROS_INFO("No duplicate vertices");
    for (int i = 0; i < mesh.triangles.size(); i++)
    {
        for (int j = 0; j < mesh.triangles.size(); j++)
        {
            if (i == j)
                continue;
            auto f1 = mesh.triangles[i].vertex_indices;
            auto f2 = mesh.triangles[j].vertex_indices;
            if (f1[0] == f2[0] && f1[1] == f2[1] && f1[2] == f2[2])
            {
                ROS_WARN("Mesh contains duplicate face!");
                return mesh;
            }
        }
    }
    ROS_INFO("No duplicate faces");
    return mesh;
}
[ INFO] [1627486721.860609990] [/rviz_1627486714898213024]: Loading part from /home/rob/checkout/halfsphere.stl
[ INFO] [1627486721.893491121] [/rviz_1627486714898213024]: 8373 vertices found
[ INFO] [1627486721.893561973] [/rviz_1627486714898213024]: 16562 faces found
[ INFO] [1627486721.895515564] [/rviz_1627486714898213024]: Built cell locator
[ INFO] [1627486721.903951288] [/rviz_1627486714898213024]: Computed 16562 normals
[ INFO] [1627486739.676315302] [/rviz_1627486714898213024]: No duplicate vertices
[ INFO] [1627486739.687466058] [/rviz_1627486714898213024]: No duplicate faces