Merge pull request #121 from marip8/fix/raster_wrt_global_axes

Raster w.r.t. a specified direction

noether_msgs/msg/SurfaceWalkRasterGeneratorConfig.msg

@@ -5,13 +5,5 @@ float64 intersection_plane_height 	# Used by the raster_tool_path_planner when o
 float64 min_hole_size 			# A path may pass over holes smaller than this, but must be broken when larger holes are encounterd.
 float64 min_segment_size 		# The minimum segment size to allow when finding intersections; small segments will be discarded
 float64 raster_rot_offset               # Specifies the direction of the raster path in radians
-
-# When set to TRUE: initial cuts are determined using the axes of the
-# coordinate frame in which the mesh is placed.  This may cause the
-# initial cut to miss the part.
-# When set to FALSE: initial cuts are determined using the centroid and
-# principal axes of the part.  This is the old default behavior.  (Note
-# that ROS sets message fields to 0/False by default.)
-bool raster_wrt_global_axes
-
 bool generate_extra_rasters   # Whether an extra raster stroke should be added to the beginning and end of the raster pattern
+geometry_msgs/Vector3 cut_direction  # Desired raster direction, in mesh coordinate system (i.e. global coordinate system)

tool_path_planner/include/tool_path_planner/surface_walk_raster_generator.h

@@ -64,7 +64,6 @@ class SurfaceWalkRasterGenerator : public PathGenerator
   static constexpr double DEFAULT_INTERSECTION_PLANE_HEIGHT = 0;
   static constexpr double DEFAULT_TOOL_OFFSET = 0;
   static constexpr bool DEFAULT_GENERATE_EXTRA_RASTERS = false;
-  static constexpr bool DEFAULT_RASTER_WRT_GLOBAL_AXIS = false;
 
 public:
   struct Config
@@ -77,7 +76,6 @@ class SurfaceWalkRasterGenerator : public PathGenerator
     double intersection_plane_height{ DEFAULT_INTERSECTION_PLANE_HEIGHT };
     double tool_offset{ DEFAULT_TOOL_OFFSET };
     bool generate_extra_rasters{ DEFAULT_GENERATE_EXTRA_RASTERS };
-    bool raster_wrt_global_axes{ DEFAULT_RASTER_WRT_GLOBAL_AXIS };
     double cut_direction[3]{ 0, 0, 0 };
     bool debug{ false };
 

tool_path_planner/src/surface_walk_raster_generator.cpp

@@ -1229,40 +1229,41 @@ vtkSmartPointer<vtkPolyData> SurfaceWalkRasterGenerator::createStartCurve()
   vtkSmartPointer<vtkPoints> pts = vtkSmartPointer<vtkPoints>::New();
   line->SetPoints(pts);
 
-  Eigen::Vector3d max, min;
+  Eigen::Vector3d raster_axis, rotation_axis;
   {
     // Calculate the object-oriented bounding box to determine how wide a cutting plane should be
     vtkSmartPointer<vtkOBBTree> obb_tree = vtkSmartPointer<vtkOBBTree>::New();
     obb_tree->SetTolerance(0.001);
     obb_tree->SetLazyEvaluation(0);
-    Eigen::Vector3d mid, corner, norm_size;
+    Eigen::Vector3d min, max, mid, corner, norm_size;
     obb_tree->ComputeOBB(mesh_data_->GetPoints(), corner.data(), max.data(), mid.data(), min.data(), norm_size.data());
 
     // Use the specified cut direction if it is not [0, 0, 0]
     Eigen::Vector3d cut_dir{ config_.cut_direction };
     if (!cut_dir.isApprox(Eigen::Vector3d::Zero()))
     {
-      max = cut_dir.normalized() * max.norm();
+      // Project the cut direction onto the plane of the mesh defined by the average normal at [0, 0, 0]
+      Eigen::Hyperplane<double, 3> plane(avg_norm, 0.0);
+      raster_axis = plane.projection(cut_dir).normalized() * max.norm();
+      rotation_axis = avg_norm.normalized();
     }
-    else
+    else if (norm_size[1] / norm_size[0] > 0.99)
     {
       // ComputeOBB uses PCA to find the principle axes, thus for square objects it returns the diagonals instead
-      // of the minimum bounding box.  Compare the first and second axes to see if they are within 1% of each other
-      if (norm_size[1] / norm_size[0] > 0.99)
-      {
-        // if object is square, need to average max and mid in order to get the correct axes of the object
-        max = (max.normalized() + mid.normalized()) * max.norm();
-      }
+      // of the minimum bounding box.  If the first and second axes are within 1% of each other, average max and mid
+      // to get the desired axes of the object
+      raster_axis = (max.normalized() + mid.normalized()) * max.norm();
+    }
+    else
+    {
+      // Simply use the max OBB axis as the raster axis and the min OBB axis as the rotation axis
+      raster_axis = max;
+      rotation_axis = min;
     }
   }
 
   // Rotate the largest principal axis around the smallest principal axis by the specified angle
-  Eigen::Vector3d raster_axis = Eigen::AngleAxisd(config_.raster_rot_offset, min.normalized()) * max;
-
-  // TODO: Add the ability to define a rotation in mesh coordinates that is then projected onto the mesh plane
-  //  if (config_.raster_wrt_global_axes)
-  //  {
-  //  }
+  raster_axis = Eigen::AngleAxisd(config_.raster_rot_offset, rotation_axis.normalized()) * raster_axis;
 
   // Use raster_axis to create additional points for the starting curve
   {

tool_path_planner/src/utilities.cpp

@@ -152,8 +152,11 @@ bool toSurfaceWalkConfigMsg(noether_msgs::SurfaceWalkRasterGeneratorConfig& conf
   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.generate_extra_rasters = config.generate_extra_rasters;
+
+  config_msg.cut_direction.x = config.cut_direction[0];
+  config_msg.cut_direction.y = config.cut_direction[1];
+  config_msg.cut_direction.z = config.cut_direction[2];
   return true;
 }
 
@@ -208,8 +211,12 @@ bool toSurfaceWalkConfig(SurfaceWalkRasterGenerator::Config& config,
   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;
   config.generate_extra_rasters = config_msg.generate_extra_rasters;
+
+  config.cut_direction[0] = config_msg.cut_direction.x;
+  config.cut_direction[1] = config_msg.cut_direction.y;
+  config.cut_direction[2] = config_msg.cut_direction.z;
+
   return true;
 }
 

tool_path_planner/test/utest.cpp

@@ -370,7 +370,6 @@ TEST(IntersectTest, SurfaceWalkRasterRotationTest)
     tool.intersection_plane_height = 0.2;  // 0.5 works best, not sure if this should be included in the tool
     tool.min_hole_size = 0.1;
     tool.raster_rot_offset = direction;
-    tool.raster_wrt_global_axes = false;
     tool.debug = false;
     planner.setConfiguration(tool);
     runRasterRotationTest(planner, mesh);
@@ -393,7 +392,6 @@ TEST(IntersectTest, PlaneSlicerRasterRotationTest)
     //    tool.tool_offset = 0.0;                // currently unused
     tool.min_hole_size = 0.1;
     tool.raster_rot_offset = direction;
-    //    tool.raster_wrt_global_axes = false;
     //    tool.debug = false;
     planner.setConfiguration(tool);
     runRasterRotationTest(planner, mesh);
@@ -532,7 +530,6 @@ TEST(IntersectTest, SurfaceWalkExtraRasterTest)
   tool.intersection_plane_height = 0.2;  // 0.5 works best, not sure if this should be included in the tool
   tool.min_hole_size = 0.1;
   tool.raster_rot_offset = 0.0;
-  tool.raster_wrt_global_axes = false;
   tool.generate_extra_rasters = false;
   tool.debug = false;