diff --git a/noether_msgs/msg/PlaneSlicerRasterGeneratorConfig.msg b/noether_msgs/msg/PlaneSlicerRasterGeneratorConfig.msg index ca33e269..db7a14e9 100644 --- a/noether_msgs/msg/PlaneSlicerRasterGeneratorConfig.msg +++ b/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 diff --git a/tool_path_planner/include/tool_path_planner/plane_slicer_raster_generator.h b/tool_path_planner/include/tool_path_planner/plane_slicer_raster_generator.h index c538a53d..a5354d34 100644 --- a/tool_path_planner/include/tool_path_planner/plane_slicer_raster_generator.h +++ b/tool_path_planner/include/tool_path_planner/plane_slicer_raster_generator.h @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -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 { diff --git a/tool_path_planner/include/tool_path_planner/utilities.h b/tool_path_planner/include/tool_path_planner/utilities.h index 3641e046..e90b7281 100644 --- a/tool_path_planner/include/tool_path_planner/utilities.h +++ b/tool_path_planner/include/tool_path_planner/utilities.h @@ -124,6 +124,4 @@ namespace tool_path_planner } - - #endif /* INCLUDE_TOOL_PATH_PLANNER_UTILITIES_H_ */ diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index 6c28058e..c64797dc 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -458,10 +458,40 @@ boost::optional PlaneSlicerRasterGenerator::generate() { CONSOLE_BRIDGE_logDebug("%s No mesh data has been provided",getName().c_str()); } - // computing mayor axis using oob - vtkSmartPointer oob = vtkSmartPointer::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 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 oob = vtkSmartPointer::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 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); diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index 52c76c90..092b45c2 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/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; }