diff --git a/noether_msgs/msg/SurfaceWalkRasterGeneratorConfig.msg b/noether_msgs/msg/SurfaceWalkRasterGeneratorConfig.msg index 509a6462..a5ad965a 100644 --- a/noether_msgs/msg/SurfaceWalkRasterGeneratorConfig.msg +++ b/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) diff --git a/tool_path_planner/include/tool_path_planner/surface_walk_raster_generator.h b/tool_path_planner/include/tool_path_planner/surface_walk_raster_generator.h index 56ccb40d..5cef2a51 100644 --- a/tool_path_planner/include/tool_path_planner/surface_walk_raster_generator.h +++ b/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 }; diff --git a/tool_path_planner/src/surface_walk_raster_generator.cpp b/tool_path_planner/src/surface_walk_raster_generator.cpp index 360b47e5..c6a3b50f 100644 --- a/tool_path_planner/src/surface_walk_raster_generator.cpp +++ b/tool_path_planner/src/surface_walk_raster_generator.cpp @@ -1229,40 +1229,41 @@ vtkSmartPointer SurfaceWalkRasterGenerator::createStartCurve() vtkSmartPointer pts = vtkSmartPointer::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 obb_tree = vtkSmartPointer::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 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 { diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index 5f8f8cb2..cc0d7435 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/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; } diff --git a/tool_path_planner/test/utest.cpp b/tool_path_planner/test/utest.cpp index 5a4f653c..b5255792 100644 --- a/tool_path_planner/test/utest.cpp +++ b/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;