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

Author: DavidMerzJr

noether_msgs/msg/PlaneSlicerRasterGeneratorConfig.msg

@@ -1,6 +1,17 @@
 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 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
+
+# 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.  X is the
+# longest direction of the bounding box, Z is the shortest, Y is middle, following right hand rule.
+# If left as 0 (magnitude < 0.000001), defaults to [0,1,0], the mid direction of the bounding box.
+geometry_msgs/Vector3 raster_direction

tool_path_planner/include/tool_path_planner/plane_slicer_raster_generator.h

@@ -26,6 +26,7 @@
 #include <vtkSmartPointer.h>
 #include <vtkPolyData.h>
 #include <boost/optional.hpp>
+#include <Eigen/Dense>
 #include <pcl/PolygonMesh.h>
 #include <shape_msgs/Mesh.h>
 #include <vtkCellLocator.h>
@@ -45,6 +46,7 @@ class PlaneSlicerRasterGenerator : public PathGenerator
   static constexpr double DEFAULT_MIN_SEGMENT_SIZE = 0.01;
   static constexpr double DEFAULT_SEARCH_RADIUS = 0.01;
   static constexpr double DEFAULT_MIN_HOLE_SIZE = 1e-2;
+  static constexpr bool DEFAULT_RASTER_WRT_GLOBAL_AXES = false;
 
 public:
 
@@ -56,6 +58,8 @@ class PlaneSlicerRasterGenerator : public PathGenerator
     double min_segment_size {DEFAULT_MIN_SEGMENT_SIZE};
     double search_radius {DEFAULT_SEARCH_RADIUS};
     double min_hole_size {DEFAULT_MIN_HOLE_SIZE};
+    bool raster_wrt_global_axes {DEFAULT_RASTER_WRT_GLOBAL_AXES};
+    Eigen::Vector3d raster_direction {Eigen::Vector3d::UnitY()};
 
     Json::Value toJson() const
     {

tool_path_planner/include/tool_path_planner/utilities.h

@@ -124,6 +124,4 @@ namespace tool_path_planner
 
 }
 
-
-
 #endif /* INCLUDE_TOOL_PATH_PLANNER_UTILITIES_H_ */

tool_path_planner/src/plane_slicer_raster_generator.cpp

@@ -458,10 +458,40 @@ boost::optional<ToolPaths> PlaneSlicerRasterGenerator::generate()
   {
     CONSOLE_BRIDGE_logDebug("%s No mesh data has been provided",getName().c_str());
   }
-  // computing mayor axis using oob
-  vtkSmartPointer<vtkOBBTree> oob = vtkSmartPointer<vtkOBBTree>::New();
-  Vector3d corner, x_dir, y_dir, z_dir, sizes;// x is longest, y is mid and z is smallest
-  oob->ComputeOBB(mesh_data_,corner.data(), x_dir.data(), y_dir.data(), z_dir.data(), sizes.data());
+  // Assign the longest axis of the bounding box to x, middle to y, and shortest to z.
+  Vector3d corner, x_dir, y_dir, z_dir, sizes;
+  if (config_.raster_wrt_global_axes)
+  {
+    // Determine extent of mesh along axes of current coordinate frame
+    VectorXd bounds(6);
+    std::vector<Vector3d> extents;
+    mesh_data_->GetBounds(bounds.data());
+    extents.push_back(Vector3d::UnitX() * (bounds[1] - bounds[0]));  // Extent in x-direction of supplied mesh coordinate frame
+    extents.push_back(Vector3d::UnitY() * (bounds[3] - bounds[2]));  // Extent in y-direction of supplied mesh coordinate frame
+    extents.push_back(Vector3d::UnitZ() * (bounds[5] - bounds[4]));  // Extent in z-direction of supplied mesh coordinate frame
+
+    // find min and max magnitude.
+    int max = 0;
+    int min = 0;
+    for (std::size_t i = 1; i < extents.size(); ++i)
+    {
+      if (extents[max].squaredNorm() < extents[i].squaredNorm())
+        max = i;
+      else if (extents[min].squaredNorm() > extents[i].squaredNorm())
+        min = i;
+    }
+
+    // Assign the axes in order.  Computing y saves comparisons and guarantees right-handedness.
+    x_dir = extents[max].normalized();
+    z_dir = extents[min].normalized();
+    y_dir = z_dir.cross(x_dir).normalized();
+  }
+  else
+  {
+    // computing major axes using oob and assign to x_dir, y_dir, z_dir
+    vtkSmartPointer<vtkOBBTree> oob = vtkSmartPointer<vtkOBBTree>::New();
+    oob->ComputeOBB(mesh_data_,corner.data(), x_dir.data(), y_dir.data(), z_dir.data(), sizes.data());
+  }
 
   // Compute the center of mass
   Vector3d origin;
@@ -498,10 +528,11 @@ boost::optional<ToolPaths> PlaneSlicerRasterGenerator::generate()
   half_ext = sizes / 2.0;
   center = Eigen::Vector3d(bounds[0], bounds[2], bounds[3]) + half_ext;
 
-  // now cutting the mesh with planes along the y axis
-  // @todo This should be calculated instead of being fixed along the y-axis
+  // Apply the rotation offset about the short direction (new Z axis) of the bounding box
   Isometry3d rotation_offset = Isometry3d::Identity() * AngleAxisd(config_.raster_rot_offset, Vector3d::UnitZ());
-  Vector3d raster_dir = (rotation_offset * Vector3d::UnitY()).normalized();
+
+  // Calculate direction of raster strokes, rotated by the above-specified amount
+  Vector3d raster_dir = (rotation_offset * config_.raster_direction).normalized();
 
   // Calculate all 8 corners projected onto the raster direction vector
   Eigen::VectorXd dist(8);

tool_path_planner/src/utilities.cpp

@@ -165,10 +165,14 @@ bool toPlaneSlicerConfigMsg(noether_msgs::PlaneSlicerRasterGeneratorConfig &conf
 {
   config_msg.point_spacing = config.point_spacing;
   config_msg.raster_spacing = config.raster_spacing;
-// config_msg.tool_offset = config.tool_offset;
   config_msg.min_hole_size = config.min_hole_size;
   config_msg.min_segment_size = config.min_segment_size;
   config_msg.raster_rot_offset = config.raster_rot_offset;
+  config_msg.raster_wrt_global_axes = config.raster_wrt_global_axes;
+
+  config_msg.raster_direction.x = config.raster_direction.x();
+  config_msg.raster_direction.y = config.raster_direction.y();
+  config_msg.raster_direction.z = config.raster_direction.z();
 
   return true;
 }
@@ -220,10 +224,22 @@ bool toPlaneSlicerConfig(PlaneSlicerRasterGenerator::Config& config,
 {
   config.point_spacing = config_msg.point_spacing;
   config.raster_spacing = config_msg.raster_spacing;
-// config.tool_offset = config_msg.tool_offset;
   config.min_hole_size = config_msg.min_hole_size;
   config.min_segment_size = config_msg.min_segment_size;
   config.raster_rot_offset = config_msg.raster_rot_offset;
+  config.raster_wrt_global_axes = config_msg.raster_wrt_global_axes;
+
+  // Check that the raster direction was set; we are not interested in direction [0,0,0]
+  double norm_squared =
+      config_msg.raster_direction.x * config_msg.raster_direction.x +
+      config_msg.raster_direction.y * config_msg.raster_direction.y +
+      config_msg.raster_direction.z * config_msg.raster_direction.z;
+  if (norm_squared > 0.000001)
+  {
+    config.raster_direction.x() = config_msg.raster_direction.x;
+    config.raster_direction.y() = config_msg.raster_direction.y;
+    config.raster_direction.z() = config_msg.raster_direction.z;
+  }
 
   return true;
 }