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

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
+
+# maximum segment length
+float64 max_segment_length

noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg

@@ -20,3 +20,6 @@ int32 point_spacing_method
 
 # Point distance parameter used in conjunction with the spacing method
 float64 point_dist
+
+# maximum segment length
+float64 max_segment_length

noether_msgs/msg/PlaneSlicerRasterGeneratorConfig.msg

@@ -1,6 +1,18 @@
-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
+bool generate_extra_rasters  # Whether an extra raster stroke should be added to the beginning and end of the raster pattern
+
+# 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/eigen_value_edge_generator.h

@@ -99,6 +99,8 @@ class EigenValueEdgeGenerator : public PathGenerator
 
     double merge_dist =
         0.01; /** @brief any two consecutive points with a shortest distance smaller than this value are merged */
+
+    double max_segment_length = 1.0; /** @brief maximum segment length */
   };
 
   EigenValueEdgeGenerator() = default;

tool_path_planner/include/tool_path_planner/halfedge_edge_generator.h

@@ -66,6 +66,9 @@ class HalfedgeEdgeGenerator : public PathGenerator
 
     /**@brief point distance parameter used in conjunction with the spacing method */
     double point_dist = 0.01;
+
+    /** @brief maximum segment length */
+    double max_segment_length = 1.0;
   };
 
   HalfedgeEdgeGenerator() = default;

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,8 @@ 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;
+  static constexpr bool DEFAULT_GENERATE_EXTRA_RASTERS = true;
 
 public:
   struct Config
@@ -55,7 +58,9 @@ 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() };
+    bool generate_extra_rasters{ DEFAULT_GENERATE_EXTRA_RASTERS };
     Json::Value toJson() const
     {
       Json::Value jv(Json::ValueType::objectValue);
@@ -65,6 +70,12 @@ class PlaneSlicerRasterGenerator : public PathGenerator
       jv["min_segment_size"] = min_segment_size;
       jv["search_radius"] = search_radius;
       jv["min_hole_size"] = min_hole_size;
+      jv["raster_wrt_global_axes"] = raster_wrt_global_axes;
+      Json::Value raster_dir(Json::ValueType::objectValue);
+      raster_dir["x"] = raster_direction.x();
+      raster_dir["y"] = raster_direction.y();
+      raster_dir["z"] = raster_direction.z();
+      jv["raster_direction"] = raster_dir;
 
       return jv;
     }
@@ -100,6 +111,26 @@ class PlaneSlicerRasterGenerator : public PathGenerator
                                                                                   DEFAULT_SEARCH_RADIUS;
       min_hole_size = validate(jv, "min_hole_size", Json::ValueType::realValue) ? jv["min_hole_size"].asDouble() :
                                                                                   DEFAULT_MIN_HOLE_SIZE;
+      raster_wrt_global_axes = validate(jv, "raster_wrt_global_axes", Json::ValueType::booleanValue) ?
+                                   jv["raster_wrt_global_axes"].asBool() :
+                                   DEFAULT_RASTER_WRT_GLOBAL_AXES;
+      if (jv["raster_direction"].isNull() || jv["raster_direction"].type() != Json::ValueType::objectValue)
+      {
+        ROS_ERROR("Malformed Raster Direction in Json value");
+        return false;
+      }
+      if (validate(jv["raster_direction"], "x", Json::ValueType::objectValue) &&
+          validate(jv["raster_direction"], "y", Json::ValueType::objectValue) &&
+          validate(jv["raster_direction"], "z", Json::ValueType::objectValue))
+      {
+        raster_direction.x() = jv["raster_direction"]["x"].asDouble();
+        raster_direction.y() = jv["raster_direction"]["y"].asDouble();
+        raster_direction.z() = jv["raster_direction"]["z"].asDouble();
+      }
+      else
+      {
+        raster_direction = Eigen::Vector3d::UnitY();
+      }
       return true;
     }
 
@@ -121,6 +152,11 @@ class PlaneSlicerRasterGenerator : public PathGenerator
       ss << "raster_rot_offset: " << raster_rot_offset << std::endl;
       ss << "min_segment_size: " << min_segment_size << std::endl;
       ss << "search_radius: " << search_radius << std::endl;
+      ss << "raster_wrt_global_axes: " << raster_wrt_global_axes << std::endl;
+      ss << "raster_direction: " << std::endl;
+      ss << "  x: " << raster_direction.x() << std::endl;
+      ss << "  y: " << raster_direction.y() << std::endl;
+      ss << "  z: " << raster_direction.z() << std::endl;
       return ss.str();
     }
   };

tool_path_planner/include/tool_path_planner/utilities.h

@@ -118,6 +118,11 @@ static std::string getClassName()
   return (status == 0) ? res.get() : mangled_name;
 }
 
+ToolPaths splitSegments(ToolPaths tool_paths, double max_segment_length);
+
+// duplicates first and last segment of each toolpath in tool_paths offset by the offset distance
+ToolPaths addExtraPaths(ToolPaths tool_paths, double offset_distance);
+
 }  // namespace tool_path_planner
 
 #endif /* INCLUDE_TOOL_PATH_PLANNER_UTILITIES_H_ */

tool_path_planner/src/eigen_value_edge_generator.cpp

@@ -330,6 +330,14 @@ boost::optional<ToolPaths> EigenValueEdgeGenerator::generate()
       edge_paths.push_back({ edge_segment });
     }
   }
+
+  if (config_.max_segment_length > 0)
+  {
+    edge_paths = tool_path_planner::splitSegments(edge_paths, config_.max_segment_length);
+  }
+
+  CONSOLE_BRIDGE_logInform("Found %lu valid edge segments", edge_paths.size());
+
   return edge_paths;
 }
 

tool_path_planner/src/halfedge_edge_generator.cpp

@@ -580,7 +580,13 @@ boost::optional<ToolPaths> HalfedgeEdgeGenerator::generate()
     return boost::none;
   }
 
+  if (config_.max_segment_length > 0)
+  {
+    edge_paths = tool_path_planner::splitSegments(edge_paths, config_.max_segment_length);
+  }
+
   CONSOLE_BRIDGE_logInform("Found %lu valid edge segments", edge_paths.size());
+
   return edge_paths;
 }
 

tool_path_planner/src/plane_slicer_raster_generator.cpp

@@ -409,7 +409,6 @@ void PlaneSlicerRasterGenerator::setInput(vtkSmartPointer<vtkPolyData> mesh)
   }
   else
   {
-    CONSOLE_BRIDGE_logWarn("%s generating normal data", getName().c_str());
     vtkSmartPointer<vtkPolyDataNormals> normal_generator = vtkSmartPointer<vtkPolyDataNormals>::New();
     normal_generator->SetInputData(mesh_data_);
     normal_generator->ComputePointNormalsOn();
@@ -453,10 +452,43 @@ 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;
@@ -492,10 +524,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);
@@ -681,7 +714,15 @@ boost::optional<ToolPaths> PlaneSlicerRasterGenerator::generate()
   }
 
   // converting to poses msg now
-  rasters = convertToPoses(rasters_data_vec);
+  if (config_.generate_extra_rasters)
+  {
+    ToolPaths temp_rasters = convertToPoses(rasters_data_vec);
+    rasters = addExtraPaths(temp_rasters, config_.raster_spacing);
+  }
+  else
+  {
+    rasters = convertToPoses(rasters_data_vec);
+  }
   return rasters;
 }
 

tool_path_planner/src/surface_walk_raster_generator.cpp

@@ -496,7 +496,11 @@ int findClosestPoint(const std::vector<double>& pt, const std::vector<std::vecto
   return index;
 }
 
-bool SurfaceWalkRasterGenerator::setConfiguration(const Config& config) { config_ = config; }
+bool SurfaceWalkRasterGenerator::setConfiguration(const Config& config)
+{
+  config_ = config;
+  return true;
+}
 
 void SurfaceWalkRasterGenerator::setInput(pcl::PolygonMesh::ConstPtr mesh)
 {