WIP: Add Axis-Aligned Bounding Box option to Plane-Slicer Planner

.github/workflows/bionic_build.yml

@@ -20,7 +20,7 @@ jobs:
       ROS_REPO: main
       NOT_TEST_BUILD: true
       UPSTREAM_WORKSPACE: 'dependencies_ros1.rosinstall'
-      ROSDEP_SKIP_KEYS: "iwyu cmake_common_scripts"
+      ROSDEP_SKIP_KEYS: "iwyu ros_industrial_cmake_boilerplate"
       CCACHE_DIR: "/home/runner/work/noether/noether/Bionic-Build/.ccache"
       TARGET_CMAKE_ARGS: "-DNURBS_FOUND=TRUE"
       DOCKER_IMAGE: "rosindustrial/noether:melodic"

.github/workflows/focal_build.yml

@@ -20,7 +20,7 @@ jobs:
       ROS_REPO: main
       NOT_TEST_BUILD: true
       UPSTREAM_WORKSPACE: 'dependencies_ros1.rosinstall'
-      ROSDEP_SKIP_KEYS: "iwyu cmake_common_scripts"
+      ROSDEP_SKIP_KEYS: "iwyu ros_industrial_cmake_boilerplate"
       CCACHE_DIR: "/home/runner/work/noether/noether/Focal-Build/.ccache"
       TARGET_CMAKE_ARGS: "-DNURBS_FOUND=FALSE"
       DOCKER_IMAGE: "rosindustrial/noether:noetic"

.github/workflows/xenial_build.yml

@@ -20,7 +20,7 @@ jobs:
       ROS_REPO: main
       NOT_TEST_BUILD: true
       UPSTREAM_WORKSPACE: 'dependencies_ros1.rosinstall'
-      ROSDEP_SKIP_KEYS: "iwyu cmake_common_scripts"
+      ROSDEP_SKIP_KEYS: "iwyu ros_industrial_cmake_boilerplate"
       CCACHE_DIR: "/home/runner/work/noether/noether/Xenial-Build/.ccache"
       TARGET_CMAKE_ARGS: "-DNURBS_FOUND=TRUE"
       DOCKER_IMAGE: "rosindustrial/noether:kinetic"

dependencies_ros1.rosinstall

@@ -1,3 +1,3 @@
 # PCL ros tools, this is required because using custom version of PCL and VTK
 - git: {local-name: perception_pcl, uri: 'https://github.com/ros-perception/perception_pcl.git', version: 1.7.1}
-- git: {local-name: cmake_common_scripts, uri: 'https://github.com/ros-industrial/cmake_common_scripts.git', version: master}
+- git: {local-name: ros_industrial_cmake_boilerplate, uri: 'https://github.com/ros-industrial/ros_industrial_cmake_boilerplate.git', version: master}

noether/CMakeLists.txt

@@ -31,6 +31,7 @@ find_package(catkin REQUIRED COMPONENTS
   pcl_conversions
   roscpp
   tool_path_planner
+  smooth_pose_traj
 )
 
 find_package(vtk_viewer REQUIRED)
@@ -52,6 +53,7 @@ catkin_package(
     pcl_conversions
     roscpp
     tool_path_planner
+    smooth_pose_traj
   DEPENDS
     VTK
     PCL

noether/package.xml

@@ -20,5 +20,5 @@
   <depend>actionlib</depend>
   <depend>noether_conversions</depend>
   <depend>noether_filtering</depend>
-
+  <depend>smooth_pose_traj</depend>
 </package>

noether/src/tool_path_planner_server.cpp

@@ -11,6 +11,7 @@
 #include <tool_path_planner/plane_slicer_raster_generator.h>
 #include <tool_path_planner/eigen_value_edge_generator.h>
 #include <tool_path_planner/halfedge_edge_generator.h>
+#include <smooth_pose_traj/smooth_pose_traj.hpp>
 
 static const double FEEDBACK_PUBLISH_PERIOD = 1.0;  // seconds
 static const std::string GENERATE_TOOL_PATHS_ACTION = "generate_tool_paths";
@@ -97,6 +98,7 @@ class TppServer
       const noether_msgs::ToolPathConfig& config = goal->path_configs[i];
       const shape_msgs::Mesh& mesh = goal->surface_meshes[i];
       boost::optional<tool_path_planner::ToolPaths> tool_paths = boost::none;
+      double pt_spacing, raster_spacing;
       ROS_INFO("Planning path");
       if (config.type == noether_msgs::ToolPathConfig::SURFACE_WALK_RASTER_GENERATOR)
       {
@@ -105,6 +107,8 @@ class TppServer
         tool_path_planner::toSurfaceWalkConfig(path_config, config.surface_walk_generator);
         path_gen->setConfiguration(path_config);
         generator = path_gen;
+        pt_spacing = path_config.point_spacing;
+        raster_spacing = path_config.raster_spacing;
       }
       else if (config.type == noether_msgs::ToolPathConfig::PLANE_SLICER_RASTER_GENERATOR)
       {
@@ -113,6 +117,9 @@ class TppServer
         tool_path_planner::toPlaneSlicerConfig(path_config, config.plane_slicer_generator);
         path_gen->setConfiguration(path_config);
         generator = path_gen;
+        pt_spacing = path_config.point_spacing;
+        raster_spacing = path_config.raster_spacing;
+        smooth_pose_arrays_ = path_config.smooth_rasters;
       }
       else if (config.type == noether_msgs::ToolPathConfig::EIGEN_VALUE_EDGE_GENERATOR)
       {
@@ -121,6 +128,7 @@ class TppServer
         tool_path_planner::toEigenValueConfig(path_config, config.eigen_value_generator);
         path_gen->setConfiguration(path_config);
         generator = path_gen;
+        pt_spacing = path_config.merge_dist;
       }
       else if (config.type == noether_msgs::ToolPathConfig::HALFEDGE_EDGE_GENERATOR)
       {
@@ -129,6 +137,7 @@ class TppServer
         tool_path_planner::toHalfedgeConfig(path_config, config.halfedge_generator);
         path_gen->setConfiguration(path_config);
         generator = path_gen;
+        pt_spacing = path_config.point_dist;
       }
       else
       {
@@ -144,7 +153,6 @@ class TppServer
 
       if (tool_paths.is_initialized())
       {
-        noether_msgs::ToolPaths trp;
         for (const auto& tool_path : tool_paths.get())
         {
           noether_msgs::ToolPath tp;
@@ -157,9 +165,15 @@ class TppServer
               tf::poseEigenToMsg(p, pose);
               seg.poses.push_back(pose);
             }
+            if (smooth_pose_arrays_)
+            {
+              smooth_pose_traj::SmoothPoseTraj smoother(seg, pt_spacing, true);
+              smoother.process(seg);
+            }
             tp.segments.push_back(seg);
           }
-          trp.paths.push_back(tp);
+
+          result.tool_paths[i].paths.push_back(tp);
         }
 
         result.tool_path_validities[i] = true;
@@ -229,6 +243,7 @@ class TppServer
   ros::NodeHandle nh_;
   GenPathActionServer as_;
   std::mutex goal_process_mutex_;
+  bool smooth_pose_arrays_;
 };
 
 }  // namespace tpp_path_gen

noether_filtering/CMakeLists.txt

@@ -46,6 +46,7 @@ list(FIND CMAKE_CXX_COMPILE_FEATURES cxx_std_14 CXX_FEATURE_FOUND)
 add_library(${PROJECT_NAME} SHARED
   src/filter_group.cpp
   src/filter_manager.cpp
+  src/utils.cpp
 )
 target_link_libraries(${PROJECT_NAME} PUBLIC
   ${Boost_LIBRARIES}

noether_filtering/include/noether_filtering/utils.h

@@ -8,10 +8,14 @@
 #ifndef INCLUDE_NOETHER_FILTERING_UTILS_H_
 #define INCLUDE_NOETHER_FILTERING_UTILS_H_
 
-#include <typeinfo>
+#include <cxxabi.h>
 #include <memory>
 #include <string>
-#include <cxxabi.h>
+#include <typeinfo>
+
+#include <pcl/PolygonMesh.h>
+#include <vtkPolyData.h>
+#include <vtkSmartPointer.h>
 
 namespace noether_filtering
 {
@@ -28,6 +32,16 @@ static std::string getClassName()
   return (status == 0) ? res.get() : mangled_name;
 }
 
+/**
+ * @brief vtk2TriangleMesh - pcl's vtk2mesh function can produce a PolygonMesh containing 1-point
+ * or 2-point 'polygons', as well as squares and so on.  Many applications assume a mesh containing
+ * only triangles.  This function converts the output to only include triangles.
+ * @param poly_data - input - the VTK mesh
+ * @param mesh - output - the resulting pcl polygonmesh containing only triangles
+ * @return - number of points contained in pcl::PolygonMesh, as vtk2mesh does.
+ */
+int vtk2TriangleMesh(const vtkSmartPointer<vtkPolyData>& poly_data, pcl::PolygonMesh& mesh);
+
 }  // namespace utils
 }  // namespace noether_filtering
 

noether_filtering/src/mesh/clean_data.cpp

@@ -45,7 +45,7 @@ bool CleanData::filter(const pcl::PolygonMesh& mesh_in, pcl::PolygonMesh& mesh_o
   mesh_data = cleanPolyData->GetOutput();
   std::size_t num_end_points = mesh_data->GetNumberOfPoints();
   CONSOLE_BRIDGE_logInform("Removed duplicate points, retained %lu points from %lu", num_end_points, num_start_points);
-  VTKUtils::vtk2mesh(mesh_data, mesh_out);
+  utils::vtk2TriangleMesh(mesh_data, mesh_out);
   return true;
 }
 

noether_filtering/src/mesh/fill_holes.cpp

@@ -88,7 +88,7 @@ bool FillHoles::filter(const pcl::PolygonMesh& mesh_in, pcl::PolygonMesh& mesh_o
   mesh_data = normals_rectifier->GetOutput();
   std::size_t end_num_polys = mesh_data->GetNumberOfPolys();
   CONSOLE_BRIDGE_logInform("Filled %lu polygons", start_num_polys - end_num_polys);
-  VTKUtils::vtk2mesh(mesh_data, mesh_out);
+  utils::vtk2TriangleMesh(mesh_data, mesh_out);
   return true;
 }
 

noether_filtering/src/mesh/windowed_sinc_smoothing.cpp

@@ -91,7 +91,7 @@ bool WindowedSincSmoothing::filter(const pcl::PolygonMesh& mesh_in, pcl::Polygon
   smoother->Update();
 
   mesh_data = smoother->GetOutput();
-  VTKUtils::vtk2mesh(mesh_data, mesh_out);
+  utils::vtk2TriangleMesh(mesh_data, mesh_out);
   return true;
 }
 

noether_filtering/src/utils.cpp

@@ -0,0 +1,106 @@
+#include <noether_filtering/utils.h>
+
+#include <pcl/conversions.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/surface/vtk_smoothing/vtk_utils.h>
+#include <vtkPointData.h>
+
+namespace noether_filtering
+{
+namespace utils
+{
+int vtk2TriangleMesh(const vtkSmartPointer<vtkPolyData>& poly_data, pcl::PolygonMesh& mesh)
+{
+  mesh.polygons.clear();
+  mesh.cloud.data.clear();
+  mesh.cloud.width = mesh.cloud.height = 0;
+  mesh.cloud.is_dense = true;
+
+  vtkSmartPointer<vtkPoints> mesh_points = poly_data->GetPoints();
+  vtkIdType nr_points = mesh_points->GetNumberOfPoints();
+  vtkIdType nr_polygons = poly_data->GetNumberOfPolys();
+
+  if (nr_points == 0)
+    return 0;
+
+  vtkUnsignedCharArray* poly_colors = nullptr;
+  if (poly_data->GetPointData() != nullptr)
+    poly_colors = vtkUnsignedCharArray::SafeDownCast(poly_data->GetPointData()->GetScalars("Colors"));
+
+  // Some applications do not save the name of scalars (including PCL's native vtk_io)
+  if (!poly_colors)
+    poly_colors = vtkUnsignedCharArray::SafeDownCast(poly_data->GetPointData()->GetScalars("scalars"));
+
+  // TODO: currently only handles rgb values with 3 components
+  if (poly_colors && (poly_colors->GetNumberOfComponents() == 3))
+  {
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_temp(new pcl::PointCloud<pcl::PointXYZRGB>());
+    cloud_temp->points.resize(nr_points);
+    double point_xyz[3];
+    unsigned char point_color[3];
+    for (vtkIdType i = 0; i < mesh_points->GetNumberOfPoints(); ++i)
+    {
+      mesh_points->GetPoint(i, &point_xyz[0]);
+      (*cloud_temp)[i].x = static_cast<float>(point_xyz[0]);
+      (*cloud_temp)[i].y = static_cast<float>(point_xyz[1]);
+      (*cloud_temp)[i].z = static_cast<float>(point_xyz[2]);
+
+      poly_colors->GetTupleValue(i, &point_color[0]);
+      (*cloud_temp)[i].r = point_color[0];
+      (*cloud_temp)[i].g = point_color[1];
+      (*cloud_temp)[i].b = point_color[2];
+    }
+    cloud_temp->width = cloud_temp->size();
+    cloud_temp->height = 1;
+    cloud_temp->is_dense = true;
+
+    pcl::toPCLPointCloud2(*cloud_temp, mesh.cloud);
+  }
+  else  // in case points do not have color information:
+  {
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp(new pcl::PointCloud<pcl::PointXYZ>());
+    cloud_temp->points.resize(nr_points);
+    double point_xyz[3];
+    for (vtkIdType i = 0; i < mesh_points->GetNumberOfPoints(); ++i)
+    {
+      mesh_points->GetPoint(i, &point_xyz[0]);
+      (*cloud_temp)[i].x = static_cast<float>(point_xyz[0]);
+      (*cloud_temp)[i].y = static_cast<float>(point_xyz[1]);
+      (*cloud_temp)[i].z = static_cast<float>(point_xyz[2]);
+    }
+    cloud_temp->width = cloud_temp->size();
+    cloud_temp->height = 1;
+    cloud_temp->is_dense = true;
+
+    pcl::toPCLPointCloud2(*cloud_temp, mesh.cloud);
+  }
+
+  mesh.polygons.reserve(nr_polygons);
+#ifdef VTK_CELL_ARRAY_V2
+  vtkIdType const* cell_points;
+#else
+  vtkIdType* cell_points;
+#endif
+  vtkIdType nr_cell_points;
+  vtkCellArray* mesh_polygons = poly_data->GetPolys();
+  mesh_polygons->InitTraversal();
+  int id_poly = 0;
+  while (mesh_polygons->GetNextCell(nr_cell_points, cell_points))
+  {
+    if (nr_cell_points == 3 && cell_points[0] != cell_points[1] && cell_points[1] != cell_points[2] &&
+        cell_points[2] != cell_points[0])
+    {
+      mesh.polygons.emplace_back(pcl::Vertices());
+      mesh.polygons[id_poly].vertices.resize(nr_cell_points);
+      for (vtkIdType i = 0; i < nr_cell_points; ++i)
+        mesh.polygons[id_poly].vertices[i] = static_cast<unsigned int>(cell_points[i]);
+      ++id_poly;
+    }
+  }
+
+  return (static_cast<int>(nr_points));
+}
+
+}  // namespace utils
+}  // namespace noether_filtering

noether_msgs/CMakeLists.txt

@@ -23,6 +23,7 @@ add_message_files(
     SurfaceWalkRasterGeneratorConfig.msg
     EigenValueEdgeGeneratorConfig.msg
     HalfedgeEdgeGeneratorConfig.msg
+    HeatRasterGeneratorConfig.msg
     SegmentationConfig.msg
     ToolPathConfig.msg
     ToolPath.msg

noether_msgs/msg/EigenValueEdgeGeneratorConfig.msg

@@ -27,3 +27,6 @@ int32 max_intersecting_voxels
 
 # Any two consecutive points with a shortest distance smaller than this value are merged
 float64 merge_dist
+
+# flag to break edge paths by diagonal axes
+bool split_by_axes

noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg

@@ -20,3 +20,9 @@ int32 point_spacing_method
 
 # Point distance parameter used in conjunction with the spacing method
 float64 point_dist
+
+# flag to break edge paths by diagonal axes
+bool split_by_axes
+
+# Size (meters) of peninsulas or inlets to skip over, default of 0 will not bridge any gaps, effectively off
+float64 max_bridge_distance

noether_msgs/msg/HeatRasterGeneratorConfig.msg

@@ -0,0 +1,7 @@
+float64 point_spacing 		      # Required spacing between path points
+float64 raster_spacing 		      # Offset between two rasters
+float64 tool_offset 		      # How far off the surface the tool needs to be
+float64 min_hole_size 		      # A path passes over smaller holes breaks into two on larger wholes 
+float64 min_segment_size 	      # The minimum segment size to allowed 
+float64 raster_rot_offset             # use this angle when no sources are povided
+bool generate_extra_rasters           # Add extra strokes off the boundaries (NOT YET IMPLEMENTED BY HEAT METHOD)

noether_msgs/msg/PlaneSlicerRasterGeneratorConfig.msg

@@ -1,6 +1,21 @@
-float64 raster_spacing 	   # Offset between two rasters
-float64 point_spacing 	   # Required spacing between path points
-float64 raster_rot_offset  # Specifies the direction of the raster path in radians
-float64 min_segment_size   # The minimum segment size to allow when finding intersections; small segments will be discarded
-float64 min_hole_size 	   # A path may pass over holes smaller than this, but must be broken when larger holes are encounterd.
-float64 tool_offset 	   # How far off the surface the tool needs to be
+float64 raster_spacing 	    # Offset between two rasters
+float64 point_spacing 	     # Required spacing between path points
+float64 raster_rot_offset    # Specifies the direction of the raster path in radians. Rotates about short dimension of bounding box.
+float64 min_segment_size     # The minimum segment size to allow when finding intersections; small segments will be discarded
+float64 min_hole_size 	     # A path may pass over holes smaller than this, but must be broken when larger holes are encounterd.
+float64 tool_offset 	     # How far off the surface the tool needs to be
+float64 search_radius        # radius over which the normals are averaged
+bool generate_extra_rasters  # Whether an extra raster stroke should be added to the beginning and end of the raster pattern
+bool interleave_rasters      # create a second set of rasters interleaved inbetween first set
+bool smooth_rasters          # post process poses by fitting a cubic b-spline and re-sampling
+int32  raster_style          # 0,1,2 for mow_style, paint_style and read_style
+
+# When set to TRUE: initial cuts are determined using an axis-aligned bounding box. (global axes)
+# When set to FALSE: initial cuts are determined using an object-oriented bounding box and the
+# principal axes of the part.  (Note that ROS sets message fields to 0/False by default.)
+bool raster_wrt_global_axes
+
+# Normal of the cutting planes.  Points from the first raster to the second raster, not the
+# direction from the first point in one raster to the second point of the same raster.  Specified
+# in the frame of the input mesh.  Overrides raster_wrt_global_axes
+geometry_msgs/Vector3 raster_direction

noether_msgs/msg/ToolPathConfig.msg

@@ -3,10 +3,13 @@ uint8 PLANE_SLICER_RASTER_GENERATOR=1
 uint8 HEAT_RASTER_GENERATOR=2
 uint8 EIGEN_VALUE_EDGE_GENERATOR=3 # This does not have a config
 uint8 HALFEDGE_EDGE_GENERATOR=4
+uint8 PAINT_RASTER_STYLE=0
+uint8 MOW_RASTER_STYLE=1
+uint8 READ_RASTER_STYLE=2
 
 uint8 type
 noether_msgs/SurfaceWalkRasterGeneratorConfig surface_walk_generator
 noether_msgs/PlaneSlicerRasterGeneratorConfig plane_slicer_generator
-#noether_msgs/HeatRasterPathGeneratorConfig heat_generator
+noether_msgs/HeatRasterGeneratorConfig heat_generator
 noether_msgs/EigenValueEdgeGeneratorConfig eigen_value_generator
 noether_msgs/HalfedgeEdgeGeneratorConfig halfedge_generator