From 812f52e777913c6b0617456e9c1ab95fb66092fa Mon Sep 17 00:00:00 2001 From: "David Merz, Jr" Date: Fri, 11 Dec 2020 11:21:15 -0600 Subject: [PATCH 01/64] Add Axis-Aligned Bounding Box option to Plane-Slicer Planner --- .../msg/PlaneSlicerRasterGeneratorConfig.msg | 13 +++++- .../plane_slicer_raster_generator.h | 4 ++ .../src/plane_slicer_raster_generator.cpp | 45 ++++++++++++++++--- tool_path_planner/src/utilities.cpp | 20 ++++++++- 4 files changed, 72 insertions(+), 10 deletions(-) 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 14e22573..d4dd63ca 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: struct Config @@ -55,6 +57,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/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index 8bbbefdb..295874d3 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -453,10 +453,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; @@ -492,10 +522,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 cc0d7435..e01e1889 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; } @@ -225,10 +229,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; } From 4e328572ab24e0f664b60de336ae8738c15b64ed Mon Sep 17 00:00:00 2001 From: "David Merz, Jr" Date: Mon, 14 Dec 2020 15:53:06 -0600 Subject: [PATCH 02/64] Clang fixes and PlaneSlicerConfig Serialization --- .../plane_slicer_raster_generator.h | 31 +++++++++++++++++++ .../src/plane_slicer_raster_generator.cpp | 11 ++++--- tool_path_planner/src/utilities.cpp | 14 ++++----- 3 files changed, 44 insertions(+), 12 deletions(-) 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 d4dd63ca..48e257ae 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 @@ -69,6 +69,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; } @@ -104,6 +110,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; } @@ -125,6 +151,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(); } }; diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index 295874d3..c9515806 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -461,9 +461,12 @@ boost::optional PlaneSlicerRasterGenerator::generate() 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 + 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; @@ -485,7 +488,7 @@ boost::optional PlaneSlicerRasterGenerator::generate() { // 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()); + oob->ComputeOBB(mesh_data_, corner.data(), x_dir.data(), y_dir.data(), z_dir.data(), sizes.data()); } // Compute the center of mass diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index e01e1889..e5a74ae1 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -235,15 +235,13 @@ bool toPlaneSlicerConfig(PlaneSlicerRasterGenerator::Config& config, 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) + Eigen::Vector3d test_raster_direction; + test_raster_direction.x() = config_msg.raster_direction.x; + test_raster_direction.y() = config_msg.raster_direction.y; + test_raster_direction.z() = config_msg.raster_direction.z; + if (!test_raster_direction.isApprox(Eigen::Vector3d::Zero())) { - 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; + config.raster_direction = test_raster_direction; } return true; From 4e5c0227d2712b3203b6cf3e1b897ed1fbb127e7 Mon Sep 17 00:00:00 2001 From: "Dale, Steven P" Date: Wed, 9 Dec 2020 15:39:43 -0600 Subject: [PATCH 03/64] implemented simple max_edge_segment function --- .../eigen_value_edge_generator.h | 2 + .../halfedge_edge_generator.h | 3 ++ .../include/tool_path_planner/utilities.h | 2 + .../src/eigen_value_edge_generator.cpp | 2 +- .../src/halfedge_edge_generator.cpp | 2 +- tool_path_planner/src/utilities.cpp | 44 +++++++++++++++++++ 6 files changed, 53 insertions(+), 2 deletions(-) diff --git a/tool_path_planner/include/tool_path_planner/eigen_value_edge_generator.h b/tool_path_planner/include/tool_path_planner/eigen_value_edge_generator.h index 459f16e9..b83dd2d7 100644 --- a/tool_path_planner/include/tool_path_planner/eigen_value_edge_generator.h +++ b/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_lenth = 1.0; /** @brief maximum segment length */ }; EigenValueEdgeGenerator() = default; diff --git a/tool_path_planner/include/tool_path_planner/halfedge_edge_generator.h b/tool_path_planner/include/tool_path_planner/halfedge_edge_generator.h index 5be0e000..8fad9cc0 100644 --- a/tool_path_planner/include/tool_path_planner/halfedge_edge_generator.h +++ b/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_lenth = 1.0; }; HalfedgeEdgeGenerator() = default; diff --git a/tool_path_planner/include/tool_path_planner/utilities.h b/tool_path_planner/include/tool_path_planner/utilities.h index d2e38d82..4e4be7c7 100644 --- a/tool_path_planner/include/tool_path_planner/utilities.h +++ b/tool_path_planner/include/tool_path_planner/utilities.h @@ -118,6 +118,8 @@ static std::string getClassName() return (status == 0) ? res.get() : mangled_name; } +ToolPaths splitSegments(ToolPaths tool_paths, double max_segment_length); + } // namespace tool_path_planner #endif /* INCLUDE_TOOL_PATH_PLANNER_UTILITIES_H_ */ diff --git a/tool_path_planner/src/eigen_value_edge_generator.cpp b/tool_path_planner/src/eigen_value_edge_generator.cpp index d411dde7..57d54b85 100644 --- a/tool_path_planner/src/eigen_value_edge_generator.cpp +++ b/tool_path_planner/src/eigen_value_edge_generator.cpp @@ -330,7 +330,7 @@ boost::optional EigenValueEdgeGenerator::generate() edge_paths.push_back({ edge_segment }); } } - return edge_paths; + return tool_path_planner::splitSegments(edge_paths, config_.max_segment_lenth); } void EigenValueEdgeGenerator::setup() diff --git a/tool_path_planner/src/halfedge_edge_generator.cpp b/tool_path_planner/src/halfedge_edge_generator.cpp index beeb58b5..4c7a8de7 100644 --- a/tool_path_planner/src/halfedge_edge_generator.cpp +++ b/tool_path_planner/src/halfedge_edge_generator.cpp @@ -581,7 +581,7 @@ boost::optional HalfedgeEdgeGenerator::generate() } CONSOLE_BRIDGE_logInform("Found %lu valid edge segments", edge_paths.size()); - return edge_paths; + return tool_path_planner::splitSegments(edge_paths, config_.max_segment_lenth); } } /* namespace tool_path_planner */ diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index e5a74ae1..fc6463cb 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -298,4 +298,48 @@ bool createToolPathSegment(const pcl::PointCloud& cloud_normal return true; } +// this is a helper function for splitPaths() +double getDistance(Eigen::Isometry3d t1, Eigen::Isometry3d t2) +{ + auto p1 = t1.translation(); + auto p2 = t2.translation(); + return sqrt(pow(p1.x() - p2.x(), 2) + pow(p1.y() - p2.y(), 2) + pow(p1.z() - p2.z(), 2)); +} + +ToolPaths splitSegments(ToolPaths tool_paths, double max_segment_length) +{ + ToolPaths new_tool_paths; + for (auto tool_path : tool_paths) + { + ToolPath new_tool_path; + for (auto seg : tool_path) + { + // calculate total segment length + double total_segment_length = 0.0; + for (std::size_t point_i = 0; point_i < seg.size()-1; ++point_i) + { + total_segment_length += getDistance(seg[point_i], seg[point_i+1]); + } + int num_cuts = int(ceil(total_segment_length / max_segment_length)); + double segment_length = total_segment_length / num_cuts; + double dist_from_start = 0.0; + std::size_t point_i = 1; + for (int cut_i = 0; cut_i < num_cuts; ++cut_i) + { + ToolPathSegment new_seg; + new_seg.push_back(seg[point_i-1]); + while (dist_from_start < segment_length * (cut_i+1) && point_i < seg.size()) + { + new_seg.push_back(seg[point_i]); + dist_from_start += getDistance(seg[point_i-1], seg[point_i]); + point_i += 1; + } + new_tool_path.push_back(new_seg); + } + } + new_tool_paths.push_back(new_tool_path); + } + return new_tool_paths; +} + } // namespace tool_path_planner From a52ac5a1af3f220c3191af31fb6707d8f487866e Mon Sep 17 00:00:00 2001 From: "Dale, Steven P" Date: Thu, 17 Dec 2020 17:10:15 -0600 Subject: [PATCH 04/64] added max_segment_length msg params --- noether_msgs/msg/EigenValueEdgeGeneratorConfig.msg | 3 +++ noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg | 3 +++ tool_path_planner/src/utilities.cpp | 1 + 3 files changed, 7 insertions(+) diff --git a/noether_msgs/msg/EigenValueEdgeGeneratorConfig.msg b/noether_msgs/msg/EigenValueEdgeGeneratorConfig.msg index f4949429..b82ae79d 100644 --- a/noether_msgs/msg/EigenValueEdgeGeneratorConfig.msg +++ b/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 diff --git a/noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg b/noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg index 07892f88..45788338 100644 --- a/noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg +++ b/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 diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index fc6463cb..3e539fb1 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -139,6 +139,7 @@ bool toEigenValueConfigMsg(noether_msgs::EigenValueEdgeGeneratorConfig& config_m config_msg.min_projection_dist = config.min_projection_dist; config_msg.max_intersecting_voxels = config.max_intersecting_voxels; config_msg.merge_dist = config.merge_dist; + config_msg.max_segment_length = config.max_segment_lenth; return true; } From 12181b38527182410b86314680300ca8ce906d28 Mon Sep 17 00:00:00 2001 From: "Dale, Steven P" Date: Fri, 18 Dec 2020 09:39:30 -0600 Subject: [PATCH 05/64] fixed lenth->length typo and made splitSegments turn off for max_segments_legnths <= 0 --- .../tool_path_planner/eigen_value_edge_generator.h | 2 +- .../tool_path_planner/halfedge_edge_generator.h | 2 +- tool_path_planner/src/eigen_value_edge_generator.cpp | 10 +++++++++- tool_path_planner/src/halfedge_edge_generator.cpp | 8 +++++++- tool_path_planner/src/utilities.cpp | 2 +- 5 files changed, 19 insertions(+), 5 deletions(-) diff --git a/tool_path_planner/include/tool_path_planner/eigen_value_edge_generator.h b/tool_path_planner/include/tool_path_planner/eigen_value_edge_generator.h index b83dd2d7..d10c9f33 100644 --- a/tool_path_planner/include/tool_path_planner/eigen_value_edge_generator.h +++ b/tool_path_planner/include/tool_path_planner/eigen_value_edge_generator.h @@ -100,7 +100,7 @@ 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_lenth = 1.0; /** @brief maximum segment length */ + double max_segment_length = 1.0; /** @brief maximum segment length */ }; EigenValueEdgeGenerator() = default; diff --git a/tool_path_planner/include/tool_path_planner/halfedge_edge_generator.h b/tool_path_planner/include/tool_path_planner/halfedge_edge_generator.h index 8fad9cc0..4924fde0 100644 --- a/tool_path_planner/include/tool_path_planner/halfedge_edge_generator.h +++ b/tool_path_planner/include/tool_path_planner/halfedge_edge_generator.h @@ -68,7 +68,7 @@ class HalfedgeEdgeGenerator : public PathGenerator double point_dist = 0.01; /** @brief maximum segment length */ - double max_segment_lenth = 1.0; + double max_segment_length = 1.0; }; HalfedgeEdgeGenerator() = default; diff --git a/tool_path_planner/src/eigen_value_edge_generator.cpp b/tool_path_planner/src/eigen_value_edge_generator.cpp index 57d54b85..fd6e73c8 100644 --- a/tool_path_planner/src/eigen_value_edge_generator.cpp +++ b/tool_path_planner/src/eigen_value_edge_generator.cpp @@ -330,7 +330,15 @@ boost::optional EigenValueEdgeGenerator::generate() edge_paths.push_back({ edge_segment }); } } - return tool_path_planner::splitSegments(edge_paths, config_.max_segment_lenth); + + 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; } void EigenValueEdgeGenerator::setup() diff --git a/tool_path_planner/src/halfedge_edge_generator.cpp b/tool_path_planner/src/halfedge_edge_generator.cpp index 4c7a8de7..b6979f45 100644 --- a/tool_path_planner/src/halfedge_edge_generator.cpp +++ b/tool_path_planner/src/halfedge_edge_generator.cpp @@ -580,8 +580,14 @@ boost::optional 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 tool_path_planner::splitSegments(edge_paths, config_.max_segment_lenth); + + return edge_paths; } } /* namespace tool_path_planner */ diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index 3e539fb1..3d47b79b 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -139,7 +139,7 @@ bool toEigenValueConfigMsg(noether_msgs::EigenValueEdgeGeneratorConfig& config_m config_msg.min_projection_dist = config.min_projection_dist; config_msg.max_intersecting_voxels = config.max_intersecting_voxels; config_msg.merge_dist = config.merge_dist; - config_msg.max_segment_length = config.max_segment_lenth; + config_msg.max_segment_length = config.max_segment_length; return true; } From 40551b3593421f6e6019b6417c1ed062ffa9593c Mon Sep 17 00:00:00 2001 From: Chris Lewis Date: Fri, 15 Jan 2021 16:15:01 -0600 Subject: [PATCH 06/64] added generate_extra_rasters capability to the plane slicer raster planner --- .../msg/PlaneSlicerRasterGeneratorConfig.msg | 13 ++-- .../plane_slicer_raster_generator.h | 3 +- .../include/tool_path_planner/utilities.h | 4 ++ .../src/plane_slicer_raster_generator.cpp | 10 ++- .../src/surface_walk_raster_generator.cpp | 2 +- tool_path_planner/src/utilities.cpp | 64 +++++++++++++++++++ 6 files changed, 86 insertions(+), 10 deletions(-) diff --git a/noether_msgs/msg/PlaneSlicerRasterGeneratorConfig.msg b/noether_msgs/msg/PlaneSlicerRasterGeneratorConfig.msg index db7a14e9..1b9dfe3b 100644 --- a/noether_msgs/msg/PlaneSlicerRasterGeneratorConfig.msg +++ b/noether_msgs/msg/PlaneSlicerRasterGeneratorConfig.msg @@ -1,9 +1,10 @@ -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 +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 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 48e257ae..454045c0 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 @@ -47,6 +47,7 @@ class PlaneSlicerRasterGenerator : public PathGenerator 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 @@ -59,7 +60,7 @@ class PlaneSlicerRasterGenerator : public PathGenerator 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); diff --git a/tool_path_planner/include/tool_path_planner/utilities.h b/tool_path_planner/include/tool_path_planner/utilities.h index 4e4be7c7..19048c57 100644 --- a/tool_path_planner/include/tool_path_planner/utilities.h +++ b/tool_path_planner/include/tool_path_planner/utilities.h @@ -120,6 +120,10 @@ static std::string getClassName() 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_ */ diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index c9515806..740c1653 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -409,7 +409,6 @@ void PlaneSlicerRasterGenerator::setInput(vtkSmartPointer mesh) } else { - CONSOLE_BRIDGE_logWarn("%s generating normal data", getName().c_str()); vtkSmartPointer normal_generator = vtkSmartPointer::New(); normal_generator->SetInputData(mesh_data_); normal_generator->ComputePointNormalsOn(); @@ -715,7 +714,14 @@ boost::optional 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; } diff --git a/tool_path_planner/src/surface_walk_raster_generator.cpp b/tool_path_planner/src/surface_walk_raster_generator.cpp index c6a3b50f..16770612 100644 --- a/tool_path_planner/src/surface_walk_raster_generator.cpp +++ b/tool_path_planner/src/surface_walk_raster_generator.cpp @@ -496,7 +496,7 @@ int findClosestPoint(const std::vector& pt, const std::vector1) + { + ToolPath tp1 = tool_paths[0]; + ToolPath tp2 = tool_paths[1]; + ToolPathSegment s1 = tp1[0]; + ToolPathSegment s2 = tp2[0]; + Eigen::Isometry3d waypoint1 = s1[0]; + Eigen::Isometry3d waypoint2 = s2[s2.size()-1]; + Eigen::Vector3d v = waypoint2.translation() - waypoint1.translation(); // vector between first waypoint in each path + Eigen::Matrix4d H = waypoint1.matrix(); + double dot_product = v.x()*H(0,1) + v.y()*H(1,1) + v.z()*H(2,1); + if(dot_product > 0.0) offset_sign = -1; + } + for(ToolPathSegment seg : temp_path) + { + ToolPathSegment new_segment; + for(int i=seg.size()-1; i>=0; i--) + { + Eigen::Isometry3d waypoint = seg[i]; + Eigen::Matrix4d H = waypoint.matrix(); + waypoint.translation().x() += offset_sign * offset_distance * H(0,1); + waypoint.translation().y() += offset_sign * offset_distance * H(1,1); + waypoint.translation().z() += offset_sign * offset_distance * H(2,1); + new_segment.push_back(waypoint); + } + first_dup_tool_path.push_back(new_segment); + } + new_tool_paths.push_back(first_dup_tool_path); + for (auto tool_path : tool_paths) + { + new_tool_paths.push_back(tool_path); + } + ToolPath last_dup_tool_path;// this tool path mimics last tool_path, but offset by +y and in reverse order + + // even numbers of paths require flipping offset direction + if(tool_paths.size()%2) offset_sign = offset_sign * -1; + + temp_path = tool_paths[tool_paths.size()-1]; + for(ToolPathSegment seg : temp_path) + { + ToolPathSegment new_segment; + for(int i=seg.size()-1; i>=0; i--) + { + Eigen::Isometry3d waypoint = seg[i]; + Eigen::Matrix4d H = waypoint.matrix(); + waypoint.translation().x() += offset_sign * offset_distance * H(0,1); + waypoint.translation().y() += offset_sign * offset_distance * H(1,1); + waypoint.translation().z() += offset_sign * offset_distance * H(2,1); + new_segment.push_back(waypoint); + } + last_dup_tool_path.push_back(new_segment); + } + new_tool_paths.push_back(last_dup_tool_path); + return new_tool_paths; +} + } // namespace tool_path_planner From 54dffd56653bde696b2b82528714610b550f1f74 Mon Sep 17 00:00:00 2001 From: Chris Lewis Date: Fri, 15 Jan 2021 16:23:25 -0600 Subject: [PATCH 07/64] clang format --- .../plane_slicer_raster_generator.h | 2 +- .../include/tool_path_planner/utilities.h | 3 +- .../src/plane_slicer_raster_generator.cpp | 13 +-- .../src/surface_walk_raster_generator.cpp | 6 +- tool_path_planner/src/utilities.cpp | 95 ++++++++++--------- 5 files changed, 63 insertions(+), 56 deletions(-) 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 454045c0..0ba30be5 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 @@ -60,7 +60,7 @@ class PlaneSlicerRasterGenerator : public PathGenerator 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}; + bool generate_extra_rasters{ DEFAULT_GENERATE_EXTRA_RASTERS }; Json::Value toJson() const { Json::Value jv(Json::ValueType::objectValue); diff --git a/tool_path_planner/include/tool_path_planner/utilities.h b/tool_path_planner/include/tool_path_planner/utilities.h index 19048c57..0749e449 100644 --- a/tool_path_planner/include/tool_path_planner/utilities.h +++ b/tool_path_planner/include/tool_path_planner/utilities.h @@ -122,8 +122,7 @@ 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_ */ diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index 740c1653..d76d5169 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -714,12 +714,13 @@ boost::optional PlaneSlicerRasterGenerator::generate() } // converting to poses msg now - if(config_.generate_extra_rasters) - { - ToolPaths temp_rasters = convertToPoses(rasters_data_vec); - rasters = addExtraPaths(temp_rasters, config_.raster_spacing); - } - else{ + 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; diff --git a/tool_path_planner/src/surface_walk_raster_generator.cpp b/tool_path_planner/src/surface_walk_raster_generator.cpp index 16770612..1b1d9fc8 100644 --- a/tool_path_planner/src/surface_walk_raster_generator.cpp +++ b/tool_path_planner/src/surface_walk_raster_generator.cpp @@ -496,7 +496,11 @@ int findClosestPoint(const std::vector& pt, const std::vector1) - { - ToolPath tp1 = tool_paths[0]; - ToolPath tp2 = tool_paths[1]; - ToolPathSegment s1 = tp1[0]; - ToolPathSegment s2 = tp2[0]; - Eigen::Isometry3d waypoint1 = s1[0]; - Eigen::Isometry3d waypoint2 = s2[s2.size()-1]; - Eigen::Vector3d v = waypoint2.translation() - waypoint1.translation(); // vector between first waypoint in each path - Eigen::Matrix4d H = waypoint1.matrix(); - double dot_product = v.x()*H(0,1) + v.y()*H(1,1) + v.z()*H(2,1); - if(dot_product > 0.0) offset_sign = -1; - } - for(ToolPathSegment seg : temp_path) + if (tool_paths.size() > 1) + { + ToolPath tp1 = tool_paths[0]; + ToolPath tp2 = tool_paths[1]; + ToolPathSegment s1 = tp1[0]; + ToolPathSegment s2 = tp2[0]; + Eigen::Isometry3d waypoint1 = s1[0]; + Eigen::Isometry3d waypoint2 = s2[s2.size() - 1]; + Eigen::Vector3d v = + waypoint2.translation() - waypoint1.translation(); // vector between first waypoint in each path + Eigen::Matrix4d H = waypoint1.matrix(); + double dot_product = v.x() * H(0, 1) + v.y() * H(1, 1) + v.z() * H(2, 1); + if (dot_product > 0.0) + offset_sign = -1; + } + for (ToolPathSegment seg : temp_path) + { + ToolPathSegment new_segment; + for (int i = seg.size() - 1; i >= 0; i--) { - ToolPathSegment new_segment; - for(int i=seg.size()-1; i>=0; i--) - { - Eigen::Isometry3d waypoint = seg[i]; - Eigen::Matrix4d H = waypoint.matrix(); - waypoint.translation().x() += offset_sign * offset_distance * H(0,1); - waypoint.translation().y() += offset_sign * offset_distance * H(1,1); - waypoint.translation().z() += offset_sign * offset_distance * H(2,1); - new_segment.push_back(waypoint); - } - first_dup_tool_path.push_back(new_segment); + Eigen::Isometry3d waypoint = seg[i]; + Eigen::Matrix4d H = waypoint.matrix(); + waypoint.translation().x() += offset_sign * offset_distance * H(0, 1); + waypoint.translation().y() += offset_sign * offset_distance * H(1, 1); + waypoint.translation().z() += offset_sign * offset_distance * H(2, 1); + new_segment.push_back(waypoint); } + first_dup_tool_path.push_back(new_segment); + } new_tool_paths.push_back(first_dup_tool_path); for (auto tool_path : tool_paths) { new_tool_paths.push_back(tool_path); } - ToolPath last_dup_tool_path;// this tool path mimics last tool_path, but offset by +y and in reverse order + ToolPath last_dup_tool_path; // this tool path mimics last tool_path, but offset by +y and in reverse order // even numbers of paths require flipping offset direction - if(tool_paths.size()%2) offset_sign = offset_sign * -1; + if (tool_paths.size() % 2) + offset_sign = offset_sign * -1; - temp_path = tool_paths[tool_paths.size()-1]; - for(ToolPathSegment seg : temp_path) + temp_path = tool_paths[tool_paths.size() - 1]; + for (ToolPathSegment seg : temp_path) + { + ToolPathSegment new_segment; + for (int i = seg.size() - 1; i >= 0; i--) { - ToolPathSegment new_segment; - for(int i=seg.size()-1; i>=0; i--) - { - Eigen::Isometry3d waypoint = seg[i]; - Eigen::Matrix4d H = waypoint.matrix(); - waypoint.translation().x() += offset_sign * offset_distance * H(0,1); - waypoint.translation().y() += offset_sign * offset_distance * H(1,1); - waypoint.translation().z() += offset_sign * offset_distance * H(2,1); - new_segment.push_back(waypoint); - } - last_dup_tool_path.push_back(new_segment); + Eigen::Isometry3d waypoint = seg[i]; + Eigen::Matrix4d H = waypoint.matrix(); + waypoint.translation().x() += offset_sign * offset_distance * H(0, 1); + waypoint.translation().y() += offset_sign * offset_distance * H(1, 1); + waypoint.translation().z() += offset_sign * offset_distance * H(2, 1); + new_segment.push_back(waypoint); } + last_dup_tool_path.push_back(new_segment); + } new_tool_paths.push_back(last_dup_tool_path); return new_tool_paths; } From 0a882466fd21d36308c8ba745a109cfc0e355b2c Mon Sep 17 00:00:00 2001 From: Chris Lewis Date: Tue, 19 Jan 2021 14:59:42 -0600 Subject: [PATCH 08/64] inprogress, but rebasing --- .../include/tool_path_planner/utilities.h | 1 + .../src/plane_slicer_raster_generator.cpp | 13 +-- tool_path_planner/src/utilities.cpp | 84 +++++++++++-------- 3 files changed, 57 insertions(+), 41 deletions(-) diff --git a/tool_path_planner/include/tool_path_planner/utilities.h b/tool_path_planner/include/tool_path_planner/utilities.h index 0749e449..fedfe94a 100644 --- a/tool_path_planner/include/tool_path_planner/utilities.h +++ b/tool_path_planner/include/tool_path_planner/utilities.h @@ -123,6 +123,7 @@ 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); +double computeOffsetSign( const ToolPathSegment& adjusted_segment, const ToolPathSegment& away_from_segment); } // 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 d76d5169..3fc2ba1d 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -337,7 +337,8 @@ static tool_path_planner::ToolPaths convertToPoses(const std::vector raster_segments; raster_segments.assign(rd.raster_segments.begin(), rd.raster_segments.end()); - if (reverse) + if(0) + // if (reverse)// reverse order of the segments { std::reverse(raster_segments.begin(), raster_segments.end()); } @@ -350,10 +351,6 @@ static tool_path_planner::ToolPaths convertToPoses(const std::vector indices(num_points); std::iota(indices.begin(), indices.end(), 0); - if (reverse) - { - std::reverse(indices.begin(), indices.end()); - } for (std::size_t i = 0; i < indices.size() - 1; i++) { int idx = indices[i]; @@ -372,6 +369,12 @@ static tool_path_planner::ToolPaths convertToPoses(const std::vector0.0) + offset_sign = -1; + return(offset_sign); +} + ToolPaths addExtraPaths(ToolPaths tool_paths, double offset_distance) { ToolPaths new_tool_paths; ToolPath first_dup_tool_path; // this tool path mimics first tool_path, but offset by -y and in reverse order ToolPath temp_path = tool_paths[0]; - double offset_sign = 1; + double offset_sign = 1.0; - // determine offset sign to point away from next path - if (tool_paths.size() > 1) - { - ToolPath tp1 = tool_paths[0]; - ToolPath tp2 = tool_paths[1]; - ToolPathSegment s1 = tp1[0]; - ToolPathSegment s2 = tp2[0]; - Eigen::Isometry3d waypoint1 = s1[0]; - Eigen::Isometry3d waypoint2 = s2[s2.size() - 1]; - Eigen::Vector3d v = - waypoint2.translation() - waypoint1.translation(); // vector between first waypoint in each path - Eigen::Matrix4d H = waypoint1.matrix(); - double dot_product = v.x() * H(0, 1) + v.y() * H(1, 1) + v.z() * H(2, 1); - if (dot_product > 0.0) - offset_sign = -1; - } + // duplicate first raster with an offset and add as first raster + std::reverse(temp_path.begin(), temp_path.end());// reverse the order of segments first for (ToolPathSegment seg : temp_path) - { - ToolPathSegment new_segment; - for (int i = seg.size() - 1; i >= 0; i--) { - Eigen::Isometry3d waypoint = seg[i]; - Eigen::Matrix4d H = waypoint.matrix(); - waypoint.translation().x() += offset_sign * offset_distance * H(0, 1); - waypoint.translation().y() += offset_sign * offset_distance * H(1, 1); - waypoint.translation().z() += offset_sign * offset_distance * H(2, 1); - new_segment.push_back(waypoint); + if (tool_paths.size() > 1) + { + ToolPath tp2 = tool_paths[1]; + offset_sign = computeOffsetSign(seg,tp2[0]); + } + ToolPathSegment new_segment; + for (int i = seg.size() - 1; i >= 0; i--) + { + Eigen::Isometry3d waypoint = seg[i]; + Eigen::Matrix4d H = waypoint.matrix(); + waypoint.translation().x() += offset_sign * offset_distance * H(0, 1); + waypoint.translation().y() += offset_sign * offset_distance * H(1, 1); + waypoint.translation().z() += offset_sign * offset_distance * H(2, 1); + new_segment.push_back(waypoint); + } + first_dup_tool_path.push_back(new_segment); } - first_dup_tool_path.push_back(new_segment); - } new_tool_paths.push_back(first_dup_tool_path); - for (auto tool_path : tool_paths) - { - new_tool_paths.push_back(tool_path); - } - ToolPath last_dup_tool_path; // this tool path mimics last tool_path, but offset by +y and in reverse order - // even numbers of paths require flipping offset direction - if (tool_paths.size() % 2) - offset_sign = offset_sign * -1; + // add all the existing rasters to the new tool_path + for (auto tool_path : tool_paths) + { + new_tool_paths.push_back(tool_path); + } + // duplicate last raster with an offset and add as last raster + offset_sign = -offset_sign; + ToolPath last_dup_tool_path; // this tool path mimics last tool_path, but offset by +y and in reverse order temp_path = tool_paths[tool_paths.size() - 1]; + std::reverse(temp_path.begin(), temp_path.end()); // reverse the segment order first for (ToolPathSegment seg : temp_path) { + if (tool_paths.size() > 1) + { + ToolPath tp2 = tool_paths[tool_paths.size()-1]; + offset_sign = computeOffsetSign(seg,tp2[0]); + } + ToolPathSegment new_segment; for (int i = seg.size() - 1; i >= 0; i--) { @@ -407,6 +418,7 @@ ToolPaths addExtraPaths(ToolPaths tool_paths, double offset_distance) last_dup_tool_path.push_back(new_segment); } new_tool_paths.push_back(last_dup_tool_path); + return new_tool_paths; } From 8693919e2b5098092bd2cf80345eeaa4791e069e Mon Sep 17 00:00:00 2001 From: Chris Lewis Date: Fri, 22 Jan 2021 06:25:56 -0600 Subject: [PATCH 09/64] first compile of bug fix for raster across holes switching directions of plane slicer --- .../plane_slicer_raster_generator.h | 1 + .../src/plane_slicer_raster_generator.cpp | 67 ++++++++++++++++++- 2 files changed, 67 insertions(+), 1 deletion(-) 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 0ba30be5..c23785f4 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 @@ -182,6 +182,7 @@ class PlaneSlicerRasterGenerator : public PathGenerator std::string getName() const override; + private: bool insertNormals(const double search_radius, vtkSmartPointer& data); diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index 3fc2ba1d..3e0ee507 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -62,6 +62,65 @@ struct RasterConstructData std::vector segment_lengths; }; + +// @brief this function accepts and returns a rasterConstruct where every segment progresses in the same direction and in the right order +static RasterConstructData alignRasterCD(const RasterConstructData rcd) +{ + if(rcd.raster_segments.size() == 1) return(rcd); // do nothing if only one segment + + // determine raster direction from 1st segment, and origin as first point in first raster (raster_start) + Eigen::Vector3d raster_start, raster_end, raster_direction; + rcd.raster_segments[0]->GetPoint(0,raster_start.data()); + rcd.raster_segments[0]->GetPoint(rcd.raster_segments[0]->GetPoints()->GetNumberOfPoints()-1,raster_end.data()); + raster_direction = raster_end - raster_start; + raster_direction.normalize(); + + // determine location and direction of each successive segement, reverse any mis-directed segments + RasterConstructData temp_rcd; + std::list > seg_order; // once sorted this list will be the order of the segments + std::tuple p(0.0,0); + seg_order.push_back(p); + for(size_t i=1; iGetPoint(0,seg_start.data()); + rcd.raster_segments[i]->GetPoint(rcd.raster_segments[i]->GetPoints()->GetNumberOfPoints()-1, seg_end.data()); + seg_dir = seg_end - seg_start; + + // if segment direction is opposite raster direction, reverse the segment + if(seg_dir.dot(raster_direction) < 0.0) + { + vtkSmartPointer old_points = rcd.raster_segments[i]->GetPoints(); + vtkSmartPointer new_points; + for(int j=rcd.raster_segments[i]->GetPoints()->GetNumberOfPoints()-1; j>=0; j--) + { + new_points->InsertNextPoint(old_points->GetPoint(j)); + } + rcd.raster_segments[i]->SetPoints(new_points); + Eigen::Vector3d temp = seg_start; + seg_start = seg_end; + seg_end = temp; + } + + // determine location of this segment in raster + double seg_loc = (seg_start - raster_start).dot(raster_direction); + std::tuple p(seg_loc, i); + seg_order.push_back(p); + } + + // sort the segments by location + seg_order.sort(); + RasterConstructData new_rcd; + for(std::tuple p: seg_order) + { + size_t seg_index = std::get<1>(p); + new_rcd.raster_segments.push_back(rcd.raster_segments[seg_index]); + new_rcd.segment_lengths.push_back(rcd.segment_lengths[seg_index]); + } + return(new_rcd); +} + static Eigen::Matrix3d computeRotation(const Eigen::Vector3d& vx, const Eigen::Vector3d& vy, const Eigen::Vector3d& vz) { Eigen::Matrix3d m; @@ -712,10 +771,16 @@ boost::optional PlaneSlicerRasterGenerator::generate() r.segment_lengths.push_back(line_length); } } - + rasters_data_vec.push_back(r); } + // make sure every raster has its segments ordered and aligned correctly + for(RasterConstructData rcd : rasters_data_vec) + { + rcd = alignRasterCD(rcd); + } + // converting to poses msg now if (config_.generate_extra_rasters) { From 08c6a0041c14894c52106afb4946638abdf3f704 Mon Sep 17 00:00:00 2001 From: "David Merz, Jr" Date: Fri, 22 Jan 2021 17:14:22 -0600 Subject: [PATCH 10/64] Fix plane slicer axes for vertical parts and offsets for first slice --- .../src/plane_slicer_raster_generator.cpp | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index d76d5169..fcf44648 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -454,6 +454,7 @@ boost::optional PlaneSlicerRasterGenerator::generate() } // 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; + ROS_ERROR_STREAM(config_.raster_wrt_global_axes); if (config_.raster_wrt_global_axes) { // Determine extent of mesh along axes of current coordinate frame @@ -478,10 +479,16 @@ boost::optional PlaneSlicerRasterGenerator::generate() 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(); + // Order the axes. Computing y saves comparisons and guarantees right-handedness. + Eigen::Matrix3d rotation_transform; + rotation_transform.col(0) = extents[max].normalized(); + rotation_transform.col(2) = extents[min].normalized(); + rotation_transform.col(1) = rotation_transform.col(2).cross(rotation_transform.col(0)).normalized(); + + // Extract our axes transfored to align to the major axes of our aabb + x_dir = rotation_transform.row(0).normalized(); + y_dir = rotation_transform.row(1).normalized(); + z_dir = rotation_transform.row(2).normalized(); } else { @@ -522,7 +529,7 @@ boost::optional PlaneSlicerRasterGenerator::generate() sizes.z() = bounds[5] - bounds[4]; half_ext = sizes / 2.0; - center = Eigen::Vector3d(bounds[0], bounds[2], bounds[3]) + half_ext; + center = Eigen::Vector3d(bounds[0], bounds[2], bounds[4]) + half_ext; // 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()); @@ -752,7 +759,7 @@ bool PlaneSlicerRasterGenerator::insertNormals(const double search_radius, vtkSm kd_tree_->FindPointsWithinRadius(search_radius, query_point.data(), id_list); if (id_list->GetNumberOfIds() < 1) { - CONSOLE_BRIDGE_logWarn("%s FindPointsWithinRadius found no points for normal averaging, using closests", + CONSOLE_BRIDGE_logWarn("%s FindPointsWithinRadius found no points for normal averaging, using closest", getName().c_str()); kd_tree_->FindClosestNPoints(1, query_point.data(), id_list); From 3ac6cf98b5564443fefe800e274e9c8213b291fd Mon Sep 17 00:00:00 2001 From: Chris Lewis Date: Mon, 25 Jan 2021 09:04:06 -0600 Subject: [PATCH 11/64] reordered, but now all returned rasters flow in same order. TODO, add flag to set reverse raster style --- tool_path_planner/src/plane_slicer_raster_generator.cpp | 5 ++--- tool_path_planner/src/utilities.cpp | 4 ++-- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index 3e0ee507..117460d6 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -67,7 +67,6 @@ struct RasterConstructData static RasterConstructData alignRasterCD(const RasterConstructData rcd) { if(rcd.raster_segments.size() == 1) return(rcd); // do nothing if only one segment - // determine raster direction from 1st segment, and origin as first point in first raster (raster_start) Eigen::Vector3d raster_start, raster_end, raster_direction; rcd.raster_segments[0]->GetPoint(0,raster_start.data()); @@ -92,8 +91,8 @@ static RasterConstructData alignRasterCD(const RasterConstructData rcd) if(seg_dir.dot(raster_direction) < 0.0) { vtkSmartPointer old_points = rcd.raster_segments[i]->GetPoints(); - vtkSmartPointer new_points; - for(int j=rcd.raster_segments[i]->GetPoints()->GetNumberOfPoints()-1; j>=0; j--) + vtkSmartPointer new_points = vtkSmartPointer::New(); + for(long long int j=old_points->GetNumberOfPoints()-1; j>=0; j--) { new_points->InsertNextPoint(old_points->GetPoint(j)); } diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index a943be50..ff1d62f8 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -373,7 +373,7 @@ ToolPaths addExtraPaths(ToolPaths tool_paths, double offset_distance) offset_sign = computeOffsetSign(seg,tp2[0]); } ToolPathSegment new_segment; - for (int i = seg.size() - 1; i >= 0; i--) + for (int i = 0; i < seg.size(); i++) { Eigen::Isometry3d waypoint = seg[i]; Eigen::Matrix4d H = waypoint.matrix(); @@ -406,7 +406,7 @@ ToolPaths addExtraPaths(ToolPaths tool_paths, double offset_distance) } ToolPathSegment new_segment; - for (int i = seg.size() - 1; i >= 0; i--) + for (int i = 0; i < seg.size(); i++) { Eigen::Isometry3d waypoint = seg[i]; Eigen::Matrix4d H = waypoint.matrix(); From 308194e445c2e94c8b5e819722506b69e13a58f0 Mon Sep 17 00:00:00 2001 From: Chris Lewis Date: Mon, 25 Jan 2021 15:43:07 -0600 Subject: [PATCH 12/64] added function to reverse odd rasters, and a config to enable/disable flip orientation on odd rasters --- .../tool_path_planner/path_generator.h | 7 +++ .../plane_slicer_raster_generator.h | 5 ++- .../include/tool_path_planner/utilities.h | 10 ++++- .../src/plane_slicer_raster_generator.cpp | 16 +++---- tool_path_planner/src/utilities.cpp | 44 +++++++++++++++++-- 5 files changed, 68 insertions(+), 14 deletions(-) diff --git a/tool_path_planner/include/tool_path_planner/path_generator.h b/tool_path_planner/include/tool_path_planner/path_generator.h index 6816684b..cf3b2235 100644 --- a/tool_path_planner/include/tool_path_planner/path_generator.h +++ b/tool_path_planner/include/tool_path_planner/path_generator.h @@ -13,6 +13,13 @@ namespace tool_path_planner { + + enum RasterStyle: int + { + KEEP_ORIENTATION_ON_REVERSE_STROKES = 0, + FLIP_ORIENTATION_ON_REVERSE_STROKES = 1, + }; + /** @brief A set of contiguous waypoints that lie on the same line created by a "slice" through a surface */ using ToolPathSegment = std::vector>; /** @brief A set of tool path segments that lie on the same line created by a tool path "slice", 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 c23785f4..626ad2dd 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 @@ -33,13 +33,14 @@ #include #include #include - #include namespace tool_path_planner { + class PlaneSlicerRasterGenerator : public PathGenerator { + static constexpr RasterStyle DEFAULT_RASTER_STYLE = KEEP_ORIENTATION_ON_REVERSE_STROKES; static constexpr double DEFAULT_RASTER_SPACING = 0.04; static constexpr double DEFAULT_POINT_SPACING = 0.01; static constexpr double DEFAULT_RASTER_ROT_OFFSET = 0.0; @@ -49,6 +50,7 @@ class PlaneSlicerRasterGenerator : public PathGenerator static constexpr bool DEFAULT_RASTER_WRT_GLOBAL_AXES = false; static constexpr bool DEFAULT_GENERATE_EXTRA_RASTERS = true; + public: struct Config { @@ -61,6 +63,7 @@ class PlaneSlicerRasterGenerator : public PathGenerator 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 }; + RasterStyle raster_style{ DEFAULT_RASTER_STYLE }; Json::Value toJson() const { Json::Value jv(Json::ValueType::objectValue); diff --git a/tool_path_planner/include/tool_path_planner/utilities.h b/tool_path_planner/include/tool_path_planner/utilities.h index fedfe94a..e3819c7a 100644 --- a/tool_path_planner/include/tool_path_planner/utilities.h +++ b/tool_path_planner/include/tool_path_planner/utilities.h @@ -43,8 +43,10 @@ #include #include + namespace tool_path_planner { + /** * @brief flipPointOrder Inverts a path * @param path The input path to invert @@ -118,10 +120,14 @@ static std::string getClassName() return (status == 0) ? res.get() : mangled_name; } -ToolPaths splitSegments(ToolPaths tool_paths, double max_segment_length); +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); +ToolPaths addExtraPaths(ToolPaths& tool_paths, double offset_distance); + +// reverse direction of every other raster, flip orientation or not depending on raster style +ToolPaths reverseOddRasters(ToolPaths& tool_paths, RasterStyle raster_style); + double computeOffsetSign( const ToolPathSegment& adjusted_segment, const ToolPathSegment& away_from_segment); } // namespace tool_path_planner diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index 117460d6..64f6d3d8 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -508,7 +508,6 @@ boost::optional PlaneSlicerRasterGenerator::generate() using namespace Eigen; using IDVec = std::vector; - boost::optional rasters = boost::none; if (!mesh_data_) { CONSOLE_BRIDGE_logDebug("%s No mesh data has been provided", getName().c_str()); @@ -780,16 +779,17 @@ boost::optional PlaneSlicerRasterGenerator::generate() rcd = alignRasterCD(rcd); } - // converting to poses msg now + // converting to poses msg + ToolPaths 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); + rasters = addExtraPaths(rasters, config_.raster_spacing); } + + // switch directions of every other raster + rasters = reverseOddRasters(rasters, config_.raster_style); + return rasters; } diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index ff1d62f8..656ab5b4 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -307,10 +307,11 @@ double getDistance(Eigen::Isometry3d t1, Eigen::Isometry3d t2) return sqrt(pow(p1.x() - p2.x(), 2) + pow(p1.y() - p2.y(), 2) + pow(p1.z() - p2.z(), 2)); } -ToolPaths splitSegments(ToolPaths tool_paths, double max_segment_length) +ToolPaths splitSegments(ToolPaths& tool_paths, double max_segment_length) { + ToolPaths new_tool_paths; - for (auto tool_path : tool_paths) + for (ToolPath tool_path : tool_paths) { ToolPath new_tool_path; for (auto seg : tool_path) @@ -343,6 +344,43 @@ ToolPaths splitSegments(ToolPaths tool_paths, double max_segment_length) return new_tool_paths; } +ToolPaths reverseOddRasters(ToolPaths& tool_paths, RasterStyle raster_style) +{ + ToolPaths new_tool_paths; + int is_odd=0; + int q=0; + for (auto tool_path : tool_paths) + { + ToolPath new_tool_path; + if(!is_odd) + { + for (auto seg : tool_path) + { + new_tool_path.push_back(seg); + } + }// end if !is_odd + else + { // reverse order of segments, + for (long int i=tool_path.size()-1; i>=0; i--) + { + ToolPathSegment new_segment; + for (long int j= tool_path[i].size()-1; j >=0 ; j--) + { + // rotate around z-axis of waypoint by 180 degrees (PI) + Eigen::Isometry3d waypoint = tool_path[i][j]; + if(raster_style == FLIP_ORIENTATION_ON_REVERSE_STROKES) + waypoint.rotate(Eigen::AngleAxisd(4*atan(1), Eigen::Vector3d::UnitZ())); + new_segment.push_back(waypoint); + } + new_tool_path.push_back(new_segment); + } + } + new_tool_paths.push_back(new_tool_path); + is_odd = (is_odd+1)%2; + } + return new_tool_paths; +} + double computeOffsetSign( const ToolPathSegment& adjusted_segment, const ToolPathSegment& away_from_segment) { double offset_sign = 1; @@ -356,7 +394,7 @@ double computeOffsetSign( const ToolPathSegment& adjusted_segment, const ToolPa return(offset_sign); } -ToolPaths addExtraPaths(ToolPaths tool_paths, double offset_distance) +ToolPaths addExtraPaths(ToolPaths& tool_paths, double offset_distance) { ToolPaths new_tool_paths; ToolPath first_dup_tool_path; // this tool path mimics first tool_path, but offset by -y and in reverse order From f804f2354cb8b42ece62da6e9a042826a35847a1 Mon Sep 17 00:00:00 2001 From: Chris Lewis Date: Mon, 25 Jan 2021 15:55:14 -0600 Subject: [PATCH 13/64] clang --- .../tool_path_planner/path_generator.h | 13 +-- .../plane_slicer_raster_generator.h | 3 - .../include/tool_path_planner/utilities.h | 5 +- .../src/plane_slicer_raster_generator.cpp | 107 +++++++++--------- tool_path_planner/src/utilities.cpp | 107 +++++++++--------- 5 files changed, 114 insertions(+), 121 deletions(-) diff --git a/tool_path_planner/include/tool_path_planner/path_generator.h b/tool_path_planner/include/tool_path_planner/path_generator.h index cf3b2235..caa42da6 100644 --- a/tool_path_planner/include/tool_path_planner/path_generator.h +++ b/tool_path_planner/include/tool_path_planner/path_generator.h @@ -13,13 +13,12 @@ namespace tool_path_planner { - - enum RasterStyle: int - { - KEEP_ORIENTATION_ON_REVERSE_STROKES = 0, - FLIP_ORIENTATION_ON_REVERSE_STROKES = 1, - }; - +enum RasterStyle : int +{ + KEEP_ORIENTATION_ON_REVERSE_STROKES = 0, + FLIP_ORIENTATION_ON_REVERSE_STROKES = 1, +}; + /** @brief A set of contiguous waypoints that lie on the same line created by a "slice" through a surface */ using ToolPathSegment = std::vector>; /** @brief A set of tool path segments that lie on the same line created by a tool path "slice", 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 626ad2dd..20fe74d3 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 @@ -37,7 +37,6 @@ namespace tool_path_planner { - class PlaneSlicerRasterGenerator : public PathGenerator { static constexpr RasterStyle DEFAULT_RASTER_STYLE = KEEP_ORIENTATION_ON_REVERSE_STROKES; @@ -50,7 +49,6 @@ class PlaneSlicerRasterGenerator : public PathGenerator static constexpr bool DEFAULT_RASTER_WRT_GLOBAL_AXES = false; static constexpr bool DEFAULT_GENERATE_EXTRA_RASTERS = true; - public: struct Config { @@ -185,7 +183,6 @@ class PlaneSlicerRasterGenerator : public PathGenerator std::string getName() const override; - private: bool insertNormals(const double search_radius, vtkSmartPointer& data); diff --git a/tool_path_planner/include/tool_path_planner/utilities.h b/tool_path_planner/include/tool_path_planner/utilities.h index e3819c7a..6e6ae8d3 100644 --- a/tool_path_planner/include/tool_path_planner/utilities.h +++ b/tool_path_planner/include/tool_path_planner/utilities.h @@ -43,10 +43,8 @@ #include #include - namespace tool_path_planner { - /** * @brief flipPointOrder Inverts a path * @param path The input path to invert @@ -128,8 +126,7 @@ ToolPaths addExtraPaths(ToolPaths& tool_paths, double offset_distance); // reverse direction of every other raster, flip orientation or not depending on raster style ToolPaths reverseOddRasters(ToolPaths& tool_paths, RasterStyle raster_style); - -double computeOffsetSign( const ToolPathSegment& adjusted_segment, const ToolPathSegment& away_from_segment); +double computeOffsetSign(const ToolPathSegment& adjusted_segment, const ToolPathSegment& away_from_segment); } // 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 64f6d3d8..09d55088 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -62,62 +62,63 @@ struct RasterConstructData std::vector segment_lengths; }; - -// @brief this function accepts and returns a rasterConstruct where every segment progresses in the same direction and in the right order +// @brief this function accepts and returns a rasterConstruct where every segment progresses in the same direction and +// in the right order static RasterConstructData alignRasterCD(const RasterConstructData rcd) { - if(rcd.raster_segments.size() == 1) return(rcd); // do nothing if only one segment + if (rcd.raster_segments.size() == 1) + return (rcd); // do nothing if only one segment // determine raster direction from 1st segment, and origin as first point in first raster (raster_start) Eigen::Vector3d raster_start, raster_end, raster_direction; - rcd.raster_segments[0]->GetPoint(0,raster_start.data()); - rcd.raster_segments[0]->GetPoint(rcd.raster_segments[0]->GetPoints()->GetNumberOfPoints()-1,raster_end.data()); + rcd.raster_segments[0]->GetPoint(0, raster_start.data()); + rcd.raster_segments[0]->GetPoint(rcd.raster_segments[0]->GetPoints()->GetNumberOfPoints() - 1, raster_end.data()); raster_direction = raster_end - raster_start; raster_direction.normalize(); // determine location and direction of each successive segement, reverse any mis-directed segments RasterConstructData temp_rcd; - std::list > seg_order; // once sorted this list will be the order of the segments - std::tuple p(0.0,0); - seg_order.push_back(p); - for(size_t i=1; i > seg_order; // once sorted this list will be the order of the segments + std::tuple p(0.0, 0); + seg_order.push_back(p); + for (size_t i = 1; i < rcd.raster_segments.size(); i++) + { + // determine i'th segments direction + Eigen::Vector3d seg_start, seg_end, seg_dir; + rcd.raster_segments[i]->GetPoint(0, seg_start.data()); + rcd.raster_segments[i]->GetPoint(rcd.raster_segments[i]->GetPoints()->GetNumberOfPoints() - 1, seg_end.data()); + seg_dir = seg_end - seg_start; + + // if segment direction is opposite raster direction, reverse the segment + if (seg_dir.dot(raster_direction) < 0.0) { - // determine i'th segments direction - Eigen::Vector3d seg_start, seg_end, seg_dir; - rcd.raster_segments[i]->GetPoint(0,seg_start.data()); - rcd.raster_segments[i]->GetPoint(rcd.raster_segments[i]->GetPoints()->GetNumberOfPoints()-1, seg_end.data()); - seg_dir = seg_end - seg_start; - - // if segment direction is opposite raster direction, reverse the segment - if(seg_dir.dot(raster_direction) < 0.0) - { - vtkSmartPointer old_points = rcd.raster_segments[i]->GetPoints(); - vtkSmartPointer new_points = vtkSmartPointer::New(); - for(long long int j=old_points->GetNumberOfPoints()-1; j>=0; j--) - { - new_points->InsertNextPoint(old_points->GetPoint(j)); - } - rcd.raster_segments[i]->SetPoints(new_points); - Eigen::Vector3d temp = seg_start; - seg_start = seg_end; - seg_end = temp; - } - - // determine location of this segment in raster - double seg_loc = (seg_start - raster_start).dot(raster_direction); - std::tuple p(seg_loc, i); - seg_order.push_back(p); + vtkSmartPointer old_points = rcd.raster_segments[i]->GetPoints(); + vtkSmartPointer new_points = vtkSmartPointer::New(); + for (long long int j = old_points->GetNumberOfPoints() - 1; j >= 0; j--) + { + new_points->InsertNextPoint(old_points->GetPoint(j)); + } + rcd.raster_segments[i]->SetPoints(new_points); + Eigen::Vector3d temp = seg_start; + seg_start = seg_end; + seg_end = temp; } + // determine location of this segment in raster + double seg_loc = (seg_start - raster_start).dot(raster_direction); + std::tuple p(seg_loc, i); + seg_order.push_back(p); + } + // sort the segments by location - seg_order.sort(); + seg_order.sort(); RasterConstructData new_rcd; - for(std::tuple p: seg_order) - { - size_t seg_index = std::get<1>(p); - new_rcd.raster_segments.push_back(rcd.raster_segments[seg_index]); - new_rcd.segment_lengths.push_back(rcd.segment_lengths[seg_index]); - } - return(new_rcd); + for (std::tuple p : seg_order) + { + size_t seg_index = std::get<1>(p); + new_rcd.raster_segments.push_back(rcd.raster_segments[seg_index]); + new_rcd.segment_lengths.push_back(rcd.segment_lengths[seg_index]); + } + return (new_rcd); } static Eigen::Matrix3d computeRotation(const Eigen::Vector3d& vx, const Eigen::Vector3d& vy, const Eigen::Vector3d& vz) @@ -395,7 +396,7 @@ static tool_path_planner::ToolPaths convertToPoses(const std::vector raster_segments; raster_segments.assign(rd.raster_segments.begin(), rd.raster_segments.end()); - if(0) + if (0) // if (reverse)// reverse order of the segments { std::reverse(raster_segments.begin(), raster_segments.end()); @@ -427,8 +428,8 @@ static tool_path_planner::ToolPaths convertToPoses(const std::vector PlaneSlicerRasterGenerator::generate() r.segment_lengths.push_back(line_length); } } - + rasters_data_vec.push_back(r); } // make sure every raster has its segments ordered and aligned correctly - for(RasterConstructData rcd : rasters_data_vec) - { - rcd = alignRasterCD(rcd); - } - - // converting to poses msg + for (RasterConstructData rcd : rasters_data_vec) + { + rcd = alignRasterCD(rcd); + } + + // converting to poses msg ToolPaths rasters = convertToPoses(rasters_data_vec); - + if (config_.generate_extra_rasters) { rasters = addExtraPaths(rasters, config_.raster_spacing); diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index 656ab5b4..c55fd5e0 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -309,7 +309,6 @@ double getDistance(Eigen::Isometry3d t1, Eigen::Isometry3d t2) ToolPaths splitSegments(ToolPaths& tool_paths, double max_segment_length) { - ToolPaths new_tool_paths; for (ToolPath tool_path : tool_paths) { @@ -347,51 +346,51 @@ ToolPaths splitSegments(ToolPaths& tool_paths, double max_segment_length) ToolPaths reverseOddRasters(ToolPaths& tool_paths, RasterStyle raster_style) { ToolPaths new_tool_paths; - int is_odd=0; - int q=0; + int is_odd = 0; + int q = 0; for (auto tool_path : tool_paths) { ToolPath new_tool_path; - if(!is_odd) + if (!is_odd) + { + for (auto seg : tool_path) { - for (auto seg : tool_path) - { - new_tool_path.push_back(seg); - } - }// end if !is_odd + new_tool_path.push_back(seg); + } + } // end if !is_odd else - { // reverse order of segments, - for (long int i=tool_path.size()-1; i>=0; i--) - { - ToolPathSegment new_segment; - for (long int j= tool_path[i].size()-1; j >=0 ; j--) - { - // rotate around z-axis of waypoint by 180 degrees (PI) - Eigen::Isometry3d waypoint = tool_path[i][j]; - if(raster_style == FLIP_ORIENTATION_ON_REVERSE_STROKES) - waypoint.rotate(Eigen::AngleAxisd(4*atan(1), Eigen::Vector3d::UnitZ())); - new_segment.push_back(waypoint); - } - new_tool_path.push_back(new_segment); - } + { // reverse order of segments, + for (long int i = tool_path.size() - 1; i >= 0; i--) + { + ToolPathSegment new_segment; + for (long int j = tool_path[i].size() - 1; j >= 0; j--) + { + // rotate around z-axis of waypoint by 180 degrees (PI) + Eigen::Isometry3d waypoint = tool_path[i][j]; + if (raster_style == FLIP_ORIENTATION_ON_REVERSE_STROKES) + waypoint.rotate(Eigen::AngleAxisd(4 * atan(1), Eigen::Vector3d::UnitZ())); + new_segment.push_back(waypoint); + } + new_tool_path.push_back(new_segment); } + } new_tool_paths.push_back(new_tool_path); - is_odd = (is_odd+1)%2; + is_odd = (is_odd + 1) % 2; } return new_tool_paths; } -double computeOffsetSign( const ToolPathSegment& adjusted_segment, const ToolPathSegment& away_from_segment) +double computeOffsetSign(const ToolPathSegment& adjusted_segment, const ToolPathSegment& away_from_segment) { double offset_sign = 1; Eigen::Isometry3d waypoint1 = adjusted_segment[0]; - Eigen::Isometry3d waypoint2 = away_from_segment[0]; - Eigen::Vector3d v = waypoint2.translation() - waypoint1.translation(); + Eigen::Isometry3d waypoint2 = away_from_segment[0]; + Eigen::Vector3d v = waypoint2.translation() - waypoint1.translation(); Eigen::Matrix4d H = waypoint1.matrix(); double dot_product = v.x() * H(0, 1) + v.y() * H(1, 1) + v.z() * H(2, 1); - if (dot_product >0.0) + if (dot_product > 0.0) offset_sign = -1; - return(offset_sign); + return (offset_sign); } ToolPaths addExtraPaths(ToolPaths& tool_paths, double offset_distance) @@ -402,46 +401,46 @@ ToolPaths addExtraPaths(ToolPaths& tool_paths, double offset_distance) double offset_sign = 1.0; // duplicate first raster with an offset and add as first raster - std::reverse(temp_path.begin(), temp_path.end());// reverse the order of segments first + std::reverse(temp_path.begin(), temp_path.end()); // reverse the order of segments first for (ToolPathSegment seg : temp_path) + { + if (tool_paths.size() > 1) { - if (tool_paths.size() > 1) - { - ToolPath tp2 = tool_paths[1]; - offset_sign = computeOffsetSign(seg,tp2[0]); - } - ToolPathSegment new_segment; - for (int i = 0; i < seg.size(); i++) - { - Eigen::Isometry3d waypoint = seg[i]; - Eigen::Matrix4d H = waypoint.matrix(); - waypoint.translation().x() += offset_sign * offset_distance * H(0, 1); - waypoint.translation().y() += offset_sign * offset_distance * H(1, 1); - waypoint.translation().z() += offset_sign * offset_distance * H(2, 1); - new_segment.push_back(waypoint); - } - first_dup_tool_path.push_back(new_segment); + ToolPath tp2 = tool_paths[1]; + offset_sign = computeOffsetSign(seg, tp2[0]); } + ToolPathSegment new_segment; + for (int i = 0; i < seg.size(); i++) + { + Eigen::Isometry3d waypoint = seg[i]; + Eigen::Matrix4d H = waypoint.matrix(); + waypoint.translation().x() += offset_sign * offset_distance * H(0, 1); + waypoint.translation().y() += offset_sign * offset_distance * H(1, 1); + waypoint.translation().z() += offset_sign * offset_distance * H(2, 1); + new_segment.push_back(waypoint); + } + first_dup_tool_path.push_back(new_segment); + } new_tool_paths.push_back(first_dup_tool_path); // add all the existing rasters to the new tool_path for (auto tool_path : tool_paths) - { - new_tool_paths.push_back(tool_path); - } + { + new_tool_paths.push_back(tool_path); + } // duplicate last raster with an offset and add as last raster offset_sign = -offset_sign; ToolPath last_dup_tool_path; // this tool path mimics last tool_path, but offset by +y and in reverse order temp_path = tool_paths[tool_paths.size() - 1]; - std::reverse(temp_path.begin(), temp_path.end()); // reverse the segment order first + std::reverse(temp_path.begin(), temp_path.end()); // reverse the segment order first for (ToolPathSegment seg : temp_path) { if (tool_paths.size() > 1) - { - ToolPath tp2 = tool_paths[tool_paths.size()-1]; - offset_sign = computeOffsetSign(seg,tp2[0]); - } + { + ToolPath tp2 = tool_paths[tool_paths.size() - 1]; + offset_sign = computeOffsetSign(seg, tp2[0]); + } ToolPathSegment new_segment; for (int i = 0; i < seg.size(); i++) From 4ffd2722d24dbd22d7d09b0cb7d6e85b3eeebe61 Mon Sep 17 00:00:00 2001 From: Chris Lewis Date: Mon, 25 Jan 2021 16:09:02 -0600 Subject: [PATCH 14/64] removed some code between if(0) --- .../src/plane_slicer_raster_generator.cpp | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index 09d55088..026502d1 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -389,19 +389,11 @@ static tool_path_planner::ToolPaths convertToPoses(const std::vector raster_segments; raster_segments.assign(rd.raster_segments.begin(), rd.raster_segments.end()); - if (0) - // if (reverse)// reverse order of the segments - { - std::reverse(raster_segments.begin(), raster_segments.end()); - } - for (const PolyDataPtr& polydata : raster_segments) { tool_path_planner::ToolPathSegment raster_path_segment; @@ -428,12 +420,6 @@ static tool_path_planner::ToolPaths convertToPoses(const std::vector Date: Tue, 26 Jan 2021 08:57:13 -0600 Subject: [PATCH 15/64] minor changes to catch zero length cases --- .../include/tool_path_planner/utilities.h | 6 +++--- .../src/plane_slicer_raster_generator.cpp | 9 ++++++++- tool_path_planner/src/utilities.cpp | 10 +++++----- 3 files changed, 16 insertions(+), 9 deletions(-) diff --git a/tool_path_planner/include/tool_path_planner/utilities.h b/tool_path_planner/include/tool_path_planner/utilities.h index 6e6ae8d3..755215b7 100644 --- a/tool_path_planner/include/tool_path_planner/utilities.h +++ b/tool_path_planner/include/tool_path_planner/utilities.h @@ -118,13 +118,13 @@ static std::string getClassName() return (status == 0) ? res.get() : mangled_name; } -ToolPaths splitSegments(ToolPaths& tool_paths, double max_segment_length); +ToolPaths splitSegments(const 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); +ToolPaths addExtraPaths(const ToolPaths& tool_paths, double offset_distance); // reverse direction of every other raster, flip orientation or not depending on raster style -ToolPaths reverseOddRasters(ToolPaths& tool_paths, RasterStyle raster_style); +ToolPaths reverseOddRasters(const ToolPaths& tool_paths, RasterStyle raster_style); double computeOffsetSign(const ToolPathSegment& adjusted_segment, const ToolPathSegment& away_from_segment); } // namespace tool_path_planner diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index 026502d1..cadc2bdc 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -66,8 +66,15 @@ struct RasterConstructData // in the right order static RasterConstructData alignRasterCD(const RasterConstructData rcd) { - if (rcd.raster_segments.size() == 1) + if (rcd.raster_segments.size() <= 1) return (rcd); // do nothing if only one segment + + if(rcd.raster_segments[0]->GetPoints()->GetNumberOfPoints() ==0) + { + ROS_ERROR("first raster segment has zero points, unable to alignRasterCD()"); + return(rcd); + } + // determine raster direction from 1st segment, and origin as first point in first raster (raster_start) Eigen::Vector3d raster_start, raster_end, raster_direction; rcd.raster_segments[0]->GetPoint(0, raster_start.data()); diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index c55fd5e0..a1e6ff7e 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -307,7 +307,7 @@ double getDistance(Eigen::Isometry3d t1, Eigen::Isometry3d t2) return sqrt(pow(p1.x() - p2.x(), 2) + pow(p1.y() - p2.y(), 2) + pow(p1.z() - p2.z(), 2)); } -ToolPaths splitSegments(ToolPaths& tool_paths, double max_segment_length) +ToolPaths splitSegments(const ToolPaths& tool_paths, double max_segment_length) { ToolPaths new_tool_paths; for (ToolPath tool_path : tool_paths) @@ -343,10 +343,10 @@ ToolPaths splitSegments(ToolPaths& tool_paths, double max_segment_length) return new_tool_paths; } -ToolPaths reverseOddRasters(ToolPaths& tool_paths, RasterStyle raster_style) +ToolPaths reverseOddRasters(const ToolPaths& tool_paths, RasterStyle raster_style) { ToolPaths new_tool_paths; - int is_odd = 0; + bool is_odd = false; int q = 0; for (auto tool_path : tool_paths) { @@ -375,7 +375,7 @@ ToolPaths reverseOddRasters(ToolPaths& tool_paths, RasterStyle raster_style) } } new_tool_paths.push_back(new_tool_path); - is_odd = (is_odd + 1) % 2; + is_odd = !is_odd; } return new_tool_paths; } @@ -393,7 +393,7 @@ double computeOffsetSign(const ToolPathSegment& adjusted_segment, const ToolPath return (offset_sign); } -ToolPaths addExtraPaths(ToolPaths& tool_paths, double offset_distance) +ToolPaths addExtraPaths(const ToolPaths& tool_paths, double offset_distance) { ToolPaths new_tool_paths; ToolPath first_dup_tool_path; // this tool path mimics first tool_path, but offset by -y and in reverse order From acf9da40e42cf268c4c099fc2ae32a0f2206b896 Mon Sep 17 00:00:00 2001 From: Chris Lewis Date: Tue, 26 Jan 2021 09:07:23 -0600 Subject: [PATCH 16/64] minor improvement --- tool_path_planner/src/plane_slicer_raster_generator.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index cadc2bdc..ad1dff2a 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -69,9 +69,9 @@ static RasterConstructData alignRasterCD(const RasterConstructData rcd) if (rcd.raster_segments.size() <= 1) return (rcd); // do nothing if only one segment - if(rcd.raster_segments[0]->GetPoints()->GetNumberOfPoints() ==0) + if(rcd.raster_segments[0]->GetPoints()->GetNumberOfPoints() <=1) { - ROS_ERROR("first raster segment has zero points, unable to alignRasterCD()"); + ROS_ERROR("first raster segment has 0 or 1 points, unable to alignRasterCD()"); return(rcd); } From 37ccd05097c0b1a28da949b5a3a18e3ee91b4b8e Mon Sep 17 00:00:00 2001 From: Chris Lewis Date: Tue, 26 Jan 2021 09:32:32 -0600 Subject: [PATCH 17/64] clang --- .../src/plane_slicer_raster_generator.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index ad1dff2a..d832cb34 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -69,12 +69,12 @@ static RasterConstructData alignRasterCD(const RasterConstructData rcd) if (rcd.raster_segments.size() <= 1) return (rcd); // do nothing if only one segment - if(rcd.raster_segments[0]->GetPoints()->GetNumberOfPoints() <=1) - { - ROS_ERROR("first raster segment has 0 or 1 points, unable to alignRasterCD()"); - return(rcd); - } - + if (rcd.raster_segments[0]->GetPoints()->GetNumberOfPoints() <= 1) + { + ROS_ERROR("first raster segment has 0 or 1 points, unable to alignRasterCD()"); + return (rcd); + } + // determine raster direction from 1st segment, and origin as first point in first raster (raster_start) Eigen::Vector3d raster_start, raster_end, raster_direction; rcd.raster_segments[0]->GetPoint(0, raster_start.data()); From 7b03eaed68f656ae440d1ca9e6b94949055e451d Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Wed, 27 Jan 2021 01:24:33 -0600 Subject: [PATCH 18/64] Update cmake_common_scripts to ros_industrial_cmake_boilerplate --- .github/workflows/bionic_build.yml | 2 +- .github/workflows/focal_build.yml | 2 +- .github/workflows/xenial_build.yml | 2 +- dependencies_ros1.rosinstall | 2 +- vtk_viewer/CMakeLists.txt | 2 +- vtk_viewer/package.xml | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/workflows/bionic_build.yml b/.github/workflows/bionic_build.yml index 928f84e3..98ab22ec 100644 --- a/.github/workflows/bionic_build.yml +++ b/.github/workflows/bionic_build.yml @@ -20,7 +20,7 @@ jobs: ROS_REPO: main NOT_TEST_BUILD: true UPSTREAM_WORKSPACE: 'dependencies_ros1.rosinstall' - ROSDEP_SKIP_KEYS: "iwyu cmake_common_scripts" + ROSDEP_SKIP_KEYS: "iwyu ros_industrial_cmake_boilerplate" CCACHE_DIR: "/home/runner/work/noether/noether/Bionic-Build/.ccache" TARGET_CMAKE_ARGS: "-DNURBS_FOUND=TRUE" DOCKER_IMAGE: "rosindustrial/noether:melodic" diff --git a/.github/workflows/focal_build.yml b/.github/workflows/focal_build.yml index 1c565e0a..f44a8527 100644 --- a/.github/workflows/focal_build.yml +++ b/.github/workflows/focal_build.yml @@ -20,7 +20,7 @@ jobs: ROS_REPO: main NOT_TEST_BUILD: true UPSTREAM_WORKSPACE: 'dependencies_ros1.rosinstall' - ROSDEP_SKIP_KEYS: "iwyu cmake_common_scripts" + ROSDEP_SKIP_KEYS: "iwyu ros_industrial_cmake_boilerplate" CCACHE_DIR: "/home/runner/work/noether/noether/Focal-Build/.ccache" TARGET_CMAKE_ARGS: "-DNURBS_FOUND=FALSE" DOCKER_IMAGE: "rosindustrial/noether:noetic" diff --git a/.github/workflows/xenial_build.yml b/.github/workflows/xenial_build.yml index 3d612031..0c2e8966 100644 --- a/.github/workflows/xenial_build.yml +++ b/.github/workflows/xenial_build.yml @@ -20,7 +20,7 @@ jobs: ROS_REPO: main NOT_TEST_BUILD: true UPSTREAM_WORKSPACE: 'dependencies_ros1.rosinstall' - ROSDEP_SKIP_KEYS: "iwyu cmake_common_scripts" + ROSDEP_SKIP_KEYS: "iwyu ros_industrial_cmake_boilerplate" CCACHE_DIR: "/home/runner/work/noether/noether/Xenial-Build/.ccache" TARGET_CMAKE_ARGS: "-DNURBS_FOUND=TRUE" DOCKER_IMAGE: "rosindustrial/noether:kinetic" diff --git a/dependencies_ros1.rosinstall b/dependencies_ros1.rosinstall index 564833af..1f414c8b 100644 --- a/dependencies_ros1.rosinstall +++ b/dependencies_ros1.rosinstall @@ -1,3 +1,3 @@ # PCL ros tools, this is required because using custom version of PCL and VTK - git: {local-name: perception_pcl, uri: 'https://github.com/ros-perception/perception_pcl.git', version: 1.7.1} -- git: {local-name: cmake_common_scripts, uri: 'https://github.com/ros-industrial/cmake_common_scripts.git', version: master} +- git: {local-name: ros_industrial_cmake_boilerplate, uri: 'https://github.com/ros-industrial/ros_industrial_cmake_boilerplate.git', version: master} diff --git a/vtk_viewer/CMakeLists.txt b/vtk_viewer/CMakeLists.txt index 271415fa..f60b3571 100644 --- a/vtk_viewer/CMakeLists.txt +++ b/vtk_viewer/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.5.0) project(vtk_viewer VERSION 0.1.0) -find_package(cmake_common_scripts REQUIRED) +find_package(ros_industrial_cmake_boilerplate REQUIRED) find_package(VTK REQUIRED NO_MODULE) if(VTK_FOUND AND ("${VTK_VERSION}" VERSION_LESS 7.1)) diff --git a/vtk_viewer/package.xml b/vtk_viewer/package.xml index cd9c2d45..fcc27d2d 100644 --- a/vtk_viewer/package.xml +++ b/vtk_viewer/package.xml @@ -7,7 +7,7 @@ Apache 2.0 catkin - cmake_common_scripts + ros_industrial_cmake_boilerplate libconsole-bridge-dev libpcl-all-dev From c698b1cb2b478ba77416593f932a0e816dd808d2 Mon Sep 17 00:00:00 2001 From: Colin Lewis Date: Wed, 10 Feb 2021 09:05:25 -0600 Subject: [PATCH 19/64] fixed bug in utility to add extra raster --- tool_path_planner/src/utilities.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index a1e6ff7e..7c547199 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -430,7 +430,6 @@ ToolPaths addExtraPaths(const ToolPaths& tool_paths, double offset_distance) } // duplicate last raster with an offset and add as last raster - offset_sign = -offset_sign; ToolPath last_dup_tool_path; // this tool path mimics last tool_path, but offset by +y and in reverse order temp_path = tool_paths[tool_paths.size() - 1]; std::reverse(temp_path.begin(), temp_path.end()); // reverse the segment order first @@ -438,7 +437,7 @@ ToolPaths addExtraPaths(const ToolPaths& tool_paths, double offset_distance) { if (tool_paths.size() > 1) { - ToolPath tp2 = tool_paths[tool_paths.size() - 1]; + ToolPath tp2 = tool_paths[tool_paths.size() - 2]; offset_sign = computeOffsetSign(seg, tp2[0]); } From 4999eae537a4eee5316ff5d2023b4db629cf38ef Mon Sep 17 00:00:00 2001 From: Chris Lewis Date: Fri, 9 Apr 2021 12:41:00 -0500 Subject: [PATCH 20/64] first compile no testing yet --- .../include/tool_path_planner/utilities.h | 3 + .../src/plane_slicer_raster_generator.cpp | 3 +- tool_path_planner/src/utilities.cpp | 133 ++++++++++++++++++ 3 files changed, 138 insertions(+), 1 deletion(-) diff --git a/tool_path_planner/include/tool_path_planner/utilities.h b/tool_path_planner/include/tool_path_planner/utilities.h index 755215b7..342639f7 100644 --- a/tool_path_planner/include/tool_path_planner/utilities.h +++ b/tool_path_planner/include/tool_path_planner/utilities.h @@ -123,6 +123,9 @@ ToolPaths splitSegments(const 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(const ToolPaths& tool_paths, double offset_distance); +// fills in waypoints to ensure double coverage when adjacent rasters are different lengths + ToolPaths addExtraWaypoints(const ToolPaths& tool_paths, double raster_spacing, double point_spacing); + // reverse direction of every other raster, flip orientation or not depending on raster style ToolPaths reverseOddRasters(const ToolPaths& tool_paths, RasterStyle raster_style); diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index 54190f77..a5334a01 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -785,7 +785,8 @@ boost::optional PlaneSlicerRasterGenerator::generate() if (config_.generate_extra_rasters) { - rasters = addExtraPaths(rasters, config_.raster_spacing); + // rasters = addExtraPaths(rasters, config_.raster_spacing); + rasters = addExtraWaypoints(rasters, config_.raster_spacing, config_.point_spacing); } // switch directions of every other raster diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index 7c547199..78b3baf1 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -393,6 +393,31 @@ double computeOffsetSign(const ToolPathSegment& adjusted_segment, const ToolPath return (offset_sign); } +Eigen::Vector3d computePathDirection(const ToolPath& path) +{ + // TODO check for zero waypoints in either 1st or last segment and do something reasonable + Eigen::Vector3d v; + ToolPathSegment seg = path[0]; + Eigen::Isometry3d waypoint1 = seg[0]; // first waypoint in path + seg = path[path.size()-1]; + Eigen::Isometry3d waypoint2 = seg[seg.size()-1]; // last waypoint in path + v = waypoint2.translation() - waypoint1.translation(); + v.normalize(); + return(v); +} + +double computePathDistance(const ToolPath& path, const Eigen::Isometry3d waypoint) +{ + double dist; + + return(dist); +} + +bool compare_pair(std::pair first, std::pair second) +{ + return(first.first < second.first); +} + ToolPaths addExtraPaths(const ToolPaths& tool_paths, double offset_distance) { ToolPaths new_tool_paths; @@ -458,4 +483,112 @@ ToolPaths addExtraPaths(const ToolPaths& tool_paths, double offset_distance) return new_tool_paths; } +ToolPaths addExtraWaypoints(const ToolPaths& tool_paths, double raster_spacing, double point_spacing) +{ + ToolPaths new_tool_paths; + double offset_sign=1; + + if(tool_paths.size()>1) + { + ToolPath tool_path1 = tool_paths[0]; + ToolPath tool_path2 = tool_paths[1]; + offset_sign = computeOffsetSign(tool_path1[0], tool_path2[0]); + } + + for(size_t i=0; i > waypoint_list; // once sorted this list will be the order of the segments + Eigen::Vector3d path_dir = computePathDirection(tool_path); + ToolPathSegment sseg = tool_path[0]; + Eigen::Vector3d path_start = sseg[0].translation(); + + for( ToolPathSegment seg: tool_path) // add tool_path's waypoints to the list + { + for( Eigen::Isometry3d waypoint: seg) + { + Eigen::Vector3d v = waypoint.translation() - path_start; + std::pair p(path_dir.dot(v), waypoint); + waypoint_list.push_back(p); + }// end for every waypoint in segment + }// end for every segment in path + if(i>0) + { + ToolPath prev_tool_path = tool_paths[i-1]; + for( ToolPathSegment seg: prev_tool_path)// add previous tool_path's waypoints to the list + { + for( Eigen::Isometry3d waypoint: seg) + { + Eigen::Matrix4d H = waypoint.matrix(); + waypoint.translation().x() += offset_sign * raster_spacing * H(0, 1); + waypoint.translation().y() += offset_sign * raster_spacing * H(1, 1); + waypoint.translation().z() += offset_sign * raster_spacing * H(2, 1); + Eigen::Vector3d v = waypoint.translation() - path_start; + std::pair p(path_dir.dot(v), waypoint); + waypoint_list.push_back(p); + }// end for every waypoint in segment + }// end for every segment in path + } + if(i p(path_dir.dot(v), waypoint); + waypoint_list.push_back(p); + }// end for every waypoint in segment + }// end for every segment in path + } + waypoint_list.sort(compare_pair); + // we now have a sorted list including all prev-offset, this & next-offset tool_path waypoints points + // now we just need to prune it to create new paths + // note add each point only if it is close to the point_spacing from previoius point + // create a new segment whenever the distance between successive waypoints is large relative to point_spaceing + ToolPath new_tool_path; + ToolPathSegment seg; + std::pair wp_pair = waypoint_list.front(); + Eigen::Isometry3d last_wp = wp_pair.second; + seg.push_back(last_wp); + for(std::pair waypoint_pair : waypoint_list) + { + Eigen::Isometry3d waypoint = waypoint_pair.second; + Eigen::Vector3d v = waypoint.translation() - last_wp.translation(); + double dist = sqrt(v.x()*v.x() + v.y()*v.y() + v.z()*v.z()); + if(dist> .8*point_spacing && dist<= 1.5*point_spacing) + { + last_wp = waypoint; + seg.push_back(last_wp); + } + else if(dist> 1.5*point_spacing) + { + new_tool_path.push_back(seg); + seg.clear(); + last_wp = waypoint; + seg.push_back(last_wp); + }// start a new segment + else + { + // skip this waypoint + } + } // end for every waypoint in waypoint_list + new_tool_paths.push_back(new_tool_path); + new_tool_path.clear(); + }// end for every path in tool_paths + + return new_tool_paths; +} + } // namespace tool_path_planner From d76613ed6452f020082ce4a5d97e9a93a5b4ef74 Mon Sep 17 00:00:00 2001 From: Stevie Dale Date: Fri, 16 Apr 2021 15:17:34 -0500 Subject: [PATCH 21/64] implemented segmentByAxes --- .../include/tool_path_planner/utilities.h | 10 ++ tool_path_planner/src/utilities.cpp | 168 ++++++++++++++++++ 2 files changed, 178 insertions(+) diff --git a/tool_path_planner/include/tool_path_planner/utilities.h b/tool_path_planner/include/tool_path_planner/utilities.h index 755215b7..8cfe55f3 100644 --- a/tool_path_planner/include/tool_path_planner/utilities.h +++ b/tool_path_planner/include/tool_path_planner/utilities.h @@ -127,6 +127,16 @@ ToolPaths addExtraPaths(const ToolPaths& tool_paths, double offset_distance); ToolPaths reverseOddRasters(const ToolPaths& tool_paths, RasterStyle raster_style); double computeOffsetSign(const ToolPathSegment& adjusted_segment, const ToolPathSegment& away_from_segment); + +namespace SegmentAxes { + enum SegmentAxes { + XY=0, + XZ, + YZ + }; +} +ToolPaths segmentByAxes(const ToolPaths& tool_paths, SegmentAxes::SegmentAxes segment_axes=SegmentAxes::XY); + } // namespace tool_path_planner #endif /* INCLUDE_TOOL_PATH_PLANNER_UTILITIES_H_ */ diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index 7c547199..d484759f 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -10,6 +10,7 @@ #include #include #include +#include // std::sort #include #include @@ -458,4 +459,171 @@ ToolPaths addExtraPaths(const ToolPaths& tool_paths, double offset_distance) return new_tool_paths; } +ToolPaths segmentByAxes(const ToolPaths& tool_paths, SegmentAxes::SegmentAxes segment_axes) +{ + if (tool_paths.size() == 0) + { + ROS_ERROR_STREAM("Tool paths is empty."); + // TODO: throw an exception + } + + ToolPaths new_tool_paths; + + double axes_distance = 1000.0; + std::vector corner_points; + + Eigen::Vector3d p1, p2, p3, p4; + if (segment_axes == SegmentAxes::XY) + { + p1.x() = -axes_distance; + p1.y() = -axes_distance; + p1.z() = 0; + + p2.x() = -axes_distance; + p2.y() = axes_distance; + p2.z() = 0; + + p3.x() = axes_distance; + p3.y() = -axes_distance; + p3.z() = 0; + + p4.x() = axes_distance; + p4.y() = axes_distance; + p4.z() = 0; + } + else if (segment_axes == SegmentAxes::XZ) + { + p1.x() = -axes_distance; + p1.y() = 0; + p1.z() = -axes_distance; + + p2.x() = -axes_distance; + p2.y() = 0; + p2.z() = axes_distance; + + p3.x() = axes_distance; + p3.y() = 0; + p3.z() = -axes_distance; + + p4.x() = axes_distance; + p4.y() = 0; + p4.z() = axes_distance; + } + else // if (segment_axes == SegmentAxes::YZ) + { + p1.x() = 0; + p1.y() = -axes_distance; + p1.z() = -axes_distance; + + p2.x() = 0; + p2.y() = -axes_distance; + p2.z() = axes_distance; + + p3.x() = 0; + p3.y() = axes_distance; + p3.z() = -axes_distance; + + p4.x() = 0; + p4.y() = axes_distance; + p4.z() = axes_distance; + } + + corner_points.push_back(p1); + corner_points.push_back(p2); + corner_points.push_back(p3); + corner_points.push_back(p4); + + for (std::size_t path_index = 0; path_index < tool_paths.size(); ++path_index) + { + ToolPath tool_path = tool_paths[path_index]; + if (tool_path.size() != 1) + { + ROS_ERROR_STREAM("Tool path " << path_index << " contains " << tool_path.size() << + " segments. Expected exactly 1."); + // TODO: throw an exception + } + ToolPath new_tool_path; + ToolPathSegment tool_path_segment = tool_path[0]; + if (tool_path_segment.size() == 0) + { + ROS_ERROR_STREAM("Tool path " << path_index << " segment 0 is empty."); + // TODO: throw an exception + } + + // get cut indices + std::vector cut_indices; + for (Eigen::Vector3d corner_point : corner_points) + { + double min_distance = axes_distance * 10; + int min_index = -1; + for (std::size_t point_index = 0; point_index < tool_path_segment.size(); ++ point_index) + { + Eigen::Vector3d projected_point = tool_path_segment[point_index].translation(); + if (segment_axes == SegmentAxes::XY) + { + projected_point.z() = 0; + } + else if (segment_axes == SegmentAxes::XZ) + { + projected_point.y() = 0; + } + else // if (segment_axes == SegmentAxes::YZ) + { + projected_point.x() = 0; + } + double distance = (projected_point - corner_point).norm(); + if (distance < min_distance) + { + min_distance = distance; + min_index = int(point_index); + } + } + // check if we found a closest point + if (min_index == -1) + { + // if this happens, that means that all tool path points were more than (10 * axes_distance) meters from + // the origin -- this violates the assumption that tool path points are REASONABLY close to origin + ROS_ERROR_STREAM("Could not find closest point to corner point for path " << path_index << " segment 0 " + "for corner point at x:" << corner_point.x() << " y:" << corner_point.y() << " z:" << + corner_point.z()); + // TODO: throw a custom exception + } + // check if the point was also closest to a previous corner (i.e. already in list) + bool already_exists = false; + for (std::size_t cut_index : cut_indices) + { + if (cut_index == std::size_t(min_index)) + { + already_exists = true; + } + } + // if this point isn't already in the list, add to cut indices + if (!already_exists) + { + cut_indices.push_back(std::size_t(min_index)); + } + } + + // cut tool path + // sort cut indices + std::sort(cut_indices.begin(), cut_indices.end()); + // iterate each cut index + for (std::size_t i = 0; i < cut_indices.size(); ++i) + { + ToolPathSegment new_tool_path_segment; + std::size_t index = cut_indices[i]; + // while index does not equal the next cut index (including going from last index to first index) + // add each index after the current cut index, up until the next cut index + while (index != cut_indices[(i+1)%cut_indices.size()]) + { + new_tool_path_segment.push_back(tool_path_segment[index]); + index = (index + 1) % tool_path_segment.size(); + } + new_tool_path.push_back(new_tool_path_segment); + } + new_tool_paths.push_back(new_tool_path); + } + return new_tool_paths; +} + } // namespace tool_path_planner From 9f8c123e016edaf832c6f6014401b218d49c2d0c Mon Sep 17 00:00:00 2001 From: Stevie Dale Date: Fri, 16 Apr 2021 15:33:45 -0500 Subject: [PATCH 22/64] added testing script (should probably delete this before merging) --- tool_path_planner/CMakeLists.txt | 11 ++ tool_path_planner/package.xml | 1 + tool_path_planner/src/test_segment_axes.cpp | 131 ++++++++++++++++++++ 3 files changed, 143 insertions(+) create mode 100644 tool_path_planner/src/test_segment_axes.cpp diff --git a/tool_path_planner/CMakeLists.txt b/tool_path_planner/CMakeLists.txt index ca38f2c5..7d153cfc 100644 --- a/tool_path_planner/CMakeLists.txt +++ b/tool_path_planner/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS geometry_msgs noether_msgs rosconsole + roscpp roslib shape_msgs ) @@ -48,6 +49,7 @@ catkin_package( geometry_msgs noether_msgs rosconsole + roscpp shape_msgs DEPENDS EIGEN3 @@ -81,10 +83,19 @@ add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ) +add_executable(test_segment_axes + src/test_segment_axes.cpp +) +target_link_libraries(test_segment_axes + ${catkin_LIBRARIES} + ${PROJECT_NAME} +) + ############# ## Install ## ############# install(TARGETS ${PROJECT_NAME} + test_segment_axes ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/tool_path_planner/package.xml b/tool_path_planner/package.xml index 310f39ef..2816c231 100644 --- a/tool_path_planner/package.xml +++ b/tool_path_planner/package.xml @@ -10,6 +10,7 @@ cmake_modules roslib + roscpp eigen_conversions eigen_stl_containers noether_conversions diff --git a/tool_path_planner/src/test_segment_axes.cpp b/tool_path_planner/src/test_segment_axes.cpp new file mode 100644 index 00000000..067ec555 --- /dev/null +++ b/tool_path_planner/src/test_segment_axes.cpp @@ -0,0 +1,131 @@ +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include + +#include + + +int main(int argc, char** argv) +{ + + ros::init(argc, argv, "test_segment_axes"); + ros::NodeHandle nh; + ros::Publisher pub = nh.advertise("segments", 10); + std::this_thread::sleep_for(std::chrono::seconds(10)); + + // create tool path + tool_path_planner::ToolPaths tool_paths; + tool_path_planner::ToolPath tool_path; + tool_path_planner::ToolPathSegment tool_path_segment; + + double radius = 1.0; + double angle_inc = DEG2RAD(15); + + for (int i = 0; i < int(std::round(2 * M_PI / angle_inc)); ++i) + { + Eigen::Isometry3d p = Eigen::Isometry3d::Identity(); + double angle = i * angle_inc; + if (abs(angle - DEG2RAD(0.0)) < 0.001) + { + p.translation().x() = radius; + p.translation().y() = 0; + } + else if (abs(angle - DEG2RAD(90.0)) < 0.001) + { + p.translation().x() = 0; + p.translation().y() = radius; + } + else if (abs(angle - DEG2RAD(180.0)) < 0.001) + { + p.translation().x() = -radius; + p.translation().y() = 0; + } + else if (abs(angle - DEG2RAD(270.0)) < 0.001) + { + p.translation().x() = 0; + p.translation().y() = -radius; + } + else + { + double theta = angle; + int y_sign = 1; + int x_sign = 1; + if (DEG2RAD(90.0) < angle < DEG2RAD(180.0)) + { + theta = DEG2RAD(180.0) - angle; + x_sign = -1; + } + else if (DEG2RAD(180.0) < angle < DEG2RAD(270.0)) + { + theta = angle - DEG2RAD(180.0); + y_sign = -1; + x_sign = -1; + } + else if (DEG2RAD(270.0) < angle < DEG2RAD(360.0)) + { + theta = DEG2RAD(360.0) - angle; + y_sign = -1; + } + p.translation().x() = x_sign * radius * cos(theta); + p.translation().y() = y_sign * radius * sin(theta); + } + tool_path_segment.push_back(p); + } + + tool_path.push_back(tool_path_segment); + tool_paths.push_back(tool_path); + + tool_paths = tool_path_planner::segmentByAxes(tool_paths); + + std::size_t n = tool_paths[0].size(); + std::vector> colors = { + {1.0, 0.0, 0.0}, + {0.0, 1.0, 0.0}, + {0.0, 0.0, 1.0}, + {1.0, 1.0, 0.0} + }; + + int marker_id = 0; + visualization_msgs::MarkerArray marker_array; + for (std::size_t i = 0; i < n; ++i) + { + for (Eigen::Isometry3d pose : tool_paths[0][i]) + { + visualization_msgs::Marker marker; + marker.header.frame_id = "world"; + marker.header.stamp = ros::Time::now(); + marker.ns = ""; + marker.id = marker_id; + marker.color.a = 1.0; + marker.color.r = colors[i][0]; + marker.color.g = colors[i][1]; + marker.color.b = colors[i][2]; + marker.pose = tf2::toMsg(pose); + marker.type = visualization_msgs::Marker::SPHERE; + marker.scale.x = 0.02; + marker.scale.y = 0.02; + marker.scale.z = 0.02; + marker_array.markers.push_back(marker); + pub.publish(marker_array); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + ++marker_id; + } + } + +// pub.publish(marker_array); + + ROS_ERROR_STREAM("DONE."); + + ros::spin(); + + return 0; +} From 327acf966c1a17c1f5cd3a13e01a2d4fb72b22c5 Mon Sep 17 00:00:00 2001 From: drchrislewis Date: Tue, 20 Apr 2021 10:19:25 -0500 Subject: [PATCH 23/64] addingExtraWaypoints is now working under gtest examples --- .../include/tool_path_planner/utilities.h | 2 +- .../src/plane_slicer_raster_generator.cpp | 64 ++--- tool_path_planner/src/utilities.cpp | 263 +++++++++++------- tool_path_planner/test/utest.cpp | 107 ++++++- 4 files changed, 297 insertions(+), 139 deletions(-) diff --git a/tool_path_planner/include/tool_path_planner/utilities.h b/tool_path_planner/include/tool_path_planner/utilities.h index 342639f7..3d255fef 100644 --- a/tool_path_planner/include/tool_path_planner/utilities.h +++ b/tool_path_planner/include/tool_path_planner/utilities.h @@ -124,7 +124,7 @@ ToolPaths splitSegments(const ToolPaths& tool_paths, double max_segment_length); ToolPaths addExtraPaths(const ToolPaths& tool_paths, double offset_distance); // fills in waypoints to ensure double coverage when adjacent rasters are different lengths - ToolPaths addExtraWaypoints(const ToolPaths& tool_paths, double raster_spacing, double point_spacing); +ToolPaths addExtraWaypoints(const ToolPaths& tool_paths, double raster_spacing, double point_spacing); // reverse direction of every other raster, flip orientation or not depending on raster style ToolPaths reverseOddRasters(const ToolPaths& tool_paths, RasterStyle raster_style); diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index a5334a01..a4117ad0 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -62,64 +62,67 @@ struct RasterConstructData std::vector segment_lengths; }; +static Eigen::Vector3d getSegDir(PolyDataPtr seg) +{ + if (seg->GetPoints()->GetNumberOfPoints() < 1) + { + ROS_ERROR("can't get direction from a segment with fewer than 2 points"); + } + Eigen::Vector3d seg_start, seg_end; + seg->GetPoint(0, seg_start.data()); + seg->GetPoint(seg->GetPoints()->GetNumberOfPoints() - 1, seg_end.data()); + return (seg_end - seg_start); +} + +static bool compare_ds_pair(std::pair& first, std::pair& second) +{ + return (first.first < second.first); +} + // @brief this function accepts and returns a rasterConstruct where every segment progresses in the same direction and // in the right order -static RasterConstructData alignRasterCD(const RasterConstructData rcd) +static RasterConstructData alignRasterCD(RasterConstructData& rcd, Eigen::Vector3d& raster_direction) { - if (rcd.raster_segments.size() <= 1) - return (rcd); // do nothing if only one segment - if (rcd.raster_segments[0]->GetPoints()->GetNumberOfPoints() <= 1) { ROS_ERROR("first raster segment has 0 or 1 points, unable to alignRasterCD()"); return (rcd); } - // determine raster direction from 1st segment, and origin as first point in first raster (raster_start) - Eigen::Vector3d raster_start, raster_end, raster_direction; + Eigen::Vector3d raster_start; rcd.raster_segments[0]->GetPoint(0, raster_start.data()); - rcd.raster_segments[0]->GetPoint(rcd.raster_segments[0]->GetPoints()->GetNumberOfPoints() - 1, raster_end.data()); - raster_direction = raster_end - raster_start; - raster_direction.normalize(); // determine location and direction of each successive segement, reverse any mis-directed segments RasterConstructData temp_rcd; - std::list > seg_order; // once sorted this list will be the order of the segments - std::tuple p(0.0, 0); - seg_order.push_back(p); - for (size_t i = 1; i < rcd.raster_segments.size(); i++) + std::list > seg_order; // once sorted this list will be the order of the segments + for (size_t i = 0; i < rcd.raster_segments.size(); i++) { // determine i'th segments direction - Eigen::Vector3d seg_start, seg_end, seg_dir; + Eigen::Vector3d seg_dir = getSegDir(rcd.raster_segments[i]); + Eigen::Vector3d seg_start; rcd.raster_segments[i]->GetPoint(0, seg_start.data()); - rcd.raster_segments[i]->GetPoint(rcd.raster_segments[i]->GetPoints()->GetNumberOfPoints() - 1, seg_end.data()); - seg_dir = seg_end - seg_start; - // if segment direction is opposite raster direction, reverse the segment if (seg_dir.dot(raster_direction) < 0.0) { vtkSmartPointer old_points = rcd.raster_segments[i]->GetPoints(); vtkSmartPointer new_points = vtkSmartPointer::New(); - for (long long int j = old_points->GetNumberOfPoints() - 1; j >= 0; j--) + for (long int j = static_cast(old_points->GetNumberOfPoints() - 1); j >= 0; j--) { new_points->InsertNextPoint(old_points->GetPoint(j)); } rcd.raster_segments[i]->SetPoints(new_points); - Eigen::Vector3d temp = seg_start; - seg_start = seg_end; - seg_end = temp; + rcd.raster_segments[i]->GetPoint(0, seg_start.data()); } // determine location of this segment in raster double seg_loc = (seg_start - raster_start).dot(raster_direction); - std::tuple p(seg_loc, i); + std::pair p(seg_loc, i); seg_order.push_back(p); } - // sort the segments by location - seg_order.sort(); + seg_order.sort(compare_ds_pair); RasterConstructData new_rcd; - for (std::tuple p : seg_order) + for (std::pair p : seg_order) { size_t seg_index = std::get<1>(p); new_rcd.raster_segments.push_back(rcd.raster_segments[seg_index]); @@ -207,7 +210,9 @@ static vtkSmartPointer applyParametricSpline(const vtkSmartPointerEvaluate(u.data(), pt.data(), du); // check distance - if ((pt - pt_prev).norm() >= point_spacing) + if (1) + // TODO Figure out why this reduces the number of points by half and makes them twice the point spacing + // if ((pt - pt_prev).norm() >= point_spacing) { new_points->InsertNextPoint(pt.data()); pt_prev = pt; @@ -610,7 +615,6 @@ boost::optional PlaneSlicerRasterGenerator::generate() // Calculate the start location Vector3d start_loc = center + (min_coeff * raster_dir); - vtkSmartPointer raster_data = vtkSmartPointer::New(); for (std::size_t i = 0; i < num_planes + 1; i++) { @@ -643,7 +647,6 @@ boost::optional PlaneSlicerRasterGenerator::generate() raster_data->AddInputData(stripper->GetOutput(r)); } } - // build cell locator and kd_tree to recover normals later on kd_tree_ = vtkSmartPointer::New(); kd_tree_->SetDataSet(mesh_data_); @@ -718,7 +721,6 @@ boost::optional PlaneSlicerRasterGenerator::generate() // merging segments mergeRasterSegments(raster_lines->GetPoints(), config_.min_hole_size, raster_ids); - // rectifying if (!rasters_data_vec.empty()) { @@ -775,9 +777,10 @@ boost::optional PlaneSlicerRasterGenerator::generate() } // make sure every raster has its segments ordered and aligned correctly + Eigen::Vector3d raster_direction = getSegDir(rasters_data_vec[0].raster_segments[0]); for (RasterConstructData rcd : rasters_data_vec) { - rcd = alignRasterCD(rcd); + rcd = alignRasterCD(rcd, raster_direction); } // converting to poses msg @@ -785,7 +788,6 @@ boost::optional PlaneSlicerRasterGenerator::generate() if (config_.generate_extra_rasters) { - // rasters = addExtraPaths(rasters, config_.raster_spacing); rasters = addExtraWaypoints(rasters, config_.raster_spacing, config_.point_spacing); } diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index 78b3baf1..8bb564b9 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -398,24 +398,24 @@ Eigen::Vector3d computePathDirection(const ToolPath& path) // TODO check for zero waypoints in either 1st or last segment and do something reasonable Eigen::Vector3d v; ToolPathSegment seg = path[0]; - Eigen::Isometry3d waypoint1 = seg[0]; // first waypoint in path - seg = path[path.size()-1]; - Eigen::Isometry3d waypoint2 = seg[seg.size()-1]; // last waypoint in path + Eigen::Isometry3d waypoint1 = seg[0]; // first waypoint in path + seg = path[path.size() - 1]; + Eigen::Isometry3d waypoint2 = seg[seg.size() - 1]; // last waypoint in path v = waypoint2.translation() - waypoint1.translation(); v.normalize(); - return(v); + return (v); } double computePathDistance(const ToolPath& path, const Eigen::Isometry3d waypoint) { double dist; - return(dist); + return (dist); } bool compare_pair(std::pair first, std::pair second) { - return(first.first < second.first); + return (first.first < second.first); } ToolPaths addExtraPaths(const ToolPaths& tool_paths, double offset_distance) @@ -483,110 +483,167 @@ ToolPaths addExtraPaths(const ToolPaths& tool_paths, double offset_distance) return new_tool_paths; } +ToolPath sortAndSegment(std::list >& waypoint_list, double point_spacing) +{ + waypoint_list.sort(compare_pair); + // we now have a sorted list including all prev-offset, this & next-offset tool_path waypoints points + // now we just need to prune it to create new paths + // note add each point only if it is close to the point_spacing from previoius point + // create a new segment whenever the distance between successive waypoints is large relative to point_spaceing + ToolPath new_tool_path; + ToolPathSegment seg; + std::pair wp_pair = waypoint_list.front(); + Eigen::Isometry3d last_wp = wp_pair.second; + double last_dot = wp_pair.first; + seg.push_back(last_wp); + int q = 0; + for (std::pair waypoint_pair : waypoint_list) + { + Eigen::Isometry3d waypoint = waypoint_pair.second; + Eigen::Vector3d v = waypoint.translation() - last_wp.translation(); + double dist2 = sqrt(v.x() * v.x() + v.y() * v.y() + v.z() * v.z()); + double dist = waypoint_pair.first - last_dot; + if (dist > .75 * point_spacing && dist <= 1.25 * point_spacing) + { + seg.push_back(waypoint); + last_wp = waypoint; + last_dot = waypoint_pair.first; + } + else if (dist > 1.25 * point_spacing) + { // start a new segment + new_tool_path.push_back(seg); + seg.clear(); + seg.push_back(last_wp); + last_wp = waypoint; + last_dot = waypoint_pair.first; + } + else + { // skip unless last in list + if (q == waypoint_list.size() - 1 && dist > .25 * point_spacing) + { + seg.push_back(waypoint); // keep the last on regardless of distance to proces up to the defined edges + last_dot = waypoint_pair.first; + } + } + q++; + } // end for every waypoint in waypoint_list + new_tool_path.push_back(seg); + + return (new_tool_path); +} + ToolPaths addExtraWaypoints(const ToolPaths& tool_paths, double raster_spacing, double point_spacing) { ToolPaths new_tool_paths; - double offset_sign=1; + double offset_sign = 1; + + if (tool_paths.size() > 1) + { + ToolPath tool_path1 = tool_paths[0]; + ToolPath tool_path2 = tool_paths[1]; + offset_sign = computeOffsetSign(tool_path1[0], tool_path2[0]); + } - if(tool_paths.size()>1) + for (size_t i = 0; i < tool_paths.size(); i++) + { + // create a list of tuple containing the waypoints in this tool_path and its distance along the path + // add to this list the waypoints and distances of the previous and next path offset into this path + // sort the list + // using sorted list, create new_tool_path by: + // add a new point if it is approximately point_spacing from the previous point + // end the old and start a new segment when the new point is significantly more than the point_spacing from previous + // waypoint + ToolPath tool_path = tool_paths[i]; + std::list > waypoint_list; // once sorted this list will be the order of the + // segments + Eigen::Vector3d path_dir = computePathDirection(tool_path); + ToolPathSegment sseg = tool_path[0]; + Eigen::Vector3d path_start = sseg[0].translation(); + + if (i == 0) // duplicate first tool_path but offset + { + for (ToolPathSegment seg : tool_path) // add tool_path's waypoints to the list + { + for (Eigen::Isometry3d waypoint : seg) + { + Eigen::Matrix4d H = waypoint.matrix(); + waypoint.translation().x() += offset_sign * raster_spacing * H(0, 1); + waypoint.translation().y() += offset_sign * raster_spacing * H(1, 1); + waypoint.translation().z() += offset_sign * raster_spacing * H(2, 1); + Eigen::Vector3d v = waypoint.translation() - path_start; + std::pair p(path_dir.dot(v), waypoint); + waypoint_list.push_back(p); + } // end for every waypoint in segment + } // end for every segment in path + new_tool_paths.push_back(sortAndSegment(waypoint_list, point_spacing)); + waypoint_list.clear(); + } // end duplication of fist tool_path but offset + + for (ToolPathSegment seg : tool_path) // add tool_path's waypoints to the list { - ToolPath tool_path1 = tool_paths[0]; - ToolPath tool_path2 = tool_paths[1]; - offset_sign = computeOffsetSign(tool_path1[0], tool_path2[0]); + for (Eigen::Isometry3d waypoint : seg) + { + Eigen::Vector3d v = waypoint.translation() - path_start; + std::pair p(path_dir.dot(v), waypoint); + waypoint_list.push_back(p); + } // end for every waypoint in segment + } // end for every segment in path + if (i > 0) + { + ToolPath prev_tool_path = tool_paths[i - 1]; + for (ToolPathSegment seg : prev_tool_path) // add previous tool_path's waypoints to the list + { + for (Eigen::Isometry3d waypoint : seg) + { + Eigen::Matrix4d H = waypoint.matrix(); + waypoint.translation().x() -= offset_sign * raster_spacing * H(0, 1); + waypoint.translation().y() -= offset_sign * raster_spacing * H(1, 1); + waypoint.translation().z() -= offset_sign * raster_spacing * H(2, 1); + Eigen::Vector3d v = waypoint.translation() - path_start; + std::pair p(path_dir.dot(v), waypoint); + waypoint_list.push_back(p); + } // end for every waypoint in segment + } // end for every segment in path } - - for(size_t i=0; i > waypoint_list; // once sorted this list will be the order of the segments - Eigen::Vector3d path_dir = computePathDirection(tool_path); - ToolPathSegment sseg = tool_path[0]; - Eigen::Vector3d path_start = sseg[0].translation(); - - for( ToolPathSegment seg: tool_path) // add tool_path's waypoints to the list - { - for( Eigen::Isometry3d waypoint: seg) - { - Eigen::Vector3d v = waypoint.translation() - path_start; - std::pair p(path_dir.dot(v), waypoint); - waypoint_list.push_back(p); - }// end for every waypoint in segment - }// end for every segment in path - if(i>0) - { - ToolPath prev_tool_path = tool_paths[i-1]; - for( ToolPathSegment seg: prev_tool_path)// add previous tool_path's waypoints to the list - { - for( Eigen::Isometry3d waypoint: seg) - { - Eigen::Matrix4d H = waypoint.matrix(); - waypoint.translation().x() += offset_sign * raster_spacing * H(0, 1); - waypoint.translation().y() += offset_sign * raster_spacing * H(1, 1); - waypoint.translation().z() += offset_sign * raster_spacing * H(2, 1); - Eigen::Vector3d v = waypoint.translation() - path_start; - std::pair p(path_dir.dot(v), waypoint); - waypoint_list.push_back(p); - }// end for every waypoint in segment - }// end for every segment in path - } - if(i p(path_dir.dot(v), waypoint); - waypoint_list.push_back(p); - }// end for every waypoint in segment - }// end for every segment in path - } - waypoint_list.sort(compare_pair); - // we now have a sorted list including all prev-offset, this & next-offset tool_path waypoints points - // now we just need to prune it to create new paths - // note add each point only if it is close to the point_spacing from previoius point - // create a new segment whenever the distance between successive waypoints is large relative to point_spaceing - ToolPath new_tool_path; - ToolPathSegment seg; - std::pair wp_pair = waypoint_list.front(); - Eigen::Isometry3d last_wp = wp_pair.second; - seg.push_back(last_wp); - for(std::pair waypoint_pair : waypoint_list) - { - Eigen::Isometry3d waypoint = waypoint_pair.second; - Eigen::Vector3d v = waypoint.translation() - last_wp.translation(); - double dist = sqrt(v.x()*v.x() + v.y()*v.y() + v.z()*v.z()); - if(dist> .8*point_spacing && dist<= 1.5*point_spacing) - { - last_wp = waypoint; - seg.push_back(last_wp); - } - else if(dist> 1.5*point_spacing) - { - new_tool_path.push_back(seg); - seg.clear(); - last_wp = waypoint; - seg.push_back(last_wp); - }// start a new segment - else - { - // skip this waypoint - } - } // end for every waypoint in waypoint_list - new_tool_paths.push_back(new_tool_path); - new_tool_path.clear(); - }// end for every path in tool_paths + ToolPath next_tool_path = tool_paths[i + 1]; + for (ToolPathSegment seg : next_tool_path) // add next tool_path's waypoints to the list + { + for (Eigen::Isometry3d waypoint : seg) + { + Eigen::Matrix4d H = waypoint.matrix(); + waypoint.translation().x() += offset_sign * raster_spacing * H(0, 1); + waypoint.translation().y() += offset_sign * raster_spacing * H(1, 1); + waypoint.translation().z() += offset_sign * raster_spacing * H(2, 1); + Eigen::Vector3d v = waypoint.translation() - path_start; + std::pair p(path_dir.dot(v), waypoint); + waypoint_list.push_back(p); + } // end for every waypoint in segment + } // end for every segment in path + } + new_tool_paths.push_back(sortAndSegment(waypoint_list, point_spacing)); + waypoint_list.clear(); + // duplicate last tool_path but offset + if (i == tool_paths.size() - 1) + { + for (ToolPathSegment seg : tool_path) // add tool_path's waypoints to the list + { + for (Eigen::Isometry3d waypoint : seg) + { + Eigen::Matrix4d H = waypoint.matrix(); + waypoint.translation().x() -= offset_sign * raster_spacing * H(0, 1); + waypoint.translation().y() -= offset_sign * raster_spacing * H(1, 1); + waypoint.translation().z() -= offset_sign * raster_spacing * H(2, 1); + Eigen::Vector3d v = waypoint.translation() - path_start; + std::pair p(path_dir.dot(v), waypoint); + waypoint_list.push_back(p); + } // end for every waypoint in segment + + } // end for every segment in path + new_tool_paths.push_back(sortAndSegment(waypoint_list, point_spacing)); + } // end duplication of last tool_path but offset + } // end for every path in tool_paths return new_tool_paths; } diff --git a/tool_path_planner/test/utest.cpp b/tool_path_planner/test/utest.cpp index b5255792..28a17191 100644 --- a/tool_path_planner/test/utest.cpp +++ b/tool_path_planner/test/utest.cpp @@ -18,7 +18,7 @@ #define DISPLAY_LINES 1 #define DISPLAY_NORMALS 0 #define DISPLAY_DERIVATIVES 1 -#define POINT_SPACING 0.5 +#define POINT_SPACING 1.0 vtkSmartPointer createTestMesh1(double sample_spacing = 0.5) { @@ -272,6 +272,47 @@ void runExtraRasterTest(tool_path_planner::PathGenerator& planner, // Check that the number of rasters has increased by 2 ASSERT_EQ(paths_no_extras.get().size() + 2, paths_with_extras.get().size()); + for (tool_path_planner::ToolPath path : *paths_no_extras) + { + for (tool_path_planner::ToolPathSegment seg : path) + { + double average_point_spacing = 0.; + int n_pts = 0; + Eigen::Isometry3d prev_waypoint = seg[0]; + for (Eigen::Isometry3d waypoint : seg) + { + Eigen::Vector3d v = waypoint.translation() - prev_waypoint.translation(); + average_point_spacing += sqrt(v.x() * v.x() + v.y() * v.y() + v.z() * v.z()); + prev_waypoint = waypoint; + n_pts++; + } + average_point_spacing = average_point_spacing / (n_pts - 1); + // ASSERT_NEAR(POINT_SPACING, average_point_spacing, POINT_SPACING*.25); + } + } + + for (tool_path_planner::ToolPath path : *paths_with_extras) + { + for (tool_path_planner::ToolPathSegment seg : path) + { + double average_point_spacing = 0.; + int n_pts = 0; + Eigen::Isometry3d prev_waypoint = seg[0]; + for (Eigen::Isometry3d waypoint : seg) + { + Eigen::Vector3d v = waypoint.translation() - prev_waypoint.translation(); + double dist = sqrt(v.x() * v.x() + v.y() * v.y() + v.z() * v.z()); + printf("dist = %lf\n", dist); + average_point_spacing += dist; + prev_waypoint = waypoint; + n_pts++; + } + average_point_spacing = average_point_spacing / (n_pts - 1); + printf("average_point_spacing = %lf\n", average_point_spacing); + // ASSERT_NEAR(POINT_SPACING, average_point_spacing, POINT_SPACING*.25); + } + } + #ifdef NDEBUG // release build stuff goes here CONSOLE_BRIDGE_logInform("noether/tool_path_planner test: visualization is only available in debug mode"); @@ -279,6 +320,7 @@ void runExtraRasterTest(tool_path_planner::PathGenerator& planner, #else // Debug-specific code goes here vtk_viewer::VTKViewer viz; + vtk_viewer::VTKViewer viz2; std::vector color(3); double scale = 1.0; @@ -287,6 +329,7 @@ void runExtraRasterTest(tool_path_planner::PathGenerator& planner, color[1] = 0.9f; color[2] = 0.9f; viz.addPolyDataDisplay(mesh, color); + viz2.addPolyDataDisplay(mesh, color); // Display surface normals if (DISPLAY_NORMALS) @@ -336,7 +379,7 @@ void runExtraRasterTest(tool_path_planner::PathGenerator& planner, color[0] = 0.2f; color[1] = 0.9f; color[2] = 0.2f; - viz.addPolyNormalsDisplay(paths_with_extras_data[i][j].line, color, scale); + viz2.addPolyNormalsDisplay(paths_with_extras_data[i][j].line, color, scale); } if (DISPLAY_DERIVATIVES) // display derivatives @@ -344,12 +387,12 @@ void runExtraRasterTest(tool_path_planner::PathGenerator& planner, color[0] = 0.9f; color[1] = 0.9f; color[2] = 0.2f; - viz.addPolyNormalsDisplay(paths_with_extras_data[i][j].derivatives, color, scale); + viz2.addPolyNormalsDisplay(paths_with_extras_data[i][j].derivatives, color, scale); } } } - viz.renderDisplay(); + viz2.renderDisplay(); #endif } @@ -541,6 +584,62 @@ TEST(IntersectTest, SurfaceWalkExtraRasterTest) runExtraRasterTest(planner, planner_with_extra, mesh); } +TEST(PlaneSlicerTest, PlaneSlicerExtraWaypointTest) +{ + vtkSmartPointer mesh = createTestMesh1(); + + tool_path_planner::PlaneSlicerRasterGenerator planner; + tool_path_planner::PlaneSlicerRasterGenerator planner_with_extra; + + // Set input tool data + tool_path_planner::PlaneSlicerRasterGenerator::Config tool; + tool.raster_spacing = 1.0; + tool.point_spacing = POINT_SPACING; + tool.raster_rot_offset = 0.0; + tool.min_segment_size = 0.05; + tool.search_radius = 0.05; + tool.min_hole_size = 0.8; + // tool.raster_wrt_global_axes = use:: tool_path_planner::PlaneSlicerRasterGenerator::DEFAULT_RASTER_WRT_GLOBAL_AXES; + tool.raster_direction = Eigen::Vector3d::UnitY(); + tool.generate_extra_rasters = false; + // tool.raster_style = use:: tool_path_planner::PlaneSlicerRasterGenerator::KEEP_ORIENTATION_ON_REVERSE_STROKES; + + planner.setConfiguration(tool); + + tool.generate_extra_rasters = true; + planner_with_extra.setConfiguration(tool); + + runExtraRasterTest(planner, planner_with_extra, mesh); +} + +TEST(PlaneSlicerTest, PlaneSlicerExtraWaypointTest2) +{ + vtkSmartPointer mesh = createTestMesh2(); + + tool_path_planner::PlaneSlicerRasterGenerator planner; + tool_path_planner::PlaneSlicerRasterGenerator planner_with_extra; + + // Set input tool data + tool_path_planner::PlaneSlicerRasterGenerator::Config tool; + tool.raster_spacing = 1.0; + tool.point_spacing = POINT_SPACING; + tool.raster_rot_offset = 0.0; + tool.min_segment_size = 0.05; + tool.search_radius = 0.05; + tool.min_hole_size = 0.8; + // tool.raster_wrt_global_axes = use:: tool_path_planner::PlaneSlicerRasterGenerator::DEFAULT_RASTER_WRT_GLOBAL_AXES; + tool.raster_direction = Eigen::Vector3d::UnitY(); + tool.generate_extra_rasters = false; + // tool.raster_style = use:: tool_path_planner::PlaneSlicerRasterGenerator::KEEP_ORIENTATION_ON_REVERSE_STROKES; + + planner.setConfiguration(tool); + + tool.generate_extra_rasters = true; + planner_with_extra.setConfiguration(tool); + + runExtraRasterTest(planner, planner_with_extra, mesh); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); From f58ccca6057bed06d0ca978262500b33cd171664 Mon Sep 17 00:00:00 2001 From: Stevie Dale Date: Thu, 22 Apr 2021 14:03:03 -0500 Subject: [PATCH 24/64] updated segmentByAxes with major axis detection --- .../include/tool_path_planner/utilities.h | 12 +- tool_path_planner/src/test_segment_axes.cpp | 254 +++++++++++++++--- tool_path_planner/src/utilities.cpp | 191 +++++-------- 3 files changed, 290 insertions(+), 167 deletions(-) diff --git a/tool_path_planner/include/tool_path_planner/utilities.h b/tool_path_planner/include/tool_path_planner/utilities.h index 8cfe55f3..96fbceca 100644 --- a/tool_path_planner/include/tool_path_planner/utilities.h +++ b/tool_path_planner/include/tool_path_planner/utilities.h @@ -128,14 +128,10 @@ ToolPaths reverseOddRasters(const ToolPaths& tool_paths, RasterStyle raster_styl double computeOffsetSign(const ToolPathSegment& adjusted_segment, const ToolPathSegment& away_from_segment); -namespace SegmentAxes { - enum SegmentAxes { - XY=0, - XZ, - YZ - }; -} -ToolPaths segmentByAxes(const ToolPaths& tool_paths, SegmentAxes::SegmentAxes segment_axes=SegmentAxes::XY); +//ToolPath segmentByAxes(const ToolPathSegment& tool_path_segment); +//ToolPath segmentByAxes(const ToolPathSegment& tool_path_segment, const Eigen::Vector3f& axis_1, const Eigen::Vector3f& axis_2); + +ToolPaths segmentByAxes(const ToolPaths& tool_paths, Eigen::Vector3f& major, Eigen::Vector3f& perp); } // namespace tool_path_planner diff --git a/tool_path_planner/src/test_segment_axes.cpp b/tool_path_planner/src/test_segment_axes.cpp index 067ec555..d8a6af7c 100644 --- a/tool_path_planner/src/test_segment_axes.cpp +++ b/tool_path_planner/src/test_segment_axes.cpp @@ -8,51 +8,104 @@ #include #include +#include #include #include #include +tool_path_planner::ToolPathSegment toSegment(std::vector points) +{ + tool_path_planner::ToolPathSegment tool_path_segment; + for (auto p : points) + { + Eigen::Isometry3d point; + point.translation().x() = p.x(); + point.translation().y() = p.y(); + point.translation().z() = p.z(); + tool_path_segment.push_back(point); + } + return tool_path_segment; +} -int main(int argc, char** argv) +std::vector applySinWaveToZ( + std::vector points_in, + double wave_length=0.4, + double amplitude=0.1, + Eigen::Vector3d direction = Eigen::Vector3d(1.0, 1.0, 0)) { + std::vector points_out; + for (auto p : points_in) + { + p.z() += amplitude * sin(2*M_PI*p.dot(direction) / wave_length); + points_out.push_back(p); + } + return points_out; +} - ros::init(argc, argv, "test_segment_axes"); - ros::NodeHandle nh; - ros::Publisher pub = nh.advertise("segments", 10); - std::this_thread::sleep_for(std::chrono::seconds(10)); +std::vector applySinWaveByInc( + std::vector points_in, + double inc=M_PI/12, + double phase=0, + Eigen::Vector3d amplitude=Eigen::Vector3d(0.2, 0, 0)) +{ + std::vector points_out; + double t = phase; + for (auto p : points_in) + { + p.x() += amplitude.x() * sin(t); + p.y() += amplitude.y() * sin(t); + p.z() += amplitude.z() * sin(t); + t += inc; + points_out.push_back(p); + } + return points_out; +} - // create tool path - tool_path_planner::ToolPaths tool_paths; - tool_path_planner::ToolPath tool_path; - tool_path_planner::ToolPathSegment tool_path_segment; +std::vector projectZToPlane( + std::vector points_in, + Eigen::Vector3d plane_coeff) +{ + std::vector points_out; + for (auto p : points_in) + { + double x = plane_coeff.x(); + double y = plane_coeff.y(); + p.z() = -1*x*p.x() + -1*y*p.y(); + points_out.push_back(p); + } + return points_out; +} - double radius = 1.0; - double angle_inc = DEG2RAD(15); +std::vector circle( + double radius = 1.0, + double angle_inc = DEG2RAD(5)) +{ + std::vector points; for (int i = 0; i < int(std::round(2 * M_PI / angle_inc)); ++i) { - Eigen::Isometry3d p = Eigen::Isometry3d::Identity(); + Eigen::Vector3d p; double angle = i * angle_inc; if (abs(angle - DEG2RAD(0.0)) < 0.001) { - p.translation().x() = radius; - p.translation().y() = 0; + p.x() = radius; + p.y() = 0; } else if (abs(angle - DEG2RAD(90.0)) < 0.001) { - p.translation().x() = 0; - p.translation().y() = radius; + p.x() = 0; + p.y() = radius; } else if (abs(angle - DEG2RAD(180.0)) < 0.001) { - p.translation().x() = -radius; - p.translation().y() = 0; + p.x() = -radius; + p.y() = 0; } else if (abs(angle - DEG2RAD(270.0)) < 0.001) { - p.translation().x() = 0; - p.translation().y() = -radius; + p.x() = 0; + p.y() = -radius; } else { @@ -75,16 +128,153 @@ int main(int argc, char** argv) theta = DEG2RAD(360.0) - angle; y_sign = -1; } - p.translation().x() = x_sign * radius * cos(theta); - p.translation().y() = y_sign * radius * sin(theta); + p.x() = x_sign * radius * cos(theta); + p.y() = y_sign * radius * sin(theta); } - tool_path_segment.push_back(p); + points.push_back(p); } + return points; +} + +std::vector polygon( + std::vector points_in, + double inc = 0.05) +{ + std::vector points_out; + for (std::size_t point_index = 0; point_index < points_in.size(); ++point_index) + { + double x1 = points_in[point_index].x(); + double y1 = points_in[point_index].y(); + double z1 = points_in[point_index].z(); + double x2 = points_in[(point_index+1)%points_in.size()].x(); + double y2 = points_in[(point_index+1)%points_in.size()].y(); + double z2 = points_in[(point_index+1)%points_in.size()].z(); + double dist = std::sqrt(std::pow(x1 - x2, 2) + std::pow(y1 - y2, 2) + std::pow(z1 - z2, 2)); + int n = int(std::round(dist / inc)); + double x_inc = (x2 - x1)/n; + double y_inc = (y2 - y1)/n; + double z_inc = (z2 - z1)/n; + for (int i = 0; i < n; ++i) + { + Eigen::Vector3d point( + x1 + (i * x_inc), + y1 + (i * y_inc), + z1 + (i * z_inc) + ); + points_out.push_back(point); + } + } + return points_out; +} + +std::vector rectangle( + double length=1.0, + double width=2.0, + double inc=0.05) +{ + return polygon( + { + Eigen::Vector3d(-length/2, -width/2, 0), + Eigen::Vector3d(-length/2, width/2, 0), + Eigen::Vector3d( length/2, width/2, 0), + Eigen::Vector3d( length/2, -width/2, 0) + }, + inc + ); +} + +int main(int argc, char** argv) +{ + + ros::init(argc, argv, "test_segment_axes"); + ros::NodeHandle nh; + ros::Publisher pub = nh.advertise("segments", 10); + ros::Publisher eigen_pub_1 = nh.advertise("eigen_1", 10); + ros::Publisher eigen_pub_2 = nh.advertise("eigen_2", 10); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // create tool path + tool_path_planner::ToolPaths tool_paths; + tool_path_planner::ToolPath tool_path; + + std::vector points; + points = polygon( + { + Eigen::Vector3d(-1.0 , -0.8 , 0), + Eigen::Vector3d(-0.2 , 0 , 0), + Eigen::Vector3d(-1.0 , 0.5 , 0), + Eigen::Vector3d(-0.5 , 0.5 , 0), + Eigen::Vector3d(-0.6 , 1.0 , 0), + Eigen::Vector3d( 0 , 0.5 , 0), + Eigen::Vector3d( 1 , 0.5 , 0), + Eigen::Vector3d( 0.5 , 0 , 0), + Eigen::Vector3d( 0 , -1 , 0) + }, + 0.001 + ); +// points = rectangle(1, 2, 0.01); +// points = circle(1.0, 0.05); + + points = projectZToPlane(points, Eigen::Vector3d(1.0, 1.0, 0)); + points = applySinWaveByInc(points, M_PI/120, 0, Eigen::Vector3d(0.1, 0, 0)); + points = applySinWaveByInc(points, M_PI/120, M_PI/2, Eigen::Vector3d(0, 0.0, 0.1)); + points = applySinWaveToZ(points, 0.1, 0.1, Eigen::Vector3d(0.1, 0, 0)); + + tool_path.push_back(toSegment(points)); - tool_path.push_back(tool_path_segment); tool_paths.push_back(tool_path); - tool_paths = tool_path_planner::segmentByAxes(tool_paths); + Eigen::Vector3f major, perp; + tool_paths = tool_path_planner::segmentByAxes(tool_paths, major, perp); + + + visualization_msgs::Marker marker; + marker.header.frame_id = "world"; + marker.header.stamp = ros::Time::now(); + marker.ns = ""; + marker.id = 0; + marker.color.a = 1.0; + marker.color.r = 1; + marker.color.g = 0; + marker.color.b = 0; + marker.pose = tf2::toMsg(Eigen::Isometry3d::Identity()); + marker.type = visualization_msgs::Marker::LINE_STRIP; + marker.scale.x = 0.04; + marker.scale.y = 0.04; + marker.scale.z = 0.04; + geometry_msgs::Point p1; + p1.x = 0; + p1.y = 0; + p1.z = 0; + geometry_msgs::Point p2; + p2.x = major.x()*5; + p2.y = major.y()*5; + p2.z = major.z()*5; + marker.points.push_back(p1); + marker.points.push_back(p2); + eigen_pub_1.publish(marker); + + visualization_msgs::Marker marker2; + marker2.header.frame_id = "world"; + marker2.header.stamp = ros::Time::now(); + marker2.ns = ""; + marker2.id = 0; + marker2.color.a = 1.0; + marker2.color.r = 1; + marker2.color.g = 0; + marker2.color.b = 0; + marker2.pose = tf2::toMsg(Eigen::Isometry3d::Identity()); + marker2.type = visualization_msgs::Marker::LINE_STRIP; + marker2.scale.x = 0.04; + marker2.scale.y = 0.04; + marker2.scale.z = 0.04; + geometry_msgs::Point p4; + p4.x = perp.x()*5; + p4.y = perp.y()*5; + p4.z = perp.z()*5; + marker.points.push_back(p1); + marker.points.push_back(p4); + eigen_pub_2.publish(marker); std::size_t n = tool_paths[0].size(); std::vector> colors = { @@ -94,7 +284,7 @@ int main(int argc, char** argv) {1.0, 1.0, 0.0} }; - int marker_id = 0; + int marker_id = 2; visualization_msgs::MarkerArray marker_array; for (std::size_t i = 0; i < n; ++i) { @@ -111,17 +301,17 @@ int main(int argc, char** argv) marker.color.b = colors[i][2]; marker.pose = tf2::toMsg(pose); marker.type = visualization_msgs::Marker::SPHERE; - marker.scale.x = 0.02; - marker.scale.y = 0.02; - marker.scale.z = 0.02; + marker.scale.x = 0.04; + marker.scale.y = 0.04; + marker.scale.z = 0.04; marker_array.markers.push_back(marker); - pub.publish(marker_array); - std::this_thread::sleep_for(std::chrono::milliseconds(500)); +// pub.publish(marker_array); +// std::this_thread::sleep_for(std::chrono::milliseconds(500)); ++marker_id; } } -// pub.publish(marker_array); + pub.publish(marker_array); ROS_ERROR_STREAM("DONE."); diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index d484759f..06fb3cad 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -37,6 +37,7 @@ #include #include #include +#include namespace tool_path_planner { @@ -459,7 +460,8 @@ ToolPaths addExtraPaths(const ToolPaths& tool_paths, double offset_distance) return new_tool_paths; } -ToolPaths segmentByAxes(const ToolPaths& tool_paths, SegmentAxes::SegmentAxes segment_axes) +//ToolPaths segmentByAxes(const ToolPaths& tool_paths) +ToolPaths segmentByAxes(const ToolPaths& tool_paths, Eigen::Vector3f& major, Eigen::Vector3f& perp) { if (tool_paths.size() == 0) { @@ -467,157 +469,92 @@ ToolPaths segmentByAxes(const ToolPaths& tool_paths, SegmentAxes::SegmentAxes se // TODO: throw an exception } + using Point = pcl::PointXYZ; ToolPaths new_tool_paths; - double axes_distance = 1000.0; - std::vector corner_points; - - Eigen::Vector3d p1, p2, p3, p4; - if (segment_axes == SegmentAxes::XY) - { - p1.x() = -axes_distance; - p1.y() = -axes_distance; - p1.z() = 0; - - p2.x() = -axes_distance; - p2.y() = axes_distance; - p2.z() = 0; - - p3.x() = axes_distance; - p3.y() = -axes_distance; - p3.z() = 0; - - p4.x() = axes_distance; - p4.y() = axes_distance; - p4.z() = 0; - } - else if (segment_axes == SegmentAxes::XZ) - { - p1.x() = -axes_distance; - p1.y() = 0; - p1.z() = -axes_distance; - - p2.x() = -axes_distance; - p2.y() = 0; - p2.z() = axes_distance; - - p3.x() = axes_distance; - p3.y() = 0; - p3.z() = -axes_distance; - - p4.x() = axes_distance; - p4.y() = 0; - p4.z() = axes_distance; - } - else // if (segment_axes == SegmentAxes::YZ) - { - p1.x() = 0; - p1.y() = -axes_distance; - p1.z() = -axes_distance; - - p2.x() = 0; - p2.y() = -axes_distance; - p2.z() = axes_distance; - - p3.x() = 0; - p3.y() = axes_distance; - p3.z() = -axes_distance; - - p4.x() = 0; - p4.y() = axes_distance; - p4.z() = axes_distance; - } - - corner_points.push_back(p1); - corner_points.push_back(p2); - corner_points.push_back(p3); - corner_points.push_back(p4); - - for (std::size_t path_index = 0; path_index < tool_paths.size(); ++path_index) + for (std::size_t tool_path_index = 0; tool_path_index < tool_paths.size(); ++tool_path_index) { - ToolPath tool_path = tool_paths[path_index]; + ToolPath tool_path = tool_paths[tool_path_index]; + // Sanity check - tool path must have exactly one segment if (tool_path.size() != 1) { - ROS_ERROR_STREAM("Tool path " << path_index << " contains " << tool_path.size() << + ROS_ERROR_STREAM("Tool path " << tool_path_index << " contains " << tool_path.size() << " segments. Expected exactly 1."); // TODO: throw an exception } ToolPath new_tool_path; ToolPathSegment tool_path_segment = tool_path[0]; + // Sanity check - tool path segment must not be empty if (tool_path_segment.size() == 0) { - ROS_ERROR_STREAM("Tool path " << path_index << " segment 0 is empty."); + ROS_ERROR_STREAM("Tool path " << tool_path_index << " segment 0 is empty."); // TODO: throw an exception } - // get cut indices - std::vector cut_indices; - for (Eigen::Vector3d corner_point : corner_points) + // Get major and middle axis (we don't care about minor axis) + pcl::PointCloud::Ptr cloud = boost::make_shared>(); + for (Eigen::Isometry3d p: tool_path_segment) + { + Point point(float(p.translation().x()), float(p.translation().y()), float(p.translation().z())); + cloud->push_back(point); + } + pcl::MomentOfInertiaEstimation moment; + moment.setInputCloud(cloud); + moment.compute(); + Eigen::Vector3f major_axis, middle_axis, minor_axis; + if(!moment.getEigenVectors(major_axis, middle_axis, minor_axis)) { - double min_distance = axes_distance * 10; - int min_index = -1; - for (std::size_t point_index = 0; point_index < tool_path_segment.size(); ++ point_index) + ROS_ERROR_STREAM("Could not compute Eigen Vectors."); + } +// Eigen::Vector3f perp_axis = major_axis.transpose().cross(middle_axis);//.cross(major_axis); + Eigen::Vector3f perp_axis = major_axis.cross(middle_axis).cross(major_axis); + perp_axis.normalize(); + major_axis.normalize(); + major = major_axis; + perp = perp_axis; + std::vector vectors = { + -1 * major_axis + -1 * perp_axis, + -1 * major_axis + perp_axis, + major_axis + perp_axis, + major_axis + -1 * perp_axis + }; + // Get indices to cut tool path at + std::set cut_indices; + for (Eigen::Vector3f vector : vectors) + { + float max_dot = std::numeric_limits::min(); + int max_index = -1; + for (std::size_t index = 0; index < tool_path_segment.size(); ++index) { - Eigen::Vector3d projected_point = tool_path_segment[point_index].translation(); - if (segment_axes == SegmentAxes::XY) - { - projected_point.z() = 0; - } - else if (segment_axes == SegmentAxes::XZ) - { - projected_point.y() = 0; - } - else // if (segment_axes == SegmentAxes::YZ) - { - projected_point.x() = 0; - } - double distance = (projected_point - corner_point).norm(); - if (distance < min_distance) + Eigen::Isometry3d p = tool_path_segment[index]; + Eigen::Vector3f v(float(p.translation().x()), float(p.translation().y()), float(p.translation().z())); + float dot = vector.dot(v); + if (dot > max_dot) { - min_distance = distance; - min_index = int(point_index); + max_dot = dot; + max_index = int(index); } } - // check if we found a closest point - if (min_index == -1) - { - // if this happens, that means that all tool path points were more than (10 * axes_distance) meters from - // the origin -- this violates the assumption that tool path points are REASONABLY close to origin - ROS_ERROR_STREAM("Could not find closest point to corner point for path " << path_index << " segment 0 " - "for corner point at x:" << corner_point.x() << " y:" << corner_point.y() << " z:" << - corner_point.z()); - // TODO: throw a custom exception - } - // check if the point was also closest to a previous corner (i.e. already in list) - bool already_exists = false; - for (std::size_t cut_index : cut_indices) + cut_indices.insert(max_index); + } + // Cut segment into smaller segments (at cut indices) + for (std::set::iterator i = cut_indices.begin(); i != cut_indices.end(); ++i) + { + ToolPathSegment new_tool_path_segment; + int current_index = *i; + int next_index; + if (std::next(i, 1) == cut_indices.end()) { - if (cut_index == std::size_t(min_index)) - { - already_exists = true; - } + next_index = *cut_indices.begin(); } - // if this point isn't already in the list, add to cut indices - if (!already_exists) + else { - cut_indices.push_back(std::size_t(min_index)); + next_index = *std::next(i, 1); } - } - - // cut tool path - // sort cut indices - std::sort(cut_indices.begin(), cut_indices.end()); - // iterate each cut index - for (std::size_t i = 0; i < cut_indices.size(); ++i) - { - ToolPathSegment new_tool_path_segment; - std::size_t index = cut_indices[i]; - // while index does not equal the next cut index (including going from last index to first index) - // add each index after the current cut index, up until the next cut index - while (index != cut_indices[(i+1)%cut_indices.size()]) + while (current_index != next_index) { - new_tool_path_segment.push_back(tool_path_segment[index]); - index = (index + 1) % tool_path_segment.size(); + new_tool_path_segment.push_back(tool_path_segment[std::size_t(current_index)]); + current_index = (current_index + 1) % int(tool_path_segment.size()); } new_tool_path.push_back(new_tool_path_segment); } From 2b1527faeafb8581e7e0d5cbc22d8f42c3fbcbe7 Mon Sep 17 00:00:00 2001 From: Stevie Dale Date: Thu, 22 Apr 2021 22:08:32 -0500 Subject: [PATCH 25/64] added gtest unit test for segmentByAxes, viz not working and needs more test cases --- .../include/tool_path_planner/utilities.h | 28 +- tool_path_planner/src/test_segment_axes.cpp | 90 ++---- tool_path_planner/src/utilities.cpp | 297 +++++++++++------- tool_path_planner/test/utest.cpp | 215 +++++++++++++ 4 files changed, 435 insertions(+), 195 deletions(-) diff --git a/tool_path_planner/include/tool_path_planner/utilities.h b/tool_path_planner/include/tool_path_planner/utilities.h index 96fbceca..0c6175e5 100644 --- a/tool_path_planner/include/tool_path_planner/utilities.h +++ b/tool_path_planner/include/tool_path_planner/utilities.h @@ -53,10 +53,24 @@ void flipPointOrder(ToolPath& path); /** * @brief conversion function. Not well tested yet - * @param paths The toolpaths within a raster - * @return A vector of tool path data + * @param tool_path_segment A vector of Eigen::Isometry objects describing a segment of tool path poses + * @return A structure of vtk objects describing a segment of tool path poses */ -ToolPathsData toToolPathsData(const ToolPaths& paths); +ToolPathSegmentData toToolPathSegmentData(const ToolPathSegment& tool_path_segment); + +/** + * @brief conversion function. Not well tested yet + * @param tool_path A vector of ToolPathSegment objects + * @return A vector of ToolPathSegementData obects + */ +ToolPathData toToolPathData(const ToolPath& tool_path); + +/** + * @brief conversion function. Not well tested yet + * @param tool_paths A vector of ToolPath objects + * @return A vector of ToolPathData objects + */ +ToolPathsData toToolPathsData(const ToolPaths& tool_paths); /** * @details creates a rotation matrix from the column vectors; it can then be assigned to a Isometry3d pose @@ -128,10 +142,10 @@ ToolPaths reverseOddRasters(const ToolPaths& tool_paths, RasterStyle raster_styl double computeOffsetSign(const ToolPathSegment& adjusted_segment, const ToolPathSegment& away_from_segment); -//ToolPath segmentByAxes(const ToolPathSegment& tool_path_segment); -//ToolPath segmentByAxes(const ToolPathSegment& tool_path_segment, const Eigen::Vector3f& axis_1, const Eigen::Vector3f& axis_2); - -ToolPaths segmentByAxes(const ToolPaths& tool_paths, Eigen::Vector3f& major, Eigen::Vector3f& perp); +ToolPath segmentByAxes(const ToolPathSegment& tool_path_segment); +ToolPath segmentByAxes(const ToolPathSegment& tool_path_segment, const Eigen::Vector3f& axis_1, const Eigen::Vector3f& axis_2); +ToolPaths segmentByAxes(const ToolPaths& tool_paths); +ToolPaths segmentByAxes(const ToolPaths& tool_paths, const Eigen::Vector3f& axis_1, const Eigen::Vector3f& axis_2); } // namespace tool_path_planner diff --git a/tool_path_planner/src/test_segment_axes.cpp b/tool_path_planner/src/test_segment_axes.cpp index d8a6af7c..1cd3558c 100644 --- a/tool_path_planner/src/test_segment_axes.cpp +++ b/tool_path_planner/src/test_segment_axes.cpp @@ -198,83 +198,33 @@ int main(int argc, char** argv) tool_path_planner::ToolPath tool_path; std::vector points; - points = polygon( - { - Eigen::Vector3d(-1.0 , -0.8 , 0), - Eigen::Vector3d(-0.2 , 0 , 0), - Eigen::Vector3d(-1.0 , 0.5 , 0), - Eigen::Vector3d(-0.5 , 0.5 , 0), - Eigen::Vector3d(-0.6 , 1.0 , 0), - Eigen::Vector3d( 0 , 0.5 , 0), - Eigen::Vector3d( 1 , 0.5 , 0), - Eigen::Vector3d( 0.5 , 0 , 0), - Eigen::Vector3d( 0 , -1 , 0) - }, - 0.001 - ); +// points = polygon( +// { +// Eigen::Vector3d(-1.0 , -0.8 , 0), +// Eigen::Vector3d(-0.2 , 0 , 0), +// Eigen::Vector3d(-1.0 , 0.5 , 0), +// Eigen::Vector3d(-0.5 , 0.5 , 0), +// Eigen::Vector3d(-0.6 , 1.0 , 0), +// Eigen::Vector3d( 0 , 0.5 , 0), +// Eigen::Vector3d( 1 , 0.5 , 0), +// Eigen::Vector3d( 0.5 , 0 , 0), +// Eigen::Vector3d( 0 , -1 , 0) +// }, +// 0.001 +// ); // points = rectangle(1, 2, 0.01); -// points = circle(1.0, 0.05); + points = circle(1.0, 0.01); - points = projectZToPlane(points, Eigen::Vector3d(1.0, 1.0, 0)); - points = applySinWaveByInc(points, M_PI/120, 0, Eigen::Vector3d(0.1, 0, 0)); - points = applySinWaveByInc(points, M_PI/120, M_PI/2, Eigen::Vector3d(0, 0.0, 0.1)); - points = applySinWaveToZ(points, 0.1, 0.1, Eigen::Vector3d(0.1, 0, 0)); +// points = projectZToPlane(points, Eigen::Vector3d(1.0, 1.0, 0)); +// points = applySinWaveByInc(points, M_PI/120, 0, Eigen::Vector3d(0.1, 0, 0)); +// points = applySinWaveByInc(points, M_PI/120, M_PI/2, Eigen::Vector3d(0, 0.0, 0.1)); +// points = applySinWaveToZ(points, 0.1, 0.1, Eigen::Vector3d(0.1, 0, 0)); tool_path.push_back(toSegment(points)); tool_paths.push_back(tool_path); - Eigen::Vector3f major, perp; - tool_paths = tool_path_planner::segmentByAxes(tool_paths, major, perp); - - - visualization_msgs::Marker marker; - marker.header.frame_id = "world"; - marker.header.stamp = ros::Time::now(); - marker.ns = ""; - marker.id = 0; - marker.color.a = 1.0; - marker.color.r = 1; - marker.color.g = 0; - marker.color.b = 0; - marker.pose = tf2::toMsg(Eigen::Isometry3d::Identity()); - marker.type = visualization_msgs::Marker::LINE_STRIP; - marker.scale.x = 0.04; - marker.scale.y = 0.04; - marker.scale.z = 0.04; - geometry_msgs::Point p1; - p1.x = 0; - p1.y = 0; - p1.z = 0; - geometry_msgs::Point p2; - p2.x = major.x()*5; - p2.y = major.y()*5; - p2.z = major.z()*5; - marker.points.push_back(p1); - marker.points.push_back(p2); - eigen_pub_1.publish(marker); - - visualization_msgs::Marker marker2; - marker2.header.frame_id = "world"; - marker2.header.stamp = ros::Time::now(); - marker2.ns = ""; - marker2.id = 0; - marker2.color.a = 1.0; - marker2.color.r = 1; - marker2.color.g = 0; - marker2.color.b = 0; - marker2.pose = tf2::toMsg(Eigen::Isometry3d::Identity()); - marker2.type = visualization_msgs::Marker::LINE_STRIP; - marker2.scale.x = 0.04; - marker2.scale.y = 0.04; - marker2.scale.z = 0.04; - geometry_msgs::Point p4; - p4.x = perp.x()*5; - p4.y = perp.y()*5; - p4.z = perp.z()*5; - marker.points.push_back(p1); - marker.points.push_back(p4); - eigen_pub_2.publish(marker); + tool_paths = tool_path_planner::segmentByAxes(tool_paths, Eigen::Vector3f(1.0, 0, 0), Eigen::Vector3f(0.0, 1.0, 0.0)); std::size_t n = tool_paths[0].size(); std::vector> colors = { diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index 06fb3cad..dacf3d66 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -56,55 +56,72 @@ void flipPointOrder(ToolPath& path) p *= Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ()); } -tool_path_planner::ToolPathsData toToolPathsData(const tool_path_planner::ToolPaths& paths) +tool_path_planner::ToolPathSegmentData toToolPathSegmentData(const tool_path_planner::ToolPathSegment& tool_path_segment) { using namespace Eigen; - tool_path_planner::ToolPathsData results; - for (const auto& path : paths) + tool_path_planner::ToolPathSegmentData tool_path_segment_data; + tool_path_segment_data.line = vtkSmartPointer::New(); + tool_path_segment_data.derivatives = vtkSmartPointer::New(); + + // set vertex (cell) normals + vtkSmartPointer points = vtkSmartPointer::New(); + vtkSmartPointer line_normals = vtkSmartPointer::New(); + line_normals->SetNumberOfComponents(3); // 3d normals (ie x,y,z) + line_normals->SetNumberOfTuples(static_cast(tool_path_segment.size())); + vtkSmartPointer der_normals = vtkSmartPointer::New(); + der_normals->SetNumberOfComponents(3); // 3d normals (ie x,y,z) + der_normals->SetNumberOfTuples(static_cast(tool_path_segment.size())); + + int idx = 0; + for (auto& pose : tool_path_segment) { - tool_path_planner::ToolPathData tp_data; - for (const auto& segment : path) - { - tool_path_planner::ToolPathSegmentData tps_data; - tps_data.line = vtkSmartPointer::New(); - tps_data.derivatives = vtkSmartPointer::New(); - - // set vertex (cell) normals - vtkSmartPointer points = vtkSmartPointer::New(); - vtkSmartPointer line_normals = vtkSmartPointer::New(); - line_normals->SetNumberOfComponents(3); // 3d normals (ie x,y,z) - line_normals->SetNumberOfTuples(static_cast(segment.size())); - vtkSmartPointer der_normals = vtkSmartPointer::New(); - ; - der_normals->SetNumberOfComponents(3); // 3d normals (ie x,y,z) - der_normals->SetNumberOfTuples(static_cast(segment.size())); - - int idx = 0; - for (auto& pose : segment) - { - Vector3d point, vx, vy, vz; - point = pose.translation(); - vx = pose.linear().col(0); - vx *= -1.0; - vy = pose.linear().col(1); - vz = pose.linear().col(2); - points->InsertNextPoint(point.data()); - line_normals->SetTuple(idx, vz.data()); - der_normals->SetTuple(idx, vx.data()); - - idx++; - } - tps_data.line->SetPoints(points); - tps_data.line->GetPointData()->SetNormals(line_normals); - tps_data.derivatives->GetPointData()->SetNormals(der_normals); - tps_data.derivatives->SetPoints(points); + Vector3d point, vx, vy, vz; + point = pose.translation(); + vx = pose.linear().col(0); + vx *= -1.0; + vy = pose.linear().col(1); + vz = pose.linear().col(2); + points->InsertNextPoint(point.data()); + line_normals->SetTuple(idx, vz.data()); + der_normals->SetTuple(idx, vx.data()); + + idx++; + } + tool_path_segment_data.line->SetPoints(points); + tool_path_segment_data.line->GetPointData()->SetNormals(line_normals); + tool_path_segment_data.derivatives->GetPointData()->SetNormals(der_normals); + tool_path_segment_data.derivatives->SetPoints(points); - tp_data.push_back(tps_data); - } - results.push_back(tp_data); + return tool_path_segment_data; +} + +tool_path_planner::ToolPathData toToolPathData(const tool_path_planner::ToolPath& tool_path) +{ + using namespace Eigen; + + tool_path_planner::ToolPathData tool_path_data; + for (const auto& tool_path_segment : tool_path) + { + tool_path_planner::ToolPathSegmentData tool_path_segment_data = toToolPathSegmentData(tool_path_segment); + tool_path_data.push_back(tool_path_segment_data); + } + + return tool_path_data; +} + +tool_path_planner::ToolPathsData toToolPathsData(const tool_path_planner::ToolPaths& tool_paths) +{ + using namespace Eigen; + + tool_path_planner::ToolPathsData tool_paths_data; + for (const auto& tool_path : tool_paths) + { + tool_path_planner::ToolPathData tool_path_data = toToolPathData(tool_path); + tool_paths_data.push_back(tool_path_data); } - return results; + + return tool_paths_data; } Eigen::Matrix3d toRotationMatrix(const Eigen::Vector3d& vx, const Eigen::Vector3d& vy, const Eigen::Vector3d& vz) @@ -460,8 +477,100 @@ ToolPaths addExtraPaths(const ToolPaths& tool_paths, double offset_distance) return new_tool_paths; } -//ToolPaths segmentByAxes(const ToolPaths& tool_paths) -ToolPaths segmentByAxes(const ToolPaths& tool_paths, Eigen::Vector3f& major, Eigen::Vector3f& perp) +ToolPath segmentByAxes(const ToolPathSegment& tool_path_segment) +{ + using Point = pcl::PointXYZ; + // Sanity check - tool path segment must not be empty + if (tool_path_segment.size() == 0) + { + ROS_ERROR_STREAM("Tool path segment 0 is empty."); + // TODO: throw an exception + } + + // Get major and middle axis (we don't care about minor axis) + pcl::PointCloud::Ptr cloud = boost::make_shared>(); + for (Eigen::Isometry3d p: tool_path_segment) + { + Point point(float(p.translation().x()), float(p.translation().y()), float(p.translation().z())); + cloud->push_back(point); + } + pcl::MomentOfInertiaEstimation moment; + moment.setInputCloud(cloud); + moment.compute(); + Eigen::Vector3f major_axis, middle_axis, minor_axis; + if(!moment.getEigenVectors(major_axis, middle_axis, minor_axis)) + { + ROS_ERROR_STREAM("Could not compute Eigen Vectors."); + } + // Calculated perpendicular (and parallel to plane) axis using triple product + Eigen::Vector3f perp_axis = major_axis.cross(middle_axis).cross(major_axis); + // Normalize vectors + perp_axis.normalize(); + major_axis.normalize(); + // Call base function + return segmentByAxes(tool_path_segment, major_axis, perp_axis); +} + +ToolPath segmentByAxes(const ToolPathSegment& tool_path_segment, const Eigen::Vector3f& axis_1, const Eigen::Vector3f& axis_2) +{ + // Sanity check - tool path segment must not be empty + if (tool_path_segment.size() == 0) + { + ROS_ERROR_STREAM("Tool path segment 0 is empty."); + // TODO: throw an exception + } + + ToolPath new_tool_path; + std::vector vectors = { + -1 * axis_1 + -1 * axis_2, + -1 * axis_1 + axis_2, + axis_1 + axis_2, + axis_1 + -1 * axis_2 + }; + // Get indices to cut tool path at + std::set cut_indices; + for (Eigen::Vector3f vector : vectors) + { + float max_dot = std::numeric_limits::min(); + int max_index = -1; + for (std::size_t index = 0; index < tool_path_segment.size(); ++index) + { + Eigen::Isometry3d p = tool_path_segment[index]; + Eigen::Vector3f v(float(p.translation().x()), float(p.translation().y()), float(p.translation().z())); + float dot = vector.dot(v); + if (dot > max_dot) + { + max_dot = dot; + max_index = int(index); + } + } + cut_indices.insert(max_index); + } + // Cut segment into smaller segments (at cut indices) + for (std::set::iterator i = cut_indices.begin(); i != cut_indices.end(); ++i) + { + ToolPathSegment new_tool_path_segment; + int current_index = *i; + int next_index; + if (std::next(i, 1) == cut_indices.end()) + { + next_index = *cut_indices.begin(); + } + else + { + next_index = *std::next(i, 1); + } + while (current_index != next_index) + { + new_tool_path_segment.push_back(tool_path_segment[std::size_t(current_index)]); + current_index = (current_index + 1) % int(tool_path_segment.size()); + } + new_tool_path.push_back(new_tool_path_segment); + } + return new_tool_path; +} + +ToolPaths segmentByAxes(const ToolPaths& tool_paths) { if (tool_paths.size() == 0) { @@ -482,82 +591,34 @@ ToolPaths segmentByAxes(const ToolPaths& tool_paths, Eigen::Vector3f& major, Eig " segments. Expected exactly 1."); // TODO: throw an exception } - ToolPath new_tool_path; - ToolPathSegment tool_path_segment = tool_path[0]; - // Sanity check - tool path segment must not be empty - if (tool_path_segment.size() == 0) - { - ROS_ERROR_STREAM("Tool path " << tool_path_index << " segment 0 is empty."); - // TODO: throw an exception - } + ToolPath new_tool_path = segmentByAxes(tool_path[0]); + new_tool_paths.push_back(new_tool_path); + } + return new_tool_paths; +} - // Get major and middle axis (we don't care about minor axis) - pcl::PointCloud::Ptr cloud = boost::make_shared>(); - for (Eigen::Isometry3d p: tool_path_segment) - { - Point point(float(p.translation().x()), float(p.translation().y()), float(p.translation().z())); - cloud->push_back(point); - } - pcl::MomentOfInertiaEstimation moment; - moment.setInputCloud(cloud); - moment.compute(); - Eigen::Vector3f major_axis, middle_axis, minor_axis; - if(!moment.getEigenVectors(major_axis, middle_axis, minor_axis)) - { - ROS_ERROR_STREAM("Could not compute Eigen Vectors."); - } -// Eigen::Vector3f perp_axis = major_axis.transpose().cross(middle_axis);//.cross(major_axis); - Eigen::Vector3f perp_axis = major_axis.cross(middle_axis).cross(major_axis); - perp_axis.normalize(); - major_axis.normalize(); - major = major_axis; - perp = perp_axis; - std::vector vectors = { - -1 * major_axis + -1 * perp_axis, - -1 * major_axis + perp_axis, - major_axis + perp_axis, - major_axis + -1 * perp_axis - }; - // Get indices to cut tool path at - std::set cut_indices; - for (Eigen::Vector3f vector : vectors) - { - float max_dot = std::numeric_limits::min(); - int max_index = -1; - for (std::size_t index = 0; index < tool_path_segment.size(); ++index) - { - Eigen::Isometry3d p = tool_path_segment[index]; - Eigen::Vector3f v(float(p.translation().x()), float(p.translation().y()), float(p.translation().z())); - float dot = vector.dot(v); - if (dot > max_dot) - { - max_dot = dot; - max_index = int(index); - } - } - cut_indices.insert(max_index); - } - // Cut segment into smaller segments (at cut indices) - for (std::set::iterator i = cut_indices.begin(); i != cut_indices.end(); ++i) +ToolPaths segmentByAxes(const ToolPaths& tool_paths, const Eigen::Vector3f& axis_1, const Eigen::Vector3f& axis_2) +{ + if (tool_paths.size() == 0) + { + ROS_ERROR_STREAM("Tool paths is empty."); + // TODO: throw an exception + } + + using Point = pcl::PointXYZ; + ToolPaths new_tool_paths; + + for (std::size_t tool_path_index = 0; tool_path_index < tool_paths.size(); ++tool_path_index) + { + ToolPath tool_path = tool_paths[tool_path_index]; + // Sanity check - tool path must have exactly one segment + if (tool_path.size() != 1) { - ToolPathSegment new_tool_path_segment; - int current_index = *i; - int next_index; - if (std::next(i, 1) == cut_indices.end()) - { - next_index = *cut_indices.begin(); - } - else - { - next_index = *std::next(i, 1); - } - while (current_index != next_index) - { - new_tool_path_segment.push_back(tool_path_segment[std::size_t(current_index)]); - current_index = (current_index + 1) % int(tool_path_segment.size()); - } - new_tool_path.push_back(new_tool_path_segment); + ROS_ERROR_STREAM("Tool path " << tool_path_index << " contains " << tool_path.size() << + " segments. Expected exactly 1."); + // TODO: throw an exception } + ToolPath new_tool_path = segmentByAxes(tool_path[0], axis_1, axis_2); new_tool_paths.push_back(new_tool_path); } return new_tool_paths; diff --git a/tool_path_planner/test/utest.cpp b/tool_path_planner/test/utest.cpp index b5255792..f15cee5e 100644 --- a/tool_path_planner/test/utest.cpp +++ b/tool_path_planner/test/utest.cpp @@ -353,6 +353,209 @@ void runExtraRasterTest(tool_path_planner::PathGenerator& planner, #endif } +void runSegmentByAxesTest(const tool_path_planner::ToolPathSegment& tool_path_segment, + const Eigen::Vector3f& axis_1, const Eigen::Vector3f& axis_2) +{ + vtk_viewer::VTKViewer viz; + tool_path_planner::ToolPath tool_path = tool_path_planner::segmentByAxes(tool_path_segment, axis_1, axis_2); + tool_path_planner::ToolPathData tool_path_data = tool_path_planner::toToolPathData(tool_path); + std::cerr << "n_points_in_data: " << tool_path_data[0].line->GetNumberOfPoints() << std::endl; + double scale = 1.0; + + std::vector> colors = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + {1, 1, 0}, + }; + + // Display surface normals + if (DISPLAY_NORMALS) + { + std::cerr << "Displayin'" << std::endl; + for (std::size_t i = 0; i < tool_path.size(); ++i) + { + viz.addPolyNormalsDisplay(tool_path_data[i].line, colors[i], scale); + } + } +#ifdef NDEBUG + // release build stuff goes here + CONSOLE_BRIDGE_logError("noether/tool_path_planner test: visualization is only available in debug mode"); +#else + // Debug-specific code goes here + std::cerr << "We vizin'" << std::endl; + viz.renderDisplay(); +#endif +} + +/** + * @brief Convert a list of points into a ToolPathSegment object + * @param points A vector of points + * @return A ToolPathSegment object generated from the points vector + */ +tool_path_planner::ToolPathSegment toSegment(std::vector points) +{ + tool_path_planner::ToolPathSegment tool_path_segment; + for (auto p : points) + { + Eigen::Isometry3d point; + point.translation().x() = p.x(); + point.translation().y() = p.y(); + point.translation().z() = p.z(); + tool_path_segment.push_back(point); + } + return tool_path_segment; +} + +/** + * @brief Generates a list of points forming a circle + * @param radius The radius of the generated circle + * @param angle_inc Increment degree for generated points around the circle + * @return Points of circle + */ +std::vector getCircle( + double radius = 1.0, + double angle_inc = DEG2RAD(5)) +{ + std::vector points; + + for (int i = 0; i < int(std::round(2 * M_PI / angle_inc)); ++i) + { + Eigen::Vector3d p; + double angle = i * angle_inc; + if (abs(angle - DEG2RAD(0.0)) < 0.001) + { + p.x() = radius; + p.y() = 0; + } + else if (abs(angle - DEG2RAD(90.0)) < 0.001) + { + p.x() = 0; + p.y() = radius; + } + else if (abs(angle - DEG2RAD(180.0)) < 0.001) + { + p.x() = -radius; + p.y() = 0; + } + else if (abs(angle - DEG2RAD(270.0)) < 0.001) + { + p.x() = 0; + p.y() = -radius; + } + else + { + double theta = angle; + int y_sign = 1; + int x_sign = 1; + if (DEG2RAD(90.0) < angle < DEG2RAD(180.0)) + { + theta = DEG2RAD(180.0) - angle; + x_sign = -1; + } + else if (DEG2RAD(180.0) < angle < DEG2RAD(270.0)) + { + theta = angle - DEG2RAD(180.0); + y_sign = -1; + x_sign = -1; + } + else if (DEG2RAD(270.0) < angle < DEG2RAD(360.0)) + { + theta = DEG2RAD(360.0) - angle; + y_sign = -1; + } + p.x() = x_sign * radius * cos(theta); + p.y() = y_sign * radius * sin(theta); + } + points.push_back(p); + } + return points; +} + +/** + * @brief Generates a list of points forming a polygon (from the provided vertices) + * @param vertices A list of vertices defining a polygon. Last point does NOT need to + * be equal to first points (the algorithm interpolated between last and first point + * automatically) + * @param inc Increment length for interpolated points + * @return Points of polygon + */ +std::vector getPolygon( + std::vector vertices, + double inc = 0.05) +{ + std::vector points; + for (std::size_t point_index = 0; point_index < vertices.size(); ++point_index) + { + double x1 = vertices[point_index].x(); + double y1 = vertices[point_index].y(); + double z1 = vertices[point_index].z(); + double x2 = vertices[(point_index+1)%vertices.size()].x(); + double y2 = vertices[(point_index+1)%vertices.size()].y(); + double z2 = vertices[(point_index+1)%vertices.size()].z(); + double dist = std::sqrt(std::pow(x1 - x2, 2) + std::pow(y1 - y2, 2) + std::pow(z1 - z2, 2)); + int n = int(std::round(dist / inc)); + double x_inc = (x2 - x1)/n; + double y_inc = (y2 - y1)/n; + double z_inc = (z2 - z1)/n; + for (int i = 0; i < n; ++i) + { + Eigen::Vector3d point( + x1 + (i * x_inc), + y1 + (i * y_inc), + z1 + (i * z_inc) + ); + points.push_back(point); + } + } + return points; +} + +/** + * @brief Generates a list of points forming a rectangle + * @param length Length of rectangle + * @param width Width of rectangle + * @return Points of rectangle + */ +std::vector getRectangle( + double length=1.0, + double width=2.0, + double inc=0.05) +{ + return getPolygon( + { + Eigen::Vector3d(-length/2, -width/2, 0), + Eigen::Vector3d(-length/2, width/2, 0), + Eigen::Vector3d( length/2, width/2, 0), + Eigen::Vector3d( length/2, -width/2, 0) + }, + inc + ); +} + +/** + * @brief project points to plane along z-axis + * @param points_in Points to project + * @param plane_coeff Coefficients of homogeneous plane equation: for 0 = Ax + By + Cz + * @return Projected points + */ +std::vector projectZToPlane( + std::vector points_in, + Eigen::Vector3d plane_coeff) +{ + std::vector points_out; + for (auto p : points_in) + { + double x = plane_coeff.x(); + double y = plane_coeff.y(); + // Project z value onto plane --> z = (-Ax - By) / C + p.z() = (-1*x*p.x() + -1*y*p.y())/p.z(); + points_out.push_back(p); + } + return points_out; +} + +/* TEST(IntersectTest, SurfaceWalkRasterRotationTest) { vtkSmartPointer mesh = createTestMesh1(); @@ -540,6 +743,18 @@ TEST(IntersectTest, SurfaceWalkExtraRasterTest) runExtraRasterTest(planner, planner_with_extra, mesh); } +*/ + +TEST(ProvidedAxes1Test, SegmentByAxesTest) +{ + std::vector points = getCircle(1.0, DEG2RAD(1.0)); + std::cerr << "n_points: " << points.size() << std::endl; + points = projectZToPlane(points, Eigen::Vector3d(1, 1, 1)); + tool_path_planner::ToolPathSegment tool_path_segment = toSegment(points); + std::cerr << "n_points_in_seg: " << tool_path_segment.size() << std::endl; + + runSegmentByAxesTest(tool_path_segment, Eigen::Vector3f(1.0, 1.0, 0), Eigen::Vector3f(0, 1.0, 1.0)); +} int main(int argc, char** argv) { From bf3547d09b71cfeeedec48c0384b18437b7fee3f Mon Sep 17 00:00:00 2001 From: drchrislewis Date: Wed, 28 Apr 2021 12:46:36 -0500 Subject: [PATCH 26/64] updated addWaypoints to prefer the original over the shifted. This produces smoother paths --- tool_path_planner/src/utilities.cpp | 110 +++++++++++++++++++++------- tool_path_planner/test/utest.cpp | 3 +- 2 files changed, 84 insertions(+), 29 deletions(-) diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index 8bb564b9..6fdaf9d2 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -39,6 +39,10 @@ namespace tool_path_planner { +typedef std::tuple wp_tuple; +typedef std::list tlist; +typedef std::list::iterator tlist_it; + void flipPointOrder(ToolPath& path) { // Reverse the order of the segments @@ -413,10 +417,7 @@ double computePathDistance(const ToolPath& path, const Eigen::Isometry3d waypoin return (dist); } -bool compare_pair(std::pair first, std::pair second) -{ - return (first.first < second.first); -} +bool compare_tuple(wp_tuple& first, wp_tuple& second) { return (std::get<0>(first) < std::get<0>(second)); } ToolPaths addExtraPaths(const ToolPaths& tool_paths, double offset_distance) { @@ -483,31 +484,84 @@ ToolPaths addExtraPaths(const ToolPaths& tool_paths, double offset_distance) return new_tool_paths; } -ToolPath sortAndSegment(std::list >& waypoint_list, double point_spacing) +void testAndMark(tlist_it& item1, tlist_it& item2, double point_spacing) +{ + int mark1 = std::get<2>(*item1); + int mark2 = std::get<2>(*item2); + Eigen::Isometry3d wp1 = std::get<1>(*item1); + Eigen::Isometry3d wp2 = std::get<1>(*item2); + Eigen::Vector3d v = wp1.translation() - wp2.translation(); + if (v.norm() < 0.75 * point_spacing) + { + if (mark1 == 1 && mark2 == 0) + { + std::get<2>(*item2) = -1; + } + else if (mark2 == 1 && mark1 == 0) + { + std::get<2>(*item1) = -1; + } + } +} + +tlist pruneList(tlist& waypoint_list, double point_spacing) +{ + // mark any shifted waypoint for deletion if its got an original neighbor close by + tlist_it first = waypoint_list.begin(); + tlist_it second = std::next(waypoint_list.begin(), 1); + tlist_it third = std::next(waypoint_list.begin(), 2); + testAndMark(first, second, point_spacing); + testAndMark(first, third, point_spacing); + testAndMark(second, third, point_spacing); + for (tlist_it it = std::next(waypoint_list.begin(), 2); it != waypoint_list.end(); it++) + { + tlist_it it2 = std::next(it, -2); + tlist_it it1 = std::next(it, -1); + testAndMark(it2, it1, point_spacing); + testAndMark(it2, it, point_spacing); + testAndMark(it1, it, point_spacing); + } + + tlist new_list; + for (tlist_it it = waypoint_list.begin(); it != waypoint_list.end(); it++) + { + if (std::get<2>(*it) != -1) + { + new_list.push_back(*it); + } + } + return (new_list); +} + +ToolPath sortAndSegment(std::list >& waypoint_list, double point_spacing) { - waypoint_list.sort(compare_pair); - // we now have a sorted list including all prev-offset, this & next-offset tool_path waypoints points - // now we just need to prune it to create new paths - // note add each point only if it is close to the point_spacing from previoius point - // create a new segment whenever the distance between successive waypoints is large relative to point_spaceing ToolPath new_tool_path; ToolPathSegment seg; - std::pair wp_pair = waypoint_list.front(); - Eigen::Isometry3d last_wp = wp_pair.second; - double last_dot = wp_pair.first; + + waypoint_list.sort(compare_tuple); + // we now have a sorted list including all points offset from adjacent rasters but marked with a 0 and originals + // marked with a 1 + + // next, we prune those marked with 0 if they are close to those marked with 1 + waypoint_list = pruneList(waypoint_list, point_spacing); + + // next, we prune those simply too close to one another + Eigen::Isometry3d last_wp = std::get<1>(waypoint_list.front()); + double last_dot = std::get<0>(waypoint_list.front()); seg.push_back(last_wp); int q = 0; - for (std::pair waypoint_pair : waypoint_list) + for (wp_tuple waypoint_tuple : waypoint_list) { - Eigen::Isometry3d waypoint = waypoint_pair.second; + Eigen::Isometry3d waypoint = std::get<1>(waypoint_tuple); + int mark = std::get<2>(waypoint_tuple); Eigen::Vector3d v = waypoint.translation() - last_wp.translation(); - double dist2 = sqrt(v.x() * v.x() + v.y() * v.y() + v.z() * v.z()); - double dist = waypoint_pair.first - last_dot; - if (dist > .75 * point_spacing && dist <= 1.25 * point_spacing) + double dist2 = v.norm(); + double dist = std::get<0>(waypoint_tuple) - last_dot; + if (dist > .75 * point_spacing && dist <= 1.25 * point_spacing || mark == 1) { seg.push_back(waypoint); last_wp = waypoint; - last_dot = waypoint_pair.first; + last_dot = std::get<0>(waypoint_tuple); } else if (dist > 1.25 * point_spacing) { // start a new segment @@ -515,14 +569,14 @@ ToolPath sortAndSegment(std::list >& waypoi seg.clear(); seg.push_back(last_wp); last_wp = waypoint; - last_dot = waypoint_pair.first; + last_dot = std::get<0>(waypoint_tuple); } else { // skip unless last in list if (q == waypoint_list.size() - 1 && dist > .25 * point_spacing) { seg.push_back(waypoint); // keep the last on regardless of distance to proces up to the defined edges - last_dot = waypoint_pair.first; + last_dot = std::get<0>(waypoint_tuple); } } q++; @@ -554,8 +608,8 @@ ToolPaths addExtraWaypoints(const ToolPaths& tool_paths, double raster_spacing, // end the old and start a new segment when the new point is significantly more than the point_spacing from previous // waypoint ToolPath tool_path = tool_paths[i]; - std::list > waypoint_list; // once sorted this list will be the order of the - // segments + std::list > waypoint_list; // once sorted this list will be the order of + // the segments Eigen::Vector3d path_dir = computePathDirection(tool_path); ToolPathSegment sseg = tool_path[0]; Eigen::Vector3d path_start = sseg[0].translation(); @@ -571,7 +625,7 @@ ToolPaths addExtraWaypoints(const ToolPaths& tool_paths, double raster_spacing, waypoint.translation().y() += offset_sign * raster_spacing * H(1, 1); waypoint.translation().z() += offset_sign * raster_spacing * H(2, 1); Eigen::Vector3d v = waypoint.translation() - path_start; - std::pair p(path_dir.dot(v), waypoint); + std::tuple p(path_dir.dot(v), waypoint, 0); waypoint_list.push_back(p); } // end for every waypoint in segment } // end for every segment in path @@ -584,7 +638,7 @@ ToolPaths addExtraWaypoints(const ToolPaths& tool_paths, double raster_spacing, for (Eigen::Isometry3d waypoint : seg) { Eigen::Vector3d v = waypoint.translation() - path_start; - std::pair p(path_dir.dot(v), waypoint); + std::tuple p(path_dir.dot(v), waypoint, 1); waypoint_list.push_back(p); } // end for every waypoint in segment } // end for every segment in path @@ -600,7 +654,7 @@ ToolPaths addExtraWaypoints(const ToolPaths& tool_paths, double raster_spacing, waypoint.translation().y() -= offset_sign * raster_spacing * H(1, 1); waypoint.translation().z() -= offset_sign * raster_spacing * H(2, 1); Eigen::Vector3d v = waypoint.translation() - path_start; - std::pair p(path_dir.dot(v), waypoint); + std::tuple p(path_dir.dot(v), waypoint, 0); waypoint_list.push_back(p); } // end for every waypoint in segment } // end for every segment in path @@ -617,7 +671,7 @@ ToolPaths addExtraWaypoints(const ToolPaths& tool_paths, double raster_spacing, waypoint.translation().y() += offset_sign * raster_spacing * H(1, 1); waypoint.translation().z() += offset_sign * raster_spacing * H(2, 1); Eigen::Vector3d v = waypoint.translation() - path_start; - std::pair p(path_dir.dot(v), waypoint); + std::tuple p(path_dir.dot(v), waypoint, 0); waypoint_list.push_back(p); } // end for every waypoint in segment } // end for every segment in path @@ -636,7 +690,7 @@ ToolPaths addExtraWaypoints(const ToolPaths& tool_paths, double raster_spacing, waypoint.translation().y() -= offset_sign * raster_spacing * H(1, 1); waypoint.translation().z() -= offset_sign * raster_spacing * H(2, 1); Eigen::Vector3d v = waypoint.translation() - path_start; - std::pair p(path_dir.dot(v), waypoint); + std::tuple p(path_dir.dot(v), waypoint, 1); waypoint_list.push_back(p); } // end for every waypoint in segment diff --git a/tool_path_planner/test/utest.cpp b/tool_path_planner/test/utest.cpp index 28a17191..c345b4aa 100644 --- a/tool_path_planner/test/utest.cpp +++ b/tool_path_planner/test/utest.cpp @@ -396,6 +396,7 @@ void runExtraRasterTest(tool_path_planner::PathGenerator& planner, #endif } +/* TEST(IntersectTest, SurfaceWalkRasterRotationTest) { vtkSmartPointer mesh = createTestMesh1(); @@ -583,7 +584,7 @@ TEST(IntersectTest, SurfaceWalkExtraRasterTest) runExtraRasterTest(planner, planner_with_extra, mesh); } - +*/ TEST(PlaneSlicerTest, PlaneSlicerExtraWaypointTest) { vtkSmartPointer mesh = createTestMesh1(); From 6bee60fcc1e149b29b321695f71f92e8679b7cd5 Mon Sep 17 00:00:00 2001 From: drchrislewis Date: Thu, 29 Apr 2021 11:05:17 -0500 Subject: [PATCH 27/64] modified prunning to use dot product as method. --- tool_path_planner/src/utilities.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index 6fdaf9d2..cb112c09 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -488,10 +488,12 @@ void testAndMark(tlist_it& item1, tlist_it& item2, double point_spacing) { int mark1 = std::get<2>(*item1); int mark2 = std::get<2>(*item2); - Eigen::Isometry3d wp1 = std::get<1>(*item1); - Eigen::Isometry3d wp2 = std::get<1>(*item2); - Eigen::Vector3d v = wp1.translation() - wp2.translation(); - if (v.norm() < 0.75 * point_spacing) + // Eigen::Isometry3d wp1 = std::get<1>(*item1); + // Eigen::Isometry3d wp2 = std::get<1>(*item2); + // Eigen::Vector3d v = wp1.translation() - wp2.translation(); + // if (v.norm() < 0.75 * point_spacing) + double dist = fabs(std::get<0>(*item1) - std::get<0>(*item2)); + if(dist < 0.75 * point_spacing) { if (mark1 == 1 && mark2 == 0) { From 4ccb86ac04a7e6f6259376f42adbe9777c7870a2 Mon Sep 17 00:00:00 2001 From: Stevie Dale Date: Thu, 29 Apr 2021 16:48:43 -0500 Subject: [PATCH 28/64] replaces splitSegements() with segmentByAxes() in edge generators --- .../include/tool_path_planner/eigen_value_edge_generator.h | 2 +- .../include/tool_path_planner/halfedge_edge_generator.h | 4 ++-- tool_path_planner/src/eigen_value_edge_generator.cpp | 4 ++-- tool_path_planner/src/halfedge_edge_generator.cpp | 4 ++-- tool_path_planner/src/utilities.cpp | 2 +- 5 files changed, 8 insertions(+), 8 deletions(-) diff --git a/tool_path_planner/include/tool_path_planner/eigen_value_edge_generator.h b/tool_path_planner/include/tool_path_planner/eigen_value_edge_generator.h index d10c9f33..fb167177 100644 --- a/tool_path_planner/include/tool_path_planner/eigen_value_edge_generator.h +++ b/tool_path_planner/include/tool_path_planner/eigen_value_edge_generator.h @@ -100,7 +100,7 @@ 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 */ + bool segment_by_axes = true; /** @brief flag indicating whether returned toolpaths should be segmented along the major and middle axis */ }; EigenValueEdgeGenerator() = default; diff --git a/tool_path_planner/include/tool_path_planner/halfedge_edge_generator.h b/tool_path_planner/include/tool_path_planner/halfedge_edge_generator.h index 4924fde0..38fb9129 100644 --- a/tool_path_planner/include/tool_path_planner/halfedge_edge_generator.h +++ b/tool_path_planner/include/tool_path_planner/halfedge_edge_generator.h @@ -67,8 +67,8 @@ 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; + /** @brief flag indicating whether returned toolpaths should be segmented along the major and middle axis */ + bool segment_by_axes = true; }; HalfedgeEdgeGenerator() = default; diff --git a/tool_path_planner/src/eigen_value_edge_generator.cpp b/tool_path_planner/src/eigen_value_edge_generator.cpp index fd6e73c8..84861d41 100644 --- a/tool_path_planner/src/eigen_value_edge_generator.cpp +++ b/tool_path_planner/src/eigen_value_edge_generator.cpp @@ -331,9 +331,9 @@ boost::optional EigenValueEdgeGenerator::generate() } } - if (config_.max_segment_length > 0) + if (config_.segment_by_axes) { - edge_paths = tool_path_planner::splitSegments(edge_paths, config_.max_segment_length); + edge_paths = tool_path_planner::segmentByAxes(edge_paths); } CONSOLE_BRIDGE_logInform("Found %lu valid edge segments", edge_paths.size()); diff --git a/tool_path_planner/src/halfedge_edge_generator.cpp b/tool_path_planner/src/halfedge_edge_generator.cpp index b6979f45..148c8c45 100644 --- a/tool_path_planner/src/halfedge_edge_generator.cpp +++ b/tool_path_planner/src/halfedge_edge_generator.cpp @@ -580,9 +580,9 @@ boost::optional HalfedgeEdgeGenerator::generate() return boost::none; } - if (config_.max_segment_length > 0) + if (config_.segment_by_axes) { - edge_paths = tool_path_planner::splitSegments(edge_paths, config_.max_segment_length); + edge_paths = tool_path_planner::segmentByAxes(edge_paths); } CONSOLE_BRIDGE_logInform("Found %lu valid edge segments", edge_paths.size()); diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index dacf3d66..73ab3aea 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -158,7 +158,7 @@ bool toEigenValueConfigMsg(noether_msgs::EigenValueEdgeGeneratorConfig& config_m config_msg.min_projection_dist = config.min_projection_dist; config_msg.max_intersecting_voxels = config.max_intersecting_voxels; config_msg.merge_dist = config.merge_dist; - config_msg.max_segment_length = config.max_segment_length; + config_msg.segment_by_axes = config.segment_by_axes; return true; } From 725267dae23da7a6320dc5b26ff45418940745f8 Mon Sep 17 00:00:00 2001 From: drchrislewis Date: Tue, 4 May 2021 15:59:35 -0500 Subject: [PATCH 29/64] modified conditions for adding extra waypoints. This should eliminate errant extra waypoints added due to high curvature issues. --- tool_path_planner/src/utilities.cpp | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index cb112c09..07823cc5 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -557,15 +557,22 @@ ToolPath sortAndSegment(std::list >& Eigen::Isometry3d waypoint = std::get<1>(waypoint_tuple); int mark = std::get<2>(waypoint_tuple); Eigen::Vector3d v = waypoint.translation() - last_wp.translation(); - double dist2 = v.norm(); - double dist = std::get<0>(waypoint_tuple) - last_dot; - if (dist > .75 * point_spacing && dist <= 1.25 * point_spacing || mark == 1) + double cart_spacing = v.norm(); + double dot_spacing = std::get<0>(waypoint_tuple) - last_dot; + + // complex if statement + // spacing computed using dot-distance is close to the point spacing AND + // spacing computed using cartesian distance is close to point spacing + // then add add to segment + if (dot_spacing > .75 * point_spacing && dot_spacing <= 1.25 * point_spacing && + cart_spacing > .75 * point_spacing && cart_spacing <= 1.25 * point_spacing || + mark == 1) { seg.push_back(waypoint); last_wp = waypoint; last_dot = std::get<0>(waypoint_tuple); } - else if (dist > 1.25 * point_spacing) + else if (dot_spacing > 1.25 * point_spacing) // only add extra if dot spacing is large { // start a new segment new_tool_path.push_back(seg); seg.clear(); @@ -575,7 +582,7 @@ ToolPath sortAndSegment(std::list >& } else { // skip unless last in list - if (q == waypoint_list.size() - 1 && dist > .25 * point_spacing) + if (q == waypoint_list.size() - 1 && dot_spacing > .25 * point_spacing) { seg.push_back(waypoint); // keep the last on regardless of distance to proces up to the defined edges last_dot = std::get<0>(waypoint_tuple); From d42398420601e2e51192877bfdbf494ea5867e13 Mon Sep 17 00:00:00 2001 From: drchrislewis Date: Wed, 5 May 2021 10:55:15 -0500 Subject: [PATCH 30/64] fixed a few bugs in addExtraWaypoints, added a test that loads a mesh from /tmp if it exists to facilitate better testing --- tool_path_planner/src/utilities.cpp | 27 ++++++++++++--------- tool_path_planner/test/utest.cpp | 37 ++++++++++++++++++++++++++--- 2 files changed, 50 insertions(+), 14 deletions(-) diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index 07823cc5..33726917 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -564,34 +564,39 @@ ToolPath sortAndSegment(std::list >& // spacing computed using dot-distance is close to the point spacing AND // spacing computed using cartesian distance is close to point spacing // then add add to segment - if (dot_spacing > .75 * point_spacing && dot_spacing <= 1.25 * point_spacing && - cart_spacing > .75 * point_spacing && cart_spacing <= 1.25 * point_spacing || - mark == 1) + if (dot_spacing > .7 * point_spacing && dot_spacing <= 1.3 * point_spacing && + cart_spacing > .7 * point_spacing && cart_spacing <= 1.3 * point_spacing || + (mark == 1 && dot_spacing >0.0 && cart_spacing< 1.3 * point_spacing)) { seg.push_back(waypoint); last_wp = waypoint; last_dot = std::get<0>(waypoint_tuple); } - else if (dot_spacing > 1.25 * point_spacing) // only add extra if dot spacing is large + else if (dot_spacing > 1.3 * point_spacing && cart_spacing > 1.3 * point_spacing) // only add extra if dot spacing is large { // start a new segment - new_tool_path.push_back(seg); + if(seg.size()>3) + { + new_tool_path.push_back(seg); // throw away very short segments + } seg.clear(); - seg.push_back(last_wp); + seg.push_back(waypoint); last_wp = waypoint; last_dot = std::get<0>(waypoint_tuple); } else - { // skip unless last in list - if (q == waypoint_list.size() - 1 && dot_spacing > .25 * point_spacing) + { // skip unless last in list, significantly spaced from last waypoint, but not too far + if (q == waypoint_list.size() - 1 && dot_spacing > .3 * point_spacing && cart_spacing < 1.3*point_spacing) { - seg.push_back(waypoint); // keep the last on regardless of distance to proces up to the defined edges + // seg.push_back(waypoint); // keep the last on regardless of distance to proces up to the defined edges last_dot = std::get<0>(waypoint_tuple); } } q++; } // end for every waypoint in waypoint_list - new_tool_path.push_back(seg); - + if(seg.size()>3) + { + new_tool_path.push_back(seg); + } return (new_tool_path); } diff --git a/tool_path_planner/test/utest.cpp b/tool_path_planner/test/utest.cpp index c345b4aa..4f00667c 100644 --- a/tool_path_planner/test/utest.cpp +++ b/tool_path_planner/test/utest.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include #include @@ -20,6 +21,17 @@ #define DISPLAY_DERIVATIVES 1 #define POINT_SPACING 1.0 +vtkSmartPointer loadTempMesh() +{ + vtkSmartPointer polydata; + vtkSmartPointer reader = vtkSmartPointer::New (); + reader->SetFileName ("/tmp/test_mesh.ply"); + reader->Update (); + polydata = reader->GetOutput (); + printf("Loaded /tmp/test_mesh.ply with %ld points/vertices.\n", polydata->GetNumberOfPoints ()); + return polydata; +} + vtkSmartPointer createTestMesh1(double sample_spacing = 0.5) { // Get mesh @@ -255,7 +267,8 @@ void runTestCaseRansac(tool_path_planner::PathGenerator& planner, vtkSmartPointe void runExtraRasterTest(tool_path_planner::PathGenerator& planner, tool_path_planner::PathGenerator& planner_with_extras, - vtkSmartPointer mesh) + vtkSmartPointer mesh, + double scale = 1.0) { // Set input mesh planner.setInput(mesh); @@ -322,7 +335,6 @@ void runExtraRasterTest(tool_path_planner::PathGenerator& planner, vtk_viewer::VTKViewer viz; vtk_viewer::VTKViewer viz2; std::vector color(3); - double scale = 1.0; // Display mesh results color[0] = 0.9f; @@ -628,7 +640,7 @@ TEST(PlaneSlicerTest, PlaneSlicerExtraWaypointTest2) tool.min_segment_size = 0.05; tool.search_radius = 0.05; tool.min_hole_size = 0.8; - // tool.raster_wrt_global_axes = use:: tool_path_planner::PlaneSlicerRasterGenerator::DEFAULT_RASTER_WRT_GLOBAL_AXES; + tool.raster_direction = Eigen::Vector3d::UnitY(); tool.generate_extra_rasters = false; // tool.raster_style = use:: tool_path_planner::PlaneSlicerRasterGenerator::KEEP_ORIENTATION_ON_REVERSE_STROKES; @@ -639,6 +651,25 @@ TEST(PlaneSlicerTest, PlaneSlicerExtraWaypointTest2) planner_with_extra.setConfiguration(tool); runExtraRasterTest(planner, planner_with_extra, mesh); + + mesh = loadTempMesh(); + + tool.raster_spacing = .1; + tool.point_spacing = .025; + tool.raster_rot_offset = 0.0; + tool.min_segment_size = 0.05; + tool.search_radius = 0.05; + tool.min_hole_size = 0.008; + tool.raster_direction = Eigen::Vector3d::UnitY(); + tool.generate_extra_rasters = false; + // tool.raster_wrt_global_axes = use:: tool_path_planner::PlaneSlicerRasterGenerator::DEFAULT_RASTER_WRT_GLOBAL_AXES; + // tool.raster_style = use:: tool_path_planner::PlaneSlicerRasterGenerator::KEEP_ORIENTATION_ON_REVERSE_STROKES; + planner.setConfiguration(tool); + + tool.generate_extra_rasters = true; + planner_with_extra.setConfiguration(tool); + + runExtraRasterTest(planner, planner_with_extra, mesh, 0.1); } int main(int argc, char** argv) From d8ab78e25c9f846068e1fa73e06a460ac529a05e Mon Sep 17 00:00:00 2001 From: Colin Lewis Date: Thu, 6 May 2021 11:23:54 -0500 Subject: [PATCH 31/64] edge path points are condsidered in a centroid frame to improve behavior --- .../msg/EigenValueEdgeGeneratorConfig.msg | 3 +++ noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg | 3 +++ tool_path_planner/src/utilities.cpp | 16 +++++++++++++++- 3 files changed, 21 insertions(+), 1 deletion(-) diff --git a/noether_msgs/msg/EigenValueEdgeGeneratorConfig.msg b/noether_msgs/msg/EigenValueEdgeGeneratorConfig.msg index b82ae79d..0aea566a 100644 --- a/noether_msgs/msg/EigenValueEdgeGeneratorConfig.msg +++ b/noether_msgs/msg/EigenValueEdgeGeneratorConfig.msg @@ -30,3 +30,6 @@ float64 merge_dist # maximum segment length float64 max_segment_length + +# flag to break edge paths by diagonal axes +bool segment_by_axes diff --git a/noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg b/noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg index 45788338..9f4f35a2 100644 --- a/noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg +++ b/noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg @@ -23,3 +23,6 @@ float64 point_dist # maximum segment length float64 max_segment_length + +# flag to break edge paths by diagonal axes +bool segment_by_axes diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index 73ab3aea..5b153cad 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -529,6 +529,19 @@ ToolPath segmentByAxes(const ToolPathSegment& tool_path_segment, const Eigen::Ve }; // Get indices to cut tool path at std::set cut_indices; + + Eigen::Vector3f path_center(0.0, 0.0, 0.0); + for (std::size_t index = 0; index < tool_path_segment.size(); ++index) + { + Eigen::Isometry3d p = tool_path_segment[index]; + path_center[0] += p.translation().x(); + path_center[1] += p.translation().y(); + path_center[2] += p.translation().z(); + } + + path_center = path_center / tool_path_segment.size(); + + std::vector translated_points; for (Eigen::Vector3f vector : vectors) { float max_dot = std::numeric_limits::min(); @@ -536,7 +549,8 @@ ToolPath segmentByAxes(const ToolPathSegment& tool_path_segment, const Eigen::Ve for (std::size_t index = 0; index < tool_path_segment.size(); ++index) { Eigen::Isometry3d p = tool_path_segment[index]; - Eigen::Vector3f v(float(p.translation().x()), float(p.translation().y()), float(p.translation().z())); + Eigen::Vector3f v(float(p.translation().x() - path_center[0]), float(p.translation().y() - path_center[1]), float(p.translation().z() - path_center[2])); + translated_points.push_back(v); float dot = vector.dot(v); if (dot > max_dot) { From e3bc6665540b0937cca2464285cd509b4343c9b9 Mon Sep 17 00:00:00 2001 From: Colin Lewis Date: Thu, 6 May 2021 11:28:00 -0500 Subject: [PATCH 32/64] removed debug data structure --- tool_path_planner/src/utilities.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index 5b153cad..e32645b7 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -541,7 +541,6 @@ ToolPath segmentByAxes(const ToolPathSegment& tool_path_segment, const Eigen::Ve path_center = path_center / tool_path_segment.size(); - std::vector translated_points; for (Eigen::Vector3f vector : vectors) { float max_dot = std::numeric_limits::min(); @@ -550,7 +549,6 @@ ToolPath segmentByAxes(const ToolPathSegment& tool_path_segment, const Eigen::Ve { Eigen::Isometry3d p = tool_path_segment[index]; Eigen::Vector3f v(float(p.translation().x() - path_center[0]), float(p.translation().y() - path_center[1]), float(p.translation().z() - path_center[2])); - translated_points.push_back(v); float dot = vector.dot(v); if (dot > max_dot) { From 510511a2b33e085634d4b92ee5dd69100a4d2f7b Mon Sep 17 00:00:00 2001 From: Stevie Dale Date: Thu, 6 May 2021 12:37:57 -0500 Subject: [PATCH 33/64] refactored segementByAxes to splitByAxes --- .../msg/EigenValueEdgeGeneratorConfig.msg | 5 +---- noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg | 5 +---- .../eigen_value_edge_generator.h | 2 +- .../tool_path_planner/halfedge_edge_generator.h | 4 ++-- .../include/tool_path_planner/utilities.h | 8 ++++---- .../src/eigen_value_edge_generator.cpp | 4 ++-- .../src/halfedge_edge_generator.cpp | 4 ++-- tool_path_planner/src/test_segment_axes.cpp | 2 +- tool_path_planner/src/utilities.cpp | 16 ++++++++-------- tool_path_planner/test/utest.cpp | 2 +- 10 files changed, 23 insertions(+), 29 deletions(-) diff --git a/noether_msgs/msg/EigenValueEdgeGeneratorConfig.msg b/noether_msgs/msg/EigenValueEdgeGeneratorConfig.msg index 0aea566a..37669d7c 100644 --- a/noether_msgs/msg/EigenValueEdgeGeneratorConfig.msg +++ b/noether_msgs/msg/EigenValueEdgeGeneratorConfig.msg @@ -28,8 +28,5 @@ 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 - # flag to break edge paths by diagonal axes -bool segment_by_axes +bool split_by_axes diff --git a/noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg b/noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg index 9f4f35a2..78a25016 100644 --- a/noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg +++ b/noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg @@ -21,8 +21,5 @@ int32 point_spacing_method # Point distance parameter used in conjunction with the spacing method float64 point_dist -# maximum segment length -float64 max_segment_length - # flag to break edge paths by diagonal axes -bool segment_by_axes +bool split_by_axes diff --git a/tool_path_planner/include/tool_path_planner/eigen_value_edge_generator.h b/tool_path_planner/include/tool_path_planner/eigen_value_edge_generator.h index fb167177..3cc30cfb 100644 --- a/tool_path_planner/include/tool_path_planner/eigen_value_edge_generator.h +++ b/tool_path_planner/include/tool_path_planner/eigen_value_edge_generator.h @@ -100,7 +100,7 @@ class EigenValueEdgeGenerator : public PathGenerator double merge_dist = 0.01; /** @brief any two consecutive points with a shortest distance smaller than this value are merged */ - bool segment_by_axes = true; /** @brief flag indicating whether returned toolpaths should be segmented along the major and middle axis */ + bool split_by_axes = true; /** @brief flag indicating whether returned toolpaths should be split along the major and middle axis */ }; EigenValueEdgeGenerator() = default; diff --git a/tool_path_planner/include/tool_path_planner/halfedge_edge_generator.h b/tool_path_planner/include/tool_path_planner/halfedge_edge_generator.h index 38fb9129..3544d1e9 100644 --- a/tool_path_planner/include/tool_path_planner/halfedge_edge_generator.h +++ b/tool_path_planner/include/tool_path_planner/halfedge_edge_generator.h @@ -67,8 +67,8 @@ class HalfedgeEdgeGenerator : public PathGenerator /**@brief point distance parameter used in conjunction with the spacing method */ double point_dist = 0.01; - /** @brief flag indicating whether returned toolpaths should be segmented along the major and middle axis */ - bool segment_by_axes = true; + /** @brief flag indicating whether returned toolpaths should be split along the major and middle axis */ + bool split_by_axes = true; }; HalfedgeEdgeGenerator() = default; diff --git a/tool_path_planner/include/tool_path_planner/utilities.h b/tool_path_planner/include/tool_path_planner/utilities.h index 0c6175e5..e58b9f23 100644 --- a/tool_path_planner/include/tool_path_planner/utilities.h +++ b/tool_path_planner/include/tool_path_planner/utilities.h @@ -142,10 +142,10 @@ ToolPaths reverseOddRasters(const ToolPaths& tool_paths, RasterStyle raster_styl double computeOffsetSign(const ToolPathSegment& adjusted_segment, const ToolPathSegment& away_from_segment); -ToolPath segmentByAxes(const ToolPathSegment& tool_path_segment); -ToolPath segmentByAxes(const ToolPathSegment& tool_path_segment, const Eigen::Vector3f& axis_1, const Eigen::Vector3f& axis_2); -ToolPaths segmentByAxes(const ToolPaths& tool_paths); -ToolPaths segmentByAxes(const ToolPaths& tool_paths, const Eigen::Vector3f& axis_1, const Eigen::Vector3f& axis_2); +ToolPath splitByAxes(const ToolPathSegment& tool_path_segment); +ToolPath splitByAxes(const ToolPathSegment& tool_path_segment, const Eigen::Vector3f& axis_1, const Eigen::Vector3f& axis_2); +ToolPaths splitByAxes(const ToolPaths& tool_paths); +ToolPaths splitByAxes(const ToolPaths& tool_paths, const Eigen::Vector3f& axis_1, const Eigen::Vector3f& axis_2); } // namespace tool_path_planner diff --git a/tool_path_planner/src/eigen_value_edge_generator.cpp b/tool_path_planner/src/eigen_value_edge_generator.cpp index 84861d41..3c493933 100644 --- a/tool_path_planner/src/eigen_value_edge_generator.cpp +++ b/tool_path_planner/src/eigen_value_edge_generator.cpp @@ -331,9 +331,9 @@ boost::optional EigenValueEdgeGenerator::generate() } } - if (config_.segment_by_axes) + if (config_.split_by_axes) { - edge_paths = tool_path_planner::segmentByAxes(edge_paths); + edge_paths = tool_path_planner::splitByAxes(edge_paths); } CONSOLE_BRIDGE_logInform("Found %lu valid edge segments", edge_paths.size()); diff --git a/tool_path_planner/src/halfedge_edge_generator.cpp b/tool_path_planner/src/halfedge_edge_generator.cpp index 148c8c45..54fd577c 100644 --- a/tool_path_planner/src/halfedge_edge_generator.cpp +++ b/tool_path_planner/src/halfedge_edge_generator.cpp @@ -580,9 +580,9 @@ boost::optional HalfedgeEdgeGenerator::generate() return boost::none; } - if (config_.segment_by_axes) + if (config_.split_by_axes) { - edge_paths = tool_path_planner::segmentByAxes(edge_paths); + edge_paths = tool_path_planner::splitByAxes(edge_paths); } CONSOLE_BRIDGE_logInform("Found %lu valid edge segments", edge_paths.size()); diff --git a/tool_path_planner/src/test_segment_axes.cpp b/tool_path_planner/src/test_segment_axes.cpp index 1cd3558c..80e24206 100644 --- a/tool_path_planner/src/test_segment_axes.cpp +++ b/tool_path_planner/src/test_segment_axes.cpp @@ -224,7 +224,7 @@ int main(int argc, char** argv) tool_paths.push_back(tool_path); - tool_paths = tool_path_planner::segmentByAxes(tool_paths, Eigen::Vector3f(1.0, 0, 0), Eigen::Vector3f(0.0, 1.0, 0.0)); + tool_paths = tool_path_planner::splitByAxes(tool_paths, Eigen::Vector3f(1.0, 0, 0), Eigen::Vector3f(0.0, 1.0, 0.0)); std::size_t n = tool_paths[0].size(); std::vector> colors = { diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index e32645b7..b521e46b 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -158,7 +158,7 @@ bool toEigenValueConfigMsg(noether_msgs::EigenValueEdgeGeneratorConfig& config_m config_msg.min_projection_dist = config.min_projection_dist; config_msg.max_intersecting_voxels = config.max_intersecting_voxels; config_msg.merge_dist = config.merge_dist; - config_msg.segment_by_axes = config.segment_by_axes; + config_msg.split_by_axes = config.split_by_axes; return true; } @@ -477,7 +477,7 @@ ToolPaths addExtraPaths(const ToolPaths& tool_paths, double offset_distance) return new_tool_paths; } -ToolPath segmentByAxes(const ToolPathSegment& tool_path_segment) +ToolPath splitByAxes(const ToolPathSegment& tool_path_segment) { using Point = pcl::PointXYZ; // Sanity check - tool path segment must not be empty @@ -508,10 +508,10 @@ ToolPath segmentByAxes(const ToolPathSegment& tool_path_segment) perp_axis.normalize(); major_axis.normalize(); // Call base function - return segmentByAxes(tool_path_segment, major_axis, perp_axis); + return splitByAxes(tool_path_segment, major_axis, perp_axis); } -ToolPath segmentByAxes(const ToolPathSegment& tool_path_segment, const Eigen::Vector3f& axis_1, const Eigen::Vector3f& axis_2) +ToolPath splitByAxes(const ToolPathSegment& tool_path_segment, const Eigen::Vector3f& axis_1, const Eigen::Vector3f& axis_2) { // Sanity check - tool path segment must not be empty if (tool_path_segment.size() == 0) @@ -582,7 +582,7 @@ ToolPath segmentByAxes(const ToolPathSegment& tool_path_segment, const Eigen::Ve return new_tool_path; } -ToolPaths segmentByAxes(const ToolPaths& tool_paths) +ToolPaths splitByAxes(const ToolPaths& tool_paths) { if (tool_paths.size() == 0) { @@ -603,13 +603,13 @@ ToolPaths segmentByAxes(const ToolPaths& tool_paths) " segments. Expected exactly 1."); // TODO: throw an exception } - ToolPath new_tool_path = segmentByAxes(tool_path[0]); + ToolPath new_tool_path = splitByAxes(tool_path[0]); new_tool_paths.push_back(new_tool_path); } return new_tool_paths; } -ToolPaths segmentByAxes(const ToolPaths& tool_paths, const Eigen::Vector3f& axis_1, const Eigen::Vector3f& axis_2) +ToolPaths splitByAxes(const ToolPaths& tool_paths, const Eigen::Vector3f& axis_1, const Eigen::Vector3f& axis_2) { if (tool_paths.size() == 0) { @@ -630,7 +630,7 @@ ToolPaths segmentByAxes(const ToolPaths& tool_paths, const Eigen::Vector3f& axis " segments. Expected exactly 1."); // TODO: throw an exception } - ToolPath new_tool_path = segmentByAxes(tool_path[0], axis_1, axis_2); + ToolPath new_tool_path = splitByAxes(tool_path[0], axis_1, axis_2); new_tool_paths.push_back(new_tool_path); } return new_tool_paths; diff --git a/tool_path_planner/test/utest.cpp b/tool_path_planner/test/utest.cpp index f15cee5e..9fe1539e 100644 --- a/tool_path_planner/test/utest.cpp +++ b/tool_path_planner/test/utest.cpp @@ -357,7 +357,7 @@ void runSegmentByAxesTest(const tool_path_planner::ToolPathSegment& tool_path_se const Eigen::Vector3f& axis_1, const Eigen::Vector3f& axis_2) { vtk_viewer::VTKViewer viz; - tool_path_planner::ToolPath tool_path = tool_path_planner::segmentByAxes(tool_path_segment, axis_1, axis_2); + tool_path_planner::ToolPath tool_path = tool_path_planner::splitByAxes(tool_path_segment, axis_1, axis_2); tool_path_planner::ToolPathData tool_path_data = tool_path_planner::toToolPathData(tool_path); std::cerr << "n_points_in_data: " << tool_path_data[0].line->GetNumberOfPoints() << std::endl; double scale = 1.0; From 793fdb425b8b309b509e1161c314b1663a32bdf0 Mon Sep 17 00:00:00 2001 From: "David Merz, Jr" Date: Wed, 12 May 2021 08:37:27 -0500 Subject: [PATCH 34/64] Reset Value in Utest --- tool_path_planner/test/utest.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tool_path_planner/test/utest.cpp b/tool_path_planner/test/utest.cpp index 4f00667c..9b84bf4e 100644 --- a/tool_path_planner/test/utest.cpp +++ b/tool_path_planner/test/utest.cpp @@ -19,7 +19,7 @@ #define DISPLAY_LINES 1 #define DISPLAY_NORMALS 0 #define DISPLAY_DERIVATIVES 1 -#define POINT_SPACING 1.0 +#define POINT_SPACING 0.5 vtkSmartPointer loadTempMesh() { @@ -268,7 +268,7 @@ void runTestCaseRansac(tool_path_planner::PathGenerator& planner, vtkSmartPointe void runExtraRasterTest(tool_path_planner::PathGenerator& planner, tool_path_planner::PathGenerator& planner_with_extras, vtkSmartPointer mesh, - double scale = 1.0) + double scale = 1.0) { // Set input mesh planner.setInput(mesh); From 2861348ee2d888602bcf70052c2ab26150b3a3bb Mon Sep 17 00:00:00 2001 From: "David Merz, Jr" Date: Mon, 3 May 2021 09:03:06 -0500 Subject: [PATCH 35/64] Fix specification of raster direction to be more consistent --- .../msg/PlaneSlicerRasterGeneratorConfig.msg | 5 ++--- .../plane_slicer_raster_generator.h | 2 +- .../src/plane_slicer_raster_generator.cpp | 13 ++++++++++++- 3 files changed, 15 insertions(+), 5 deletions(-) diff --git a/noether_msgs/msg/PlaneSlicerRasterGeneratorConfig.msg b/noether_msgs/msg/PlaneSlicerRasterGeneratorConfig.msg index 1b9dfe3b..17b012b8 100644 --- a/noether_msgs/msg/PlaneSlicerRasterGeneratorConfig.msg +++ b/noether_msgs/msg/PlaneSlicerRasterGeneratorConfig.msg @@ -12,7 +12,6 @@ bool generate_extra_rasters # Whether an extra raster stroke should be added to 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. +# direction from the first point in one raster to the second point of the same raster. Specified +# in the frame of the input mesh. Overrides raster_wrt_global_axes 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 20fe74d3..1ea3212d 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 @@ -59,7 +59,7 @@ class PlaneSlicerRasterGenerator : public PathGenerator 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() }; + Eigen::Vector3d raster_direction{ Eigen::Vector3d::Zero() }; bool generate_extra_rasters{ DEFAULT_GENERATE_EXTRA_RASTERS }; RasterStyle raster_style{ DEFAULT_RASTER_STYLE }; Json::Value toJson() const diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index a4117ad0..17e8c073 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -594,7 +594,18 @@ boost::optional PlaneSlicerRasterGenerator::generate() Isometry3d rotation_offset = Isometry3d::Identity() * AngleAxisd(config_.raster_rot_offset, Vector3d::UnitZ()); // Calculate direction of raster strokes, rotated by the above-specified amount - Vector3d raster_dir = (rotation_offset * config_.raster_direction).normalized(); + Vector3d raster_dir; + if (config_.raster_direction.isApprox(Eigen::Vector3d::Zero())) + { + // If no direction was specified, use the middle dimension of the bounding box + config_.raster_direction = Eigen::Vector3d::UnitY(); + raster_dir = (rotation_offset * config_.raster_direction).normalized(); + } + else + { + // If a direction was specified, transform it into the frame of the bounding box + raster_dir = (t * rotation_offset * config_.raster_direction).normalized(); + } // Calculate all 8 corners projected onto the raster direction vector Eigen::VectorXd dist(8); From f28beacdb4a01d70c49ee5743c4c6f1a7fa1d018 Mon Sep 17 00:00:00 2001 From: "David Merz, Jr" Date: Mon, 3 May 2021 09:20:14 -0500 Subject: [PATCH 36/64] Fix repeated uses causing changes in behavior --- tool_path_planner/src/plane_slicer_raster_generator.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index 17e8c073..b8446ec5 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -598,8 +598,8 @@ boost::optional PlaneSlicerRasterGenerator::generate() if (config_.raster_direction.isApprox(Eigen::Vector3d::Zero())) { // If no direction was specified, use the middle dimension of the bounding box - config_.raster_direction = Eigen::Vector3d::UnitY(); - raster_dir = (rotation_offset * config_.raster_direction).normalized(); + raster_dir = Eigen::Vector3d::UnitY(); + raster_dir = (rotation_offset * raster_dir).normalized(); } else { From a3bea86370433c4d4fcbd0f4ea2fcd653c32b535 Mon Sep 17 00:00:00 2001 From: "David Merz, Jr" Date: Tue, 4 May 2021 16:29:38 -0500 Subject: [PATCH 37/64] Fix Vector Math --- tool_path_planner/src/plane_slicer_raster_generator.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index b8446ec5..953c04a7 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -597,6 +597,7 @@ boost::optional PlaneSlicerRasterGenerator::generate() Vector3d raster_dir; if (config_.raster_direction.isApprox(Eigen::Vector3d::Zero())) { + ROS_ERROR("APPROX ZERO"); // If no direction was specified, use the middle dimension of the bounding box raster_dir = Eigen::Vector3d::UnitY(); raster_dir = (rotation_offset * raster_dir).normalized(); @@ -604,7 +605,7 @@ boost::optional PlaneSlicerRasterGenerator::generate() else { // If a direction was specified, transform it into the frame of the bounding box - raster_dir = (t * rotation_offset * config_.raster_direction).normalized(); + raster_dir = (rotation_offset * t * config_.raster_direction).normalized(); } // Calculate all 8 corners projected onto the raster direction vector From 734f27c5674e3d23d243811cf8921b3f379142bc Mon Sep 17 00:00:00 2001 From: "David Merz, Jr" Date: Tue, 11 May 2021 23:25:00 -0500 Subject: [PATCH 38/64] Fix Strange Deflection when Specifying Raster Direction Directly Specified raster directions in the plane slicer were modified in a seemingly random way. It turns out that I was transforming the raster direction (A 3d vector) with the full transform from mesh frame to bounding box frame. However, it was being treated as a point, not a vector - i.e., the translation was being applied. I have enacted a change to ensure that only the rotation component is now being applied, and added a test case to the unit test to verify sensible behavior. --- .../src/plane_slicer_raster_generator.cpp | 7 +++- tool_path_planner/test/utest.cpp | 32 +++++++++++++++++++ 2 files changed, 38 insertions(+), 1 deletion(-) diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index 953c04a7..2478a4af 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -507,6 +507,8 @@ boost::optional PlaneSlicerRasterGenerator::generate() using namespace Eigen; using IDVec = std::vector; + console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_ERROR); + if (!mesh_data_) { CONSOLE_BRIDGE_logDebug("%s No mesh data has been provided", getName().c_str()); @@ -605,7 +607,10 @@ boost::optional PlaneSlicerRasterGenerator::generate() else { // If a direction was specified, transform it into the frame of the bounding box - raster_dir = (rotation_offset * t * config_.raster_direction).normalized(); + raster_dir = + (rotation_offset * // Rotation about short axis of bounding box + AngleAxisd(computeRotation(x_dir, y_dir, z_dir)) * // Rotation part of 't' (recalculated because Eigen makes it hard to access) + config_.raster_direction).normalized(); // Raster direction specified by user } // Calculate all 8 corners projected onto the raster direction vector diff --git a/tool_path_planner/test/utest.cpp b/tool_path_planner/test/utest.cpp index 9b84bf4e..b02f7ff0 100644 --- a/tool_path_planner/test/utest.cpp +++ b/tool_path_planner/test/utest.cpp @@ -5,6 +5,7 @@ * */ +#include #include #include #include @@ -454,6 +455,37 @@ TEST(IntersectTest, PlaneSlicerRasterRotationTest) } } +TEST(IntersectTest, PlaneSlicerRasterSpecificDirectionTest) +{ + vtkSmartPointer mesh = createTestMesh1(); + + // First, run wrt_global_axes, using default axis + // Set input tool data + tool_path_planner::PlaneSlicerRasterGenerator planner; + tool_path_planner::PlaneSlicerRasterGenerator::Config tool; + tool.point_spacing = POINT_SPACING; + tool.raster_spacing = 0.75; + // tool.tool_offset = 0.0; // currently unused + tool.min_hole_size = 0.1; + tool.raster_rot_offset = 0.0; + tool.raster_wrt_global_axes = true; + // tool.debug = false; + planner.setConfiguration(tool); + runRasterRotationTest(planner, mesh); + + // Second, run with specified axis + tool.point_spacing = POINT_SPACING; + tool.raster_spacing = 0.75; + // tool.tool_offset = 0.0; // currently unused + tool.min_hole_size = 0.1; + tool.raster_rot_offset = 0.0; + tool.raster_wrt_global_axes = false; + tool.raster_direction = Eigen::Vector3d::UnitY(); + // tool.debug = false; + planner.setConfiguration(tool); + runRasterRotationTest(planner, mesh); +} + TEST(IntersectTest, HalfedgeEdgeGeneratorTestCase0) { vtkSmartPointer mesh = createTestMesh1(); From f805353e07ea6c216afb250839b775abae8614a3 Mon Sep 17 00:00:00 2001 From: "David Merz, Jr" Date: Wed, 12 May 2021 08:33:23 -0500 Subject: [PATCH 39/64] Appease Clang and Fixup Unit Test --- .../src/plane_slicer_raster_generator.cpp | 9 ++--- tool_path_planner/src/utilities.cpp | 34 ++++++++++--------- tool_path_planner/test/utest.cpp | 17 +++++----- 3 files changed, 31 insertions(+), 29 deletions(-) diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index 2478a4af..1f640f03 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -607,10 +607,11 @@ boost::optional PlaneSlicerRasterGenerator::generate() else { // If a direction was specified, transform it into the frame of the bounding box - raster_dir = - (rotation_offset * // Rotation about short axis of bounding box - AngleAxisd(computeRotation(x_dir, y_dir, z_dir)) * // Rotation part of 't' (recalculated because Eigen makes it hard to access) - config_.raster_direction).normalized(); // Raster direction specified by user + raster_dir = (rotation_offset * // Rotation about short axis of bounding box + AngleAxisd(computeRotation(x_dir, y_dir, z_dir)) * // Rotation part of 't' (recalculated because + // Eigen makes it hard to access) + config_.raster_direction) // Raster direction specified by user + .normalized(); } // Calculate all 8 corners projected onto the raster direction vector diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index 33726917..6b1b5d92 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -493,7 +493,7 @@ void testAndMark(tlist_it& item1, tlist_it& item2, double point_spacing) // Eigen::Vector3d v = wp1.translation() - wp2.translation(); // if (v.norm() < 0.75 * point_spacing) double dist = fabs(std::get<0>(*item1) - std::get<0>(*item2)); - if(dist < 0.75 * point_spacing) + if (dist < 0.75 * point_spacing) { if (mark1 == 1 && mark2 == 0) { @@ -564,20 +564,22 @@ ToolPath sortAndSegment(std::list >& // spacing computed using dot-distance is close to the point spacing AND // spacing computed using cartesian distance is close to point spacing // then add add to segment - if (dot_spacing > .7 * point_spacing && dot_spacing <= 1.3 * point_spacing && - cart_spacing > .7 * point_spacing && cart_spacing <= 1.3 * point_spacing || - (mark == 1 && dot_spacing >0.0 && cart_spacing< 1.3 * point_spacing)) + if (dot_spacing > .7 * point_spacing && dot_spacing <= 1.3 * point_spacing && cart_spacing > .7 * point_spacing && + cart_spacing <= 1.3 * point_spacing || + (mark == 1 && dot_spacing > 0.0 && cart_spacing < 1.3 * point_spacing)) { seg.push_back(waypoint); last_wp = waypoint; last_dot = std::get<0>(waypoint_tuple); } - else if (dot_spacing > 1.3 * point_spacing && cart_spacing > 1.3 * point_spacing) // only add extra if dot spacing is large - { // start a new segment - if(seg.size()>3) - { - new_tool_path.push_back(seg); // throw away very short segments - } + // only add extra if dot spacing is large + else if (dot_spacing > 1.3 * point_spacing && cart_spacing > 1.3 * point_spacing) + { + // start a new segment + if (seg.size() > 3) + { + new_tool_path.push_back(seg); // throw away very short segments + } seg.clear(); seg.push_back(waypoint); last_wp = waypoint; @@ -585,18 +587,18 @@ ToolPath sortAndSegment(std::list >& } else { // skip unless last in list, significantly spaced from last waypoint, but not too far - if (q == waypoint_list.size() - 1 && dot_spacing > .3 * point_spacing && cart_spacing < 1.3*point_spacing) + if (q == waypoint_list.size() - 1 && dot_spacing > .3 * point_spacing && cart_spacing < 1.3 * point_spacing) { - // seg.push_back(waypoint); // keep the last on regardless of distance to proces up to the defined edges + // seg.push_back(waypoint); // keep the last on regardless of distance to proces up to the defined edges last_dot = std::get<0>(waypoint_tuple); } } q++; } // end for every waypoint in waypoint_list - if(seg.size()>3) - { - new_tool_path.push_back(seg); - } + if (seg.size() > 3) + { + new_tool_path.push_back(seg); + } return (new_tool_path); } diff --git a/tool_path_planner/test/utest.cpp b/tool_path_planner/test/utest.cpp index b02f7ff0..58f16511 100644 --- a/tool_path_planner/test/utest.cpp +++ b/tool_path_planner/test/utest.cpp @@ -25,11 +25,11 @@ vtkSmartPointer loadTempMesh() { vtkSmartPointer polydata; - vtkSmartPointer reader = vtkSmartPointer::New (); - reader->SetFileName ("/tmp/test_mesh.ply"); - reader->Update (); - polydata = reader->GetOutput (); - printf("Loaded /tmp/test_mesh.ply with %ld points/vertices.\n", polydata->GetNumberOfPoints ()); + vtkSmartPointer reader = vtkSmartPointer::New(); + reader->SetFileName("/tmp/test_mesh.ply"); + reader->Update(); + polydata = reader->GetOutput(); + printf("Loaded /tmp/test_mesh.ply with %ld points/vertices.\n", polydata->GetNumberOfPoints()); return polydata; } @@ -269,7 +269,7 @@ void runTestCaseRansac(tool_path_planner::PathGenerator& planner, vtkSmartPointe void runExtraRasterTest(tool_path_planner::PathGenerator& planner, tool_path_planner::PathGenerator& planner_with_extras, vtkSmartPointer mesh, - double scale = 1.0) + double scale = 1.0) { // Set input mesh planner.setInput(mesh); @@ -409,7 +409,6 @@ void runExtraRasterTest(tool_path_planner::PathGenerator& planner, #endif } -/* TEST(IntersectTest, SurfaceWalkRasterRotationTest) { vtkSmartPointer mesh = createTestMesh1(); @@ -480,7 +479,7 @@ TEST(IntersectTest, PlaneSlicerRasterSpecificDirectionTest) tool.min_hole_size = 0.1; tool.raster_rot_offset = 0.0; tool.raster_wrt_global_axes = false; - tool.raster_direction = Eigen::Vector3d::UnitY(); + tool.raster_direction = Eigen::Vector3d::UnitX(); // tool.debug = false; planner.setConfiguration(tool); runRasterRotationTest(planner, mesh); @@ -628,7 +627,7 @@ TEST(IntersectTest, SurfaceWalkExtraRasterTest) runExtraRasterTest(planner, planner_with_extra, mesh); } -*/ + TEST(PlaneSlicerTest, PlaneSlicerExtraWaypointTest) { vtkSmartPointer mesh = createTestMesh1(); From d845ba56e48072cf973afc291789104c65362385 Mon Sep 17 00:00:00 2001 From: Stevie Dale Date: Wed, 12 May 2021 12:55:53 -0500 Subject: [PATCH 40/64] removed test node --- tool_path_planner/CMakeLists.txt | 9 - tool_path_planner/src/test_segment_axes.cpp | 271 -------------------- 2 files changed, 280 deletions(-) delete mode 100644 tool_path_planner/src/test_segment_axes.cpp diff --git a/tool_path_planner/CMakeLists.txt b/tool_path_planner/CMakeLists.txt index 7d153cfc..d901f3c9 100644 --- a/tool_path_planner/CMakeLists.txt +++ b/tool_path_planner/CMakeLists.txt @@ -83,19 +83,10 @@ add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ) -add_executable(test_segment_axes - src/test_segment_axes.cpp -) -target_link_libraries(test_segment_axes - ${catkin_LIBRARIES} - ${PROJECT_NAME} -) - ############# ## Install ## ############# install(TARGETS ${PROJECT_NAME} - test_segment_axes ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/tool_path_planner/src/test_segment_axes.cpp b/tool_path_planner/src/test_segment_axes.cpp deleted file mode 100644 index 80e24206..00000000 --- a/tool_path_planner/src/test_segment_axes.cpp +++ /dev/null @@ -1,271 +0,0 @@ -#include -#include -#include - -#include - -#include - -#include -#include -#include -#include -#include - -#include - -tool_path_planner::ToolPathSegment toSegment(std::vector points) -{ - tool_path_planner::ToolPathSegment tool_path_segment; - for (auto p : points) - { - Eigen::Isometry3d point; - point.translation().x() = p.x(); - point.translation().y() = p.y(); - point.translation().z() = p.z(); - tool_path_segment.push_back(point); - } - return tool_path_segment; -} - -std::vector applySinWaveToZ( - std::vector points_in, - double wave_length=0.4, - double amplitude=0.1, - Eigen::Vector3d direction = Eigen::Vector3d(1.0, 1.0, 0)) -{ - std::vector points_out; - for (auto p : points_in) - { - p.z() += amplitude * sin(2*M_PI*p.dot(direction) / wave_length); - points_out.push_back(p); - } - return points_out; -} - -std::vector applySinWaveByInc( - std::vector points_in, - double inc=M_PI/12, - double phase=0, - Eigen::Vector3d amplitude=Eigen::Vector3d(0.2, 0, 0)) -{ - std::vector points_out; - double t = phase; - for (auto p : points_in) - { - p.x() += amplitude.x() * sin(t); - p.y() += amplitude.y() * sin(t); - p.z() += amplitude.z() * sin(t); - t += inc; - points_out.push_back(p); - } - return points_out; -} - -std::vector projectZToPlane( - std::vector points_in, - Eigen::Vector3d plane_coeff) -{ - std::vector points_out; - for (auto p : points_in) - { - double x = plane_coeff.x(); - double y = plane_coeff.y(); - p.z() = -1*x*p.x() + -1*y*p.y(); - points_out.push_back(p); - } - return points_out; -} - -std::vector circle( - double radius = 1.0, - double angle_inc = DEG2RAD(5)) -{ - std::vector points; - - for (int i = 0; i < int(std::round(2 * M_PI / angle_inc)); ++i) - { - Eigen::Vector3d p; - double angle = i * angle_inc; - if (abs(angle - DEG2RAD(0.0)) < 0.001) - { - p.x() = radius; - p.y() = 0; - } - else if (abs(angle - DEG2RAD(90.0)) < 0.001) - { - p.x() = 0; - p.y() = radius; - } - else if (abs(angle - DEG2RAD(180.0)) < 0.001) - { - p.x() = -radius; - p.y() = 0; - } - else if (abs(angle - DEG2RAD(270.0)) < 0.001) - { - p.x() = 0; - p.y() = -radius; - } - else - { - double theta = angle; - int y_sign = 1; - int x_sign = 1; - if (DEG2RAD(90.0) < angle < DEG2RAD(180.0)) - { - theta = DEG2RAD(180.0) - angle; - x_sign = -1; - } - else if (DEG2RAD(180.0) < angle < DEG2RAD(270.0)) - { - theta = angle - DEG2RAD(180.0); - y_sign = -1; - x_sign = -1; - } - else if (DEG2RAD(270.0) < angle < DEG2RAD(360.0)) - { - theta = DEG2RAD(360.0) - angle; - y_sign = -1; - } - p.x() = x_sign * radius * cos(theta); - p.y() = y_sign * radius * sin(theta); - } - points.push_back(p); - } - return points; -} - -std::vector polygon( - std::vector points_in, - double inc = 0.05) -{ - std::vector points_out; - for (std::size_t point_index = 0; point_index < points_in.size(); ++point_index) - { - double x1 = points_in[point_index].x(); - double y1 = points_in[point_index].y(); - double z1 = points_in[point_index].z(); - double x2 = points_in[(point_index+1)%points_in.size()].x(); - double y2 = points_in[(point_index+1)%points_in.size()].y(); - double z2 = points_in[(point_index+1)%points_in.size()].z(); - double dist = std::sqrt(std::pow(x1 - x2, 2) + std::pow(y1 - y2, 2) + std::pow(z1 - z2, 2)); - int n = int(std::round(dist / inc)); - double x_inc = (x2 - x1)/n; - double y_inc = (y2 - y1)/n; - double z_inc = (z2 - z1)/n; - for (int i = 0; i < n; ++i) - { - Eigen::Vector3d point( - x1 + (i * x_inc), - y1 + (i * y_inc), - z1 + (i * z_inc) - ); - points_out.push_back(point); - } - } - return points_out; -} - -std::vector rectangle( - double length=1.0, - double width=2.0, - double inc=0.05) -{ - return polygon( - { - Eigen::Vector3d(-length/2, -width/2, 0), - Eigen::Vector3d(-length/2, width/2, 0), - Eigen::Vector3d( length/2, width/2, 0), - Eigen::Vector3d( length/2, -width/2, 0) - }, - inc - ); -} - -int main(int argc, char** argv) -{ - - ros::init(argc, argv, "test_segment_axes"); - ros::NodeHandle nh; - ros::Publisher pub = nh.advertise("segments", 10); - ros::Publisher eigen_pub_1 = nh.advertise("eigen_1", 10); - ros::Publisher eigen_pub_2 = nh.advertise("eigen_2", 10); - std::this_thread::sleep_for(std::chrono::seconds(1)); - - // create tool path - tool_path_planner::ToolPaths tool_paths; - tool_path_planner::ToolPath tool_path; - - std::vector points; -// points = polygon( -// { -// Eigen::Vector3d(-1.0 , -0.8 , 0), -// Eigen::Vector3d(-0.2 , 0 , 0), -// Eigen::Vector3d(-1.0 , 0.5 , 0), -// Eigen::Vector3d(-0.5 , 0.5 , 0), -// Eigen::Vector3d(-0.6 , 1.0 , 0), -// Eigen::Vector3d( 0 , 0.5 , 0), -// Eigen::Vector3d( 1 , 0.5 , 0), -// Eigen::Vector3d( 0.5 , 0 , 0), -// Eigen::Vector3d( 0 , -1 , 0) -// }, -// 0.001 -// ); -// points = rectangle(1, 2, 0.01); - points = circle(1.0, 0.01); - -// points = projectZToPlane(points, Eigen::Vector3d(1.0, 1.0, 0)); -// points = applySinWaveByInc(points, M_PI/120, 0, Eigen::Vector3d(0.1, 0, 0)); -// points = applySinWaveByInc(points, M_PI/120, M_PI/2, Eigen::Vector3d(0, 0.0, 0.1)); -// points = applySinWaveToZ(points, 0.1, 0.1, Eigen::Vector3d(0.1, 0, 0)); - - tool_path.push_back(toSegment(points)); - - tool_paths.push_back(tool_path); - - tool_paths = tool_path_planner::splitByAxes(tool_paths, Eigen::Vector3f(1.0, 0, 0), Eigen::Vector3f(0.0, 1.0, 0.0)); - - std::size_t n = tool_paths[0].size(); - std::vector> colors = { - {1.0, 0.0, 0.0}, - {0.0, 1.0, 0.0}, - {0.0, 0.0, 1.0}, - {1.0, 1.0, 0.0} - }; - - int marker_id = 2; - visualization_msgs::MarkerArray marker_array; - for (std::size_t i = 0; i < n; ++i) - { - for (Eigen::Isometry3d pose : tool_paths[0][i]) - { - visualization_msgs::Marker marker; - marker.header.frame_id = "world"; - marker.header.stamp = ros::Time::now(); - marker.ns = ""; - marker.id = marker_id; - marker.color.a = 1.0; - marker.color.r = colors[i][0]; - marker.color.g = colors[i][1]; - marker.color.b = colors[i][2]; - marker.pose = tf2::toMsg(pose); - marker.type = visualization_msgs::Marker::SPHERE; - marker.scale.x = 0.04; - marker.scale.y = 0.04; - marker.scale.z = 0.04; - marker_array.markers.push_back(marker); -// pub.publish(marker_array); -// std::this_thread::sleep_for(std::chrono::milliseconds(500)); - ++marker_id; - } - } - - pub.publish(marker_array); - - ROS_ERROR_STREAM("DONE."); - - ros::spin(); - - return 0; -} From e9671a0bdb2c967af4b19fa080a01c2638f8c605 Mon Sep 17 00:00:00 2001 From: Stevie Dale Date: Wed, 12 May 2021 12:57:22 -0500 Subject: [PATCH 41/64] fixed fail cases --- tool_path_planner/src/utilities.cpp | 50 ++++++++++++++++------------- 1 file changed, 28 insertions(+), 22 deletions(-) diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index 44553ec0..2d0e69ca 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -505,22 +505,23 @@ ToolPaths addExtraPaths(const ToolPaths& tool_paths, double offset_distance) ToolPath splitByAxes(const ToolPathSegment& tool_path_segment) { - using Point = pcl::PointXYZ; // Sanity check - tool path segment must not be empty if (tool_path_segment.size() == 0) { - ROS_ERROR_STREAM("Tool path segment 0 is empty."); - // TODO: throw an exception + ROS_WARN_STREAM("Tool path segment 0 is empty."); + ToolPath tp; + tp.push_back(tool_path_segment); + return tp; } // Get major and middle axis (we don't care about minor axis) - pcl::PointCloud::Ptr cloud = boost::make_shared>(); + pcl::PointCloud::Ptr cloud = boost::make_shared>(); for (Eigen::Isometry3d p: tool_path_segment) { - Point point(float(p.translation().x()), float(p.translation().y()), float(p.translation().z())); + pcl::PointXYZ point(float(p.translation().x()), float(p.translation().y()), float(p.translation().z())); cloud->push_back(point); } - pcl::MomentOfInertiaEstimation moment; + pcl::MomentOfInertiaEstimation moment; moment.setInputCloud(cloud); moment.compute(); Eigen::Vector3f major_axis, middle_axis, minor_axis; @@ -542,8 +543,10 @@ ToolPath splitByAxes(const ToolPathSegment& tool_path_segment, const Eigen::Vect // Sanity check - tool path segment must not be empty if (tool_path_segment.size() == 0) { - ROS_ERROR_STREAM("Tool path segment 0 is empty."); - // TODO: throw an exception + ROS_WARN_STREAM("Tool path segment 0 is empty."); + ToolPath tp; + tp.push_back(tool_path_segment); + return tp; } ToolPath new_tool_path; @@ -612,11 +615,10 @@ ToolPaths splitByAxes(const ToolPaths& tool_paths) { if (tool_paths.size() == 0) { - ROS_ERROR_STREAM("Tool paths is empty."); - // TODO: throw an exception + ROS_WARN_STREAM("Tool paths is empty."); + return tool_paths; } - using Point = pcl::PointXYZ; ToolPaths new_tool_paths; for (std::size_t tool_path_index = 0; tool_path_index < tool_paths.size(); ++tool_path_index) @@ -625,12 +627,15 @@ ToolPaths splitByAxes(const ToolPaths& tool_paths) // Sanity check - tool path must have exactly one segment if (tool_path.size() != 1) { - ROS_ERROR_STREAM("Tool path " << tool_path_index << " contains " << tool_path.size() << + ROS_WARN_STREAM("Tool path " << tool_path_index << " contains " << tool_path.size() << " segments. Expected exactly 1."); - // TODO: throw an exception + new_tool_paths.push_back(tool_path); + } + else + { + ToolPath new_tool_path = splitByAxes(tool_path[0]); + new_tool_paths.push_back(new_tool_path); } - ToolPath new_tool_path = splitByAxes(tool_path[0]); - new_tool_paths.push_back(new_tool_path); } return new_tool_paths; } @@ -639,11 +644,10 @@ ToolPaths splitByAxes(const ToolPaths& tool_paths, const Eigen::Vector3f& axis_1 { if (tool_paths.size() == 0) { - ROS_ERROR_STREAM("Tool paths is empty."); - // TODO: throw an exception + ROS_WARN_STREAM("Tool paths is empty."); + return tool_paths; } - using Point = pcl::PointXYZ; ToolPaths new_tool_paths; for (std::size_t tool_path_index = 0; tool_path_index < tool_paths.size(); ++tool_path_index) @@ -652,12 +656,14 @@ ToolPaths splitByAxes(const ToolPaths& tool_paths, const Eigen::Vector3f& axis_1 // Sanity check - tool path must have exactly one segment if (tool_path.size() != 1) { - ROS_ERROR_STREAM("Tool path " << tool_path_index << " contains " << tool_path.size() << + ROS_WARN_STREAM("Tool path " << tool_path_index << " contains " << tool_path.size() << " segments. Expected exactly 1."); - // TODO: throw an exception + new_tool_paths.push_back(tool_path); + } + else { + ToolPath new_tool_path = splitByAxes(tool_path[0], axis_1, axis_2); + new_tool_paths.push_back(new_tool_path); } - ToolPath new_tool_path = splitByAxes(tool_path[0], axis_1, axis_2); - new_tool_paths.push_back(new_tool_path); } return new_tool_paths; } From 3688fa4a49547735f036e96257f01b76ccd200c2 Mon Sep 17 00:00:00 2001 From: Stevie Dale Date: Wed, 12 May 2021 13:31:05 -0500 Subject: [PATCH 42/64] removed unused dependency from CMakeLists.txt --- tool_path_planner/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/tool_path_planner/CMakeLists.txt b/tool_path_planner/CMakeLists.txt index d901f3c9..3d2e7d25 100644 --- a/tool_path_planner/CMakeLists.txt +++ b/tool_path_planner/CMakeLists.txt @@ -13,7 +13,6 @@ find_package(catkin REQUIRED COMPONENTS geometry_msgs noether_msgs rosconsole - roscpp roslib shape_msgs ) From 78b6fc2e7144d2a5aa59c1e863c51ee623afa0b9 Mon Sep 17 00:00:00 2001 From: Stevie Dale Date: Wed, 12 May 2021 13:47:11 -0500 Subject: [PATCH 43/64] support for splitting ToolPath objects with multiple ToolPathSegment objects --- tool_path_planner/src/utilities.cpp | 33 ++++++++++++----------------- 1 file changed, 14 insertions(+), 19 deletions(-) diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index 2d0e69ca..df4c132d 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -624,18 +624,15 @@ ToolPaths splitByAxes(const ToolPaths& tool_paths) for (std::size_t tool_path_index = 0; tool_path_index < tool_paths.size(); ++tool_path_index) { ToolPath tool_path = tool_paths[tool_path_index]; - // Sanity check - tool path must have exactly one segment - if (tool_path.size() != 1) - { - ROS_WARN_STREAM("Tool path " << tool_path_index << " contains " << tool_path.size() << - " segments. Expected exactly 1."); - new_tool_paths.push_back(tool_path); - } - else + ToolPath new_tool_path; + for (ToolPathSegment tool_path_segment : tool_path) { - ToolPath new_tool_path = splitByAxes(tool_path[0]); - new_tool_paths.push_back(new_tool_path); + // Split segement into multiple sub-segments + ToolPath tool_path_to_merge = splitByAxes(tool_path_segment); + // Merge sub-segments with the other sub-segments (for this tool path) + new_tool_path.insert(new_tool_path.end(), tool_path_to_merge.begin(), tool_path_to_merge.end()); } + new_tool_paths.push_back(new_tool_path); } return new_tool_paths; } @@ -653,17 +650,15 @@ ToolPaths splitByAxes(const ToolPaths& tool_paths, const Eigen::Vector3f& axis_1 for (std::size_t tool_path_index = 0; tool_path_index < tool_paths.size(); ++tool_path_index) { ToolPath tool_path = tool_paths[tool_path_index]; - // Sanity check - tool path must have exactly one segment - if (tool_path.size() != 1) + ToolPath new_tool_path; + for (ToolPathSegment tool_path_segment : tool_path) { - ROS_WARN_STREAM("Tool path " << tool_path_index << " contains " << tool_path.size() << - " segments. Expected exactly 1."); - new_tool_paths.push_back(tool_path); - } - else { - ToolPath new_tool_path = splitByAxes(tool_path[0], axis_1, axis_2); - new_tool_paths.push_back(new_tool_path); + // Split segement into multiple sub-segments + ToolPath tool_path_to_merge = splitByAxes(tool_path_segment, axis_1, axis_2); + // Merge sub-segments with the other sub-segments (for this tool path) + new_tool_path.insert(new_tool_path.end(), tool_path_to_merge.begin(), tool_path_to_merge.end()); } + new_tool_paths.push_back(new_tool_path); } return new_tool_paths; } From ef6fa61e5482c9e0c4af6b50206c1272a5707a17 Mon Sep 17 00:00:00 2001 From: "David Merz, Jr" Date: Mon, 25 Oct 2021 12:30:15 -0500 Subject: [PATCH 44/64] Trying to Correct Malformed Post-Filtering Meshes --- noether_filtering/CMakeLists.txt | 1 + .../include/noether_filtering/utils.h | 18 ++- noether_filtering/src/mesh/clean_data.cpp | 2 +- noether_filtering/src/mesh/fill_holes.cpp | 2 +- .../src/mesh/windowed_sinc_smoothing.cpp | 2 +- noether_filtering/src/utils.cpp | 109 ++++++++++++++++++ tool_path_planner/src/utilities.cpp | 1 + 7 files changed, 130 insertions(+), 5 deletions(-) create mode 100644 noether_filtering/src/utils.cpp diff --git a/noether_filtering/CMakeLists.txt b/noether_filtering/CMakeLists.txt index a5e02f76..cfd33660 100644 --- a/noether_filtering/CMakeLists.txt +++ b/noether_filtering/CMakeLists.txt @@ -46,6 +46,7 @@ list(FIND CMAKE_CXX_COMPILE_FEATURES cxx_std_14 CXX_FEATURE_FOUND) add_library(${PROJECT_NAME} SHARED src/filter_group.cpp src/filter_manager.cpp + src/utils.cpp ) target_link_libraries(${PROJECT_NAME} PUBLIC ${Boost_LIBRARIES} diff --git a/noether_filtering/include/noether_filtering/utils.h b/noether_filtering/include/noether_filtering/utils.h index 2e7959a4..ad7c5303 100644 --- a/noether_filtering/include/noether_filtering/utils.h +++ b/noether_filtering/include/noether_filtering/utils.h @@ -8,10 +8,14 @@ #ifndef INCLUDE_NOETHER_FILTERING_UTILS_H_ #define INCLUDE_NOETHER_FILTERING_UTILS_H_ -#include +#include #include #include -#include +#include + +#include +#include +#include namespace noether_filtering { @@ -28,6 +32,16 @@ static std::string getClassName() return (status == 0) ? res.get() : mangled_name; } +/** + * @brief vtk2TriangleMesh - pcl's vtk2mesh function can produce a PolygonMesh containing 1-point + * or 2-point 'polygons', as well as squares and so on. Many applications assume a mesh containing + * only triangles. This function converts the output to only include triangles. + * @param poly_data - input - the VTK mesh + * @param mesh - output - the resulting pcl polygonmesh containing only triangles + * @return - number of points contained in pcl::PolygonMesh, as vtk2mesh does. + */ +int vtk2TriangleMesh(const vtkSmartPointer& poly_data, pcl::PolygonMesh& mesh); + } // namespace utils } // namespace noether_filtering diff --git a/noether_filtering/src/mesh/clean_data.cpp b/noether_filtering/src/mesh/clean_data.cpp index a28f5a81..6d64829b 100644 --- a/noether_filtering/src/mesh/clean_data.cpp +++ b/noether_filtering/src/mesh/clean_data.cpp @@ -45,7 +45,7 @@ bool CleanData::filter(const pcl::PolygonMesh& mesh_in, pcl::PolygonMesh& mesh_o mesh_data = cleanPolyData->GetOutput(); std::size_t num_end_points = mesh_data->GetNumberOfPoints(); CONSOLE_BRIDGE_logInform("Removed duplicate points, retained %lu points from %lu", num_end_points, num_start_points); - VTKUtils::vtk2mesh(mesh_data, mesh_out); + utils::vtk2TriangleMesh(mesh_data, mesh_out); return true; } diff --git a/noether_filtering/src/mesh/fill_holes.cpp b/noether_filtering/src/mesh/fill_holes.cpp index e22fee26..63ea6924 100644 --- a/noether_filtering/src/mesh/fill_holes.cpp +++ b/noether_filtering/src/mesh/fill_holes.cpp @@ -88,7 +88,7 @@ bool FillHoles::filter(const pcl::PolygonMesh& mesh_in, pcl::PolygonMesh& mesh_o mesh_data = normals_rectifier->GetOutput(); std::size_t end_num_polys = mesh_data->GetNumberOfPolys(); CONSOLE_BRIDGE_logInform("Filled %lu polygons", start_num_polys - end_num_polys); - VTKUtils::vtk2mesh(mesh_data, mesh_out); + utils::vtk2TriangleMesh(mesh_data, mesh_out); return true; } diff --git a/noether_filtering/src/mesh/windowed_sinc_smoothing.cpp b/noether_filtering/src/mesh/windowed_sinc_smoothing.cpp index f7a43704..fbee4c61 100644 --- a/noether_filtering/src/mesh/windowed_sinc_smoothing.cpp +++ b/noether_filtering/src/mesh/windowed_sinc_smoothing.cpp @@ -91,7 +91,7 @@ bool WindowedSincSmoothing::filter(const pcl::PolygonMesh& mesh_in, pcl::Polygon smoother->Update(); mesh_data = smoother->GetOutput(); - VTKUtils::vtk2mesh(mesh_data, mesh_out); + utils::vtk2TriangleMesh(mesh_data, mesh_out); return true; } diff --git a/noether_filtering/src/utils.cpp b/noether_filtering/src/utils.cpp new file mode 100644 index 00000000..e6dc4962 --- /dev/null +++ b/noether_filtering/src/utils.cpp @@ -0,0 +1,109 @@ +#include + +#include +#include +#include +#include +#include + +namespace noether_filtering +{ +namespace utils +{ + +int vtk2TriangleMesh(const vtkSmartPointer& poly_data, pcl::PolygonMesh& mesh) +{ + mesh.polygons.clear (); + mesh.cloud.data.clear (); + mesh.cloud.width = mesh.cloud.height = 0; + mesh.cloud.is_dense = true; + + vtkSmartPointer mesh_points = poly_data->GetPoints (); + vtkIdType nr_points = mesh_points->GetNumberOfPoints (); + vtkIdType nr_polygons = poly_data->GetNumberOfPolys (); + + if (nr_points == 0) + return 0; + + vtkUnsignedCharArray* poly_colors = nullptr; + if (poly_data->GetPointData() != nullptr) + poly_colors = vtkUnsignedCharArray::SafeDownCast (poly_data->GetPointData ()->GetScalars ("Colors")); + + // Some applications do not save the name of scalars (including PCL's native vtk_io) + if (!poly_colors) + poly_colors = vtkUnsignedCharArray::SafeDownCast (poly_data->GetPointData ()->GetScalars ("scalars")); + + // TODO: currently only handles rgb values with 3 components + if (poly_colors && (poly_colors->GetNumberOfComponents () == 3)) + { + pcl::PointCloud::Ptr cloud_temp (new pcl::PointCloud()); + cloud_temp->points.resize (nr_points); + double point_xyz[3]; + unsigned char point_color[3]; + for (vtkIdType i = 0; i < mesh_points->GetNumberOfPoints (); ++i) + { + mesh_points->GetPoint (i, &point_xyz[0]); + (*cloud_temp)[i].x = static_cast (point_xyz[0]); + (*cloud_temp)[i].y = static_cast (point_xyz[1]); + (*cloud_temp)[i].z = static_cast (point_xyz[2]); + + poly_colors->GetTupleValue (i, &point_color[0]); + (*cloud_temp)[i].r = point_color[0]; + (*cloud_temp)[i].g = point_color[1]; + (*cloud_temp)[i].b = point_color[2]; + } + cloud_temp->width = cloud_temp->size (); + cloud_temp->height = 1; + cloud_temp->is_dense = true; + + pcl::toPCLPointCloud2 (*cloud_temp, mesh.cloud); + } + else // in case points do not have color information: + { + pcl::PointCloud::Ptr cloud_temp (new pcl::PointCloud ()); + cloud_temp->points.resize (nr_points); + double point_xyz[3]; + for (vtkIdType i = 0; i < mesh_points->GetNumberOfPoints (); ++i) + { + mesh_points->GetPoint (i, &point_xyz[0]); + (*cloud_temp)[i].x = static_cast (point_xyz[0]); + (*cloud_temp)[i].y = static_cast (point_xyz[1]); + (*cloud_temp)[i].z = static_cast (point_xyz[2]); + } + cloud_temp->width = cloud_temp->size (); + cloud_temp->height = 1; + cloud_temp->is_dense = true; + + pcl::toPCLPointCloud2 (*cloud_temp, mesh.cloud); + } + + mesh.polygons.reserve (nr_polygons); +#ifdef VTK_CELL_ARRAY_V2 + vtkIdType const *cell_points; +#else + vtkIdType* cell_points; +#endif + vtkIdType nr_cell_points; + vtkCellArray * mesh_polygons = poly_data->GetPolys (); + mesh_polygons->InitTraversal (); + int id_poly = 0; + while (mesh_polygons->GetNextCell (nr_cell_points, cell_points)) + { + if (nr_cell_points == 3 && + cell_points[0] != cell_points[1] && + cell_points[1] != cell_points[2] && + cell_points[2] != cell_points[0]) + { + mesh.polygons.emplace_back(pcl::Vertices()); + mesh.polygons[id_poly].vertices.resize (nr_cell_points); + for (vtkIdType i = 0; i < nr_cell_points; ++i) + mesh.polygons[id_poly].vertices[i] = static_cast (cell_points[i]); + ++id_poly; + } + } + + return (static_cast (nr_points)); +} + +} // namespace utils +} // namespace noether_filtering diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index df4c132d..f68e768c 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -12,6 +12,7 @@ #include #include // std::sort +#include #include #include #include From 49837e1e1d61812356b34e9ba233776a7cdce767 Mon Sep 17 00:00:00 2001 From: Chris Lewis Date: Thu, 27 May 2021 13:47:33 -0500 Subject: [PATCH 45/64] reinstated a heat_raster interface and made a few minor simplifying changes and comments --- noether/src/tool_path_planner_server.cpp | 3 +-- noether_msgs/CMakeLists.txt | 1 + noether_msgs/msg/HeatRasterGeneratorConfig.msg | 7 +++++++ noether_msgs/msg/ToolPathConfig.msg | 2 +- .../src/plane_slicer_raster_generator.cpp | 18 +++++++++++------- 5 files changed, 21 insertions(+), 10 deletions(-) create mode 100644 noether_msgs/msg/HeatRasterGeneratorConfig.msg diff --git a/noether/src/tool_path_planner_server.cpp b/noether/src/tool_path_planner_server.cpp index e7d6229f..a50a7e59 100644 --- a/noether/src/tool_path_planner_server.cpp +++ b/noether/src/tool_path_planner_server.cpp @@ -144,7 +144,6 @@ class TppServer if (tool_paths.is_initialized()) { - noether_msgs::ToolPaths trp; for (const auto& tool_path : tool_paths.get()) { noether_msgs::ToolPath tp; @@ -159,7 +158,7 @@ class TppServer } tp.segments.push_back(seg); } - trp.paths.push_back(tp); + result.tool_paths[i].paths.push_back(tp); } result.tool_path_validities[i] = true; diff --git a/noether_msgs/CMakeLists.txt b/noether_msgs/CMakeLists.txt index dcf35486..33c8ec7d 100644 --- a/noether_msgs/CMakeLists.txt +++ b/noether_msgs/CMakeLists.txt @@ -23,6 +23,7 @@ add_message_files( SurfaceWalkRasterGeneratorConfig.msg EigenValueEdgeGeneratorConfig.msg HalfedgeEdgeGeneratorConfig.msg + HeatRasterGeneratorConfig.msg SegmentationConfig.msg ToolPathConfig.msg ToolPath.msg diff --git a/noether_msgs/msg/HeatRasterGeneratorConfig.msg b/noether_msgs/msg/HeatRasterGeneratorConfig.msg new file mode 100644 index 00000000..167b9d5a --- /dev/null +++ b/noether_msgs/msg/HeatRasterGeneratorConfig.msg @@ -0,0 +1,7 @@ +float64 point_spacing # Required spacing between path points +float64 raster_spacing # Offset between two rasters +float64 tool_offset # How far off the surface the tool needs to be +float64 min_hole_size # A path passes over smaller holes breaks into two on larger wholes +float64 min_segment_size # The minimum segment size to allowed +float64 raster_rot_offset # use this angle when no sources are povided +bool generate_extra_rasters # Add extra strokes off the boundaries (NOT YET IMPLEMENTED BY HEAT METHOD) \ No newline at end of file diff --git a/noether_msgs/msg/ToolPathConfig.msg b/noether_msgs/msg/ToolPathConfig.msg index a5723625..57fdef89 100644 --- a/noether_msgs/msg/ToolPathConfig.msg +++ b/noether_msgs/msg/ToolPathConfig.msg @@ -7,6 +7,6 @@ uint8 HALFEDGE_EDGE_GENERATOR=4 uint8 type noether_msgs/SurfaceWalkRasterGeneratorConfig surface_walk_generator noether_msgs/PlaneSlicerRasterGeneratorConfig plane_slicer_generator -#noether_msgs/HeatRasterPathGeneratorConfig heat_generator +noether_msgs/HeatRasterGeneratorConfig heat_generator noether_msgs/EigenValueEdgeGeneratorConfig eigen_value_generator noether_msgs/HalfedgeEdgeGeneratorConfig halfedge_generator diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index 1f640f03..c4bb7117 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -401,12 +401,12 @@ static tool_path_planner::ToolPaths convertToPoses(const std::vector raster_segments; raster_segments.assign(rd.raster_segments.begin(), rd.raster_segments.end()); - for (const PolyDataPtr& polydata : raster_segments) + for (const PolyDataPtr& polydata : raster_segments) // for every segment { tool_path_planner::ToolPathSegment raster_path_segment; std::size_t num_points = polydata->GetNumberOfPoints(); @@ -414,7 +414,11 @@ static tool_path_planner::ToolPaths convertToPoses(const std::vector indices(num_points); std::iota(indices.begin(), indices.end(), 0); - for (std::size_t i = 0; i < indices.size() - 1; i++) + // for every waypoint MAKE A POSE such that + // its normal uses the mesh normal + // its x axis points toward next waypoint + // its y is z cross x + for (std::size_t i = 0; i < indices.size() - 1; i++) { int idx = indices[i]; int idx_next = indices[i + 1]; @@ -426,16 +430,16 @@ static tool_path_planner::ToolPaths convertToPoses(const std::vector PlaneSlicerRasterGenerator::generate() rasters = addExtraWaypoints(rasters, config_.raster_spacing, config_.point_spacing); } - // switch directions of every other raster + // switch directions of every other raster without rotating rasters = reverseOddRasters(rasters, config_.raster_style); return rasters; From 8cc8584352738e371060342e1e9cce85235232c1 Mon Sep 17 00:00:00 2001 From: Chris Lewis Date: Thu, 27 May 2021 15:02:16 -0500 Subject: [PATCH 46/64] clang tidy --- noether/src/tool_path_planner_server.cpp | 2 +- .../src/plane_slicer_raster_generator.cpp | 12 ++++++------ tool_path_planner/src/utilities.cpp | 7 +++---- 3 files changed, 10 insertions(+), 11 deletions(-) diff --git a/noether/src/tool_path_planner_server.cpp b/noether/src/tool_path_planner_server.cpp index a50a7e59..b0b077bd 100644 --- a/noether/src/tool_path_planner_server.cpp +++ b/noether/src/tool_path_planner_server.cpp @@ -158,7 +158,7 @@ class TppServer } tp.segments.push_back(seg); } - result.tool_paths[i].paths.push_back(tp); + result.tool_paths[i].paths.push_back(tp); } result.tool_path_validities[i] = true; diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index c4bb7117..b6c254b3 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -401,12 +401,12 @@ static tool_path_planner::ToolPaths convertToPoses(const std::vector raster_segments; raster_segments.assign(rd.raster_segments.begin(), rd.raster_segments.end()); - for (const PolyDataPtr& polydata : raster_segments) // for every segment + for (const PolyDataPtr& polydata : raster_segments) // for every segment { tool_path_planner::ToolPathSegment raster_path_segment; std::size_t num_points = polydata->GetNumberOfPoints(); @@ -418,7 +418,7 @@ static tool_path_planner::ToolPaths convertToPoses(const std::vector >& last_wp = waypoint; last_dot = std::get<0>(waypoint_tuple); } - // only add extra if dot spacing is large - else if (dot_spacing > 1.3 * point_spacing && cart_spacing > 1.3 * point_spacing) - { - // start a new segment + else if (dot_spacing > 1.3 * point_spacing && cart_spacing > 1.3 * point_spacing) // only add extra if dot spacing + // is large + { // start a new segment if (seg.size() > 3) { new_tool_path.push_back(seg); // throw away very short segments From b79f5394292cd0588697f7034c77a3cb9aa80604 Mon Sep 17 00:00:00 2001 From: Chris Lewis Date: Mon, 28 Jun 2021 08:39:22 -0500 Subject: [PATCH 47/64] added call to smoother and a utility function to generate interleaved paths for sanding --- noether/CMakeLists.txt | 2 ++ noether/package.xml | 2 +- noether/src/tool_path_planner_server.cpp | 33 +++++++++++++++++-- .../include/tool_path_planner/utilities.h | 10 ++++++ .../src/plane_slicer_raster_generator.cpp | 3 +- tool_path_planner/src/utilities.cpp | 32 ++++++++++++++++++ 6 files changed, 77 insertions(+), 5 deletions(-) diff --git a/noether/CMakeLists.txt b/noether/CMakeLists.txt index 6c75a629..e910315d 100644 --- a/noether/CMakeLists.txt +++ b/noether/CMakeLists.txt @@ -31,6 +31,7 @@ find_package(catkin REQUIRED COMPONENTS pcl_conversions roscpp tool_path_planner + smooth_pose_traj ) find_package(vtk_viewer REQUIRED) @@ -52,6 +53,7 @@ catkin_package( pcl_conversions roscpp tool_path_planner + smooth_pose_traj DEPENDS VTK PCL diff --git a/noether/package.xml b/noether/package.xml index 127ee008..21200fad 100644 --- a/noether/package.xml +++ b/noether/package.xml @@ -20,5 +20,5 @@ actionlib noether_conversions noether_filtering - + smooth_pose_traj diff --git a/noether/src/tool_path_planner_server.cpp b/noether/src/tool_path_planner_server.cpp index b0b077bd..181f1bb6 100644 --- a/noether/src/tool_path_planner_server.cpp +++ b/noether/src/tool_path_planner_server.cpp @@ -11,6 +11,8 @@ #include #include #include +#include +#include static const double FEEDBACK_PUBLISH_PERIOD = 1.0; // seconds static const std::string GENERATE_TOOL_PATHS_ACTION = "generate_tool_paths"; @@ -22,7 +24,7 @@ using GenPathActionServer = actionlib::SimpleActionServerpath_configs[i]; const shape_msgs::Mesh& mesh = goal->surface_meshes[i]; boost::optional tool_paths = boost::none; + double pt_spacing, raster_spacing; ROS_INFO("Planning path"); if (config.type == noether_msgs::ToolPathConfig::SURFACE_WALK_RASTER_GENERATOR) { @@ -105,6 +108,8 @@ class TppServer tool_path_planner::toSurfaceWalkConfig(path_config, config.surface_walk_generator); path_gen->setConfiguration(path_config); generator = path_gen; + pt_spacing = path_config.point_spacing; + raster_spacing = path_config.raster_spacing; } else if (config.type == noether_msgs::ToolPathConfig::PLANE_SLICER_RASTER_GENERATOR) { @@ -113,6 +118,8 @@ class TppServer tool_path_planner::toPlaneSlicerConfig(path_config, config.plane_slicer_generator); path_gen->setConfiguration(path_config); generator = path_gen; + pt_spacing = path_config.point_spacing; + raster_spacing = path_config.raster_spacing; } else if (config.type == noether_msgs::ToolPathConfig::EIGEN_VALUE_EDGE_GENERATOR) { @@ -121,6 +128,7 @@ class TppServer tool_path_planner::toEigenValueConfig(path_config, config.eigen_value_generator); path_gen->setConfiguration(path_config); generator = path_gen; + pt_spacing = path_config.merge_dist; } else if (config.type == noether_msgs::ToolPathConfig::HALFEDGE_EDGE_GENERATOR) { @@ -129,6 +137,7 @@ class TppServer tool_path_planner::toHalfedgeConfig(path_config, config.halfedge_generator); path_gen->setConfiguration(path_config); generator = path_gen; + pt_spacing = path_config.point_dist; } else { @@ -141,7 +150,7 @@ class TppServer generator->setInput(mesh); tool_paths = generator->generate(); - + if (tool_paths.is_initialized()) { for (const auto& tool_path : tool_paths.get()) @@ -156,11 +165,27 @@ class TppServer tf::poseEigenToMsg(p, pose); seg.poses.push_back(pose); } + if(smooth_pose_arrays_) + { + smooth_pose_traj::SmoothPoseTraj smoother(seg, pt_spacing, true); + smoother.process(seg); + } tp.segments.push_back(seg); } + + result.tool_paths[i].paths.push_back(tp); } + if(interleave_pose_arrays_) + { + for(size_t i=0; i #include +#include +#include + namespace tool_path_planner { /** @@ -150,6 +153,13 @@ ToolPath splitByAxes(const ToolPathSegment& tool_path_segment, const Eigen::Vect ToolPaths splitByAxes(const ToolPaths& tool_paths); ToolPaths splitByAxes(const ToolPaths& tool_paths, const Eigen::Vector3f& axis_1, const Eigen::Vector3f& axis_2); +/** + * @brief Appends an interleaved set tool_paths to the provided tool_paths + * @param tool_paths The input trajectory to interleave + * @param raster_spacing + */ +void InterleavePoseTraj(noether_msgs::ToolPaths& tool_paths, double raster_spacing); + } // 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 b6c254b3..61f8826f 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -810,7 +810,8 @@ boost::optional PlaneSlicerRasterGenerator::generate() if (config_.generate_extra_rasters) { - rasters = addExtraWaypoints(rasters, config_.raster_spacing, config_.point_spacing); + // ROS_ERROR("it did it anyway"); + // rasters = addExtraWaypoints(rasters, config_.raster_spacing, config_.point_spacing); } // switch directions of every other raster without rotating diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index db936093..31456d15 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -896,4 +896,36 @@ ToolPaths addExtraWaypoints(const ToolPaths& tool_paths, double raster_spacing, return new_tool_paths; } +void InterleavePoseTraj(noether_msgs::ToolPaths& tool_paths, double raster_spacing) +{ + double half_raster = raster_spacing/2.0; + printf("half raster = %lf\n", half_raster); + size_t total = tool_paths.paths.size()-1; + for (size_t i=0; i Date: Tue, 29 Jun 2021 14:58:28 -0500 Subject: [PATCH 48/64] added search_radius to service so that we average the normals over some distance rather than using just one --- noether/src/tool_path_planner_server.cpp | 3 +-- noether_msgs/msg/PlaneSlicerRasterGeneratorConfig.msg | 1 + tool_path_planner/src/utilities.cpp | 6 +++--- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/noether/src/tool_path_planner_server.cpp b/noether/src/tool_path_planner_server.cpp index 181f1bb6..7147cf86 100644 --- a/noether/src/tool_path_planner_server.cpp +++ b/noether/src/tool_path_planner_server.cpp @@ -24,7 +24,7 @@ using GenPathActionServer = actionlib::SimpleActionServer Date: Fri, 9 Jul 2021 14:46:41 -0500 Subject: [PATCH 49/64] fixed quite a few bugs in the plane slicer. These caused crashes. This is still a WIP. --- noether/src/tool_path_planner_server.cpp | 2 +- .../src/plane_slicer_raster_generator.cpp | 110 ++++++++++++++---- 2 files changed, 87 insertions(+), 25 deletions(-) diff --git a/noether/src/tool_path_planner_server.cpp b/noether/src/tool_path_planner_server.cpp index 7147cf86..9973280b 100644 --- a/noether/src/tool_path_planner_server.cpp +++ b/noether/src/tool_path_planner_server.cpp @@ -24,7 +24,7 @@ using GenPathActionServer = actionlib::SimpleActionServerGetPoints()->GetNumberOfPoints() < 1) { ROS_ERROR("can't get direction from a segment with fewer than 2 points"); + Eigen::Vector3d v; + v.x() = 1.0; + v.y() = 0.0; + v.z() = 0.0; + return(v); } Eigen::Vector3d seg_start, seg_end; seg->GetPoint(0, seg_start.data()); @@ -88,7 +93,6 @@ static RasterConstructData alignRasterCD(RasterConstructData& rcd, Eigen::Vector ROS_ERROR("first raster segment has 0 or 1 points, unable to alignRasterCD()"); return (rcd); } - Eigen::Vector3d raster_start; rcd.raster_segments[0]->GetPoint(0, raster_start.data()); @@ -120,7 +124,10 @@ static RasterConstructData alignRasterCD(RasterConstructData& rcd, Eigen::Vector seg_order.push_back(p); } // sort the segments by location - seg_order.sort(compare_ds_pair); + if(seg_order.size()>=1) + { + seg_order.sort(compare_ds_pair); + } RasterConstructData new_rcd; for (std::pair p : seg_order) { @@ -389,7 +396,7 @@ static void rectifyDirection(const vtkSmartPointer& points, bool reverse = (ref_point - p0).norm() > (ref_point - pf).norm(); if (reverse) { - for (auto& s : points_lists) + for (auto& s : points_lists) // reverse points in segments { std::reverse(s.begin(), s.end()); } @@ -397,6 +404,23 @@ static void rectifyDirection(const vtkSmartPointer& points, } } +static void computePoseData(const PolyDataPtr& polydata, int idx, Eigen::Vector3d& p, Eigen::Vector3d& vx, Eigen::Vector3d& vy, Eigen::Vector3d& vz) +{ + Eigen::Vector3d p_next; + Eigen::Vector3d p_start; + // polydata->GetPoint(idx, p.data()); + // polydata->GetPoint(idx + 1, p_next.data()); + polydata->GetPoint(0, p_start.data()); + polydata->GetPoint(idx, p.data()); + polydata->GetPoint(polydata->GetNumberOfPoints()-1, p_next.data()); + polydata->GetPointData()->GetNormals()->GetTuple(idx, vz.data()); + vz = vz.normalized(); + // vx = p_next - p; + vx = p_next - p_start; + vx = (vx - vx.dot(vz)*vz).normalized(); + vy = vz.cross(vx).normalized(); +} + static tool_path_planner::ToolPaths convertToPoses(const std::vector& rasters_data) { using namespace Eigen; @@ -410,32 +434,60 @@ static tool_path_planner::ToolPaths convertToPoses(const std::vectorGetNumberOfPoints(); - Vector3d p, p_next, vx, vy, vz; + Vector3d p, next_p; + Vector3d prev_vx, prev_vy, prev_vz; + Vector3d vx, vy, vz; + Vector3d next_vx, next_vy, next_vz; Isometry3d pose; std::vector indices(num_points); std::iota(indices.begin(), indices.end(), 0); // for every waypoint MAKE A POSE such that // its normal uses the mesh normal - // its x axis points toward next waypoint - // its y is z cross x - for (std::size_t i = 0; i < indices.size() - 1; i++) + // its x axis points along path + // its y is z cross x via right hand rule + // average 3 x-vectors to smooth out chatter caused by triangles + computePoseData(polydata, 0, p, vx, vy, vz); + prev_vx = vx; + prev_vy = vy; + prev_vz = vz; + int q=0; + for (std::size_t i = 0; i < indices.size() - 2; i++) { - int idx = indices[i]; - int idx_next = indices[i + 1]; - polydata->GetPoint(idx, p.data()); - polydata->GetPoint(idx_next, p_next.data()); - polydata->GetPointData()->GetNormals()->GetTuple(idx, vz.data()); - vx = (p_next - p).normalized(); + computePoseData(polydata, i+1, next_p, next_vx, next_vy, next_vz); + vx = (prev_vx + vx + next_vx).normalized(); vy = vz.cross(vx).normalized(); - vz = vx.cross(vy).normalized(); pose = Translation3d(p) * AngleAxisd(computeRotation(vx, vy, vz)); raster_path_segment.push_back(pose); + q++; + // ROS_ERROR("vx: %6.3lf %6.3lf %6.3lf p(%d, :) = [ %6.3lf %6.3lf %6.3lf ];", vx.x(), vx.y(), vx.z(), q, p.x(), p.y(), p.z()); + prev_vx = vx; + prev_vy = vy; + prev_vz = vz; + vx = next_vx; + vy = next_vy; + vz = next_vz; + p = next_p; } // end for every waypoint - // adding last pose - pose.translation() = p_next; // orientation stays the same as previous - raster_path_segment.push_back(pose); - + if(indices.size() >= 2) // this throws away short segments + { + // adding next to last pose + computePoseData(polydata, indices.size()-2, p, vx, vy, vz); + vx = (prev_vx + vx).normalized(); + pose = Translation3d(p) * AngleAxisd(computeRotation(vx, vy, vz)); + raster_path_segment.push_back(pose); + q++; + // ROS_ERROR("vx: %6.3lf %6.3lf %6.3lf p(%d, :) = [ %6.3lf %6.3lf %6.3lf ];", vx.x(), vx.y(), vx.z(), q, p.x(), p.y(), p.z()); + + + // adding last pose + polydata->GetPoint(indices.size()-1, p.data()); + pose = Translation3d(p) * AngleAxisd(computeRotation(vx, vy, vz)); + raster_path_segment.push_back(pose); + q++; + // ROS_ERROR("vx: %6.3lf %6.3lf %6.3lf p(%d, :) = [ %6.3lf %6.3lf %6.3lf ];", vx.x(), vx.y(), vx.z(), q, p.x(), p.y(), p.z()); + } + // ROS_ERROR("raster has %d poses", q); raster_path.push_back(raster_path_segment); } // end for every segment rasters_array.push_back(raster_path); @@ -519,7 +571,7 @@ boost::optional PlaneSlicerRasterGenerator::generate() } // 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; - ROS_ERROR_STREAM(config_.raster_wrt_global_axes); + if (config_.raster_wrt_global_axes) { // Determine extent of mesh along axes of current coordinate frame @@ -743,8 +795,9 @@ boost::optional PlaneSlicerRasterGenerator::generate() // merging segments mergeRasterSegments(raster_lines->GetPoints(), config_.min_hole_size, raster_ids); + // rectifying - if (!rasters_data_vec.empty()) + if (!rasters_data_vec.empty() && !rasters_data_vec.back().raster_segments.empty()) { Vector3d ref_point; rasters_data_vec.back().raster_segments.front()->GetPoint(0, ref_point.data()); // first point in previous raster @@ -790,14 +843,23 @@ boost::optional PlaneSlicerRasterGenerator::generate() } // saving into raster - r.raster_segments.push_back(segment_data); - r.segment_lengths.push_back(line_length); + if(segment_data->GetPoints()->GetNumberOfPoints()>0) + { + r.raster_segments.push_back(segment_data); + r.segment_lengths.push_back(line_length); + } } } - rasters_data_vec.push_back(r); + if(r.raster_segments.size()>0) + rasters_data_vec.push_back(r); } - + if(rasters_data_vec.size() == 0) + { + ROS_ERROR("no rasters found"); + ToolPaths rasters; + return(rasters); + } // make sure every raster has its segments ordered and aligned correctly Eigen::Vector3d raster_direction = getSegDir(rasters_data_vec[0].raster_segments[0]); for (RasterConstructData rcd : rasters_data_vec) From b53758df6ed78d7a32e87af8849141d61f7862c2 Mon Sep 17 00:00:00 2001 From: Chris Lewis Date: Tue, 13 Jul 2021 09:46:02 -0500 Subject: [PATCH 50/64] added parameters to service call to plane slicer for smoothing, and interleaving rasters --- noether/src/tool_path_planner_server.cpp | 12 +----- .../msg/PlaneSlicerRasterGeneratorConfig.msg | 2 + .../plane_slicer_raster_generator.h | 13 +++++++ .../src/plane_slicer_raster_generator.cpp | 23 +++++++++++- tool_path_planner/src/utilities.cpp | 37 +++---------------- 5 files changed, 44 insertions(+), 43 deletions(-) diff --git a/noether/src/tool_path_planner_server.cpp b/noether/src/tool_path_planner_server.cpp index 9973280b..8d0d8b65 100644 --- a/noether/src/tool_path_planner_server.cpp +++ b/noether/src/tool_path_planner_server.cpp @@ -24,7 +24,7 @@ using GenPathActionServer = actionlib::SimpleActionServer PlaneSlicerRasterGenerator::generate() rcd = alignRasterCD(rcd, raster_direction); } + if(config_.interleave_rasters) + { + std::vector tmp_rasters_data_vec; + // evens + for (size_t i=0; i Date: Thu, 22 Jul 2021 16:48:53 -0500 Subject: [PATCH 51/64] updated utilities to allow normal estimation using MLS --- .../msg/PlaneSlicerRasterGeneratorConfig.msg | 1 + noether_msgs/msg/ToolPathConfig.msg | 3 + .../tool_path_planner/path_generator.h | 5 +- .../include/tool_path_planner/utilities.h | 19 ++ .../src/halfedge_edge_generator.cpp | 8 +- .../src/plane_slicer_raster_generator.cpp | 8 +- tool_path_planner/src/utilities.cpp | 283 +++++++++++++++++- 7 files changed, 309 insertions(+), 18 deletions(-) diff --git a/noether_msgs/msg/PlaneSlicerRasterGeneratorConfig.msg b/noether_msgs/msg/PlaneSlicerRasterGeneratorConfig.msg index 35305795..5a073f8f 100644 --- a/noether_msgs/msg/PlaneSlicerRasterGeneratorConfig.msg +++ b/noether_msgs/msg/PlaneSlicerRasterGeneratorConfig.msg @@ -8,6 +8,7 @@ float64 search_radius # radius over which the normals are averaged bool generate_extra_rasters # Whether an extra raster stroke should be added to the beginning and end of the raster pattern bool interleave_rasters # create a second set of rasters interleaved inbetween first set bool smooth_rasters # post process poses by fitting a cubic b-spline and re-sampling +int32 raster_style # 0,1,2 for mow_style, paint_style and read_style # 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 diff --git a/noether_msgs/msg/ToolPathConfig.msg b/noether_msgs/msg/ToolPathConfig.msg index 57fdef89..49d3d358 100644 --- a/noether_msgs/msg/ToolPathConfig.msg +++ b/noether_msgs/msg/ToolPathConfig.msg @@ -3,6 +3,9 @@ uint8 PLANE_SLICER_RASTER_GENERATOR=1 uint8 HEAT_RASTER_GENERATOR=2 uint8 EIGEN_VALUE_EDGE_GENERATOR=3 # This does not have a config uint8 HALFEDGE_EDGE_GENERATOR=4 +uint8 PAINT_RASTER_STYLE=0 +uint8 MOW_RASTER_STYLE=1 +uint8 READ_RASTER_STYLE=2 uint8 type noether_msgs/SurfaceWalkRasterGeneratorConfig surface_walk_generator diff --git a/tool_path_planner/include/tool_path_planner/path_generator.h b/tool_path_planner/include/tool_path_planner/path_generator.h index caa42da6..0f25ecbd 100644 --- a/tool_path_planner/include/tool_path_planner/path_generator.h +++ b/tool_path_planner/include/tool_path_planner/path_generator.h @@ -15,8 +15,9 @@ namespace tool_path_planner { enum RasterStyle : int { - KEEP_ORIENTATION_ON_REVERSE_STROKES = 0, - FLIP_ORIENTATION_ON_REVERSE_STROKES = 1, + KEEP_ORIENTATION_ON_REVERSE_STROKES = 0, // painting style, back and fourth without flipping + FLIP_ORIENTATION_ON_REVERSE_STROKES = 1, // mowing style, back and fourth, but flip orientation at turns + PROCESS_FORWARD_DIRECTION_ONLY = 2, // reading style left to right and return }; /** @brief A set of contiguous waypoints that lie on the same line created by a "slice" through a surface */ diff --git a/tool_path_planner/include/tool_path_planner/utilities.h b/tool_path_planner/include/tool_path_planner/utilities.h index c050572f..6112d8fd 100644 --- a/tool_path_planner/include/tool_path_planner/utilities.h +++ b/tool_path_planner/include/tool_path_planner/utilities.h @@ -28,6 +28,7 @@ #include #include #include +#include #include #include @@ -160,6 +161,24 @@ ToolPaths splitByAxes(const ToolPaths& tool_paths, const Eigen::Vector3f& axis_1 */ void InterleavePoseTraj(noether_msgs::ToolPaths& tool_paths, double raster_spacing); +pcl::PointCloud computeMLSMeshNormals(const pcl::PointCloud::Ptr& mesh_cloud, double normal_search_radius); + +bool applyEqualDistance(const std::vector& pnt_indices, + const pcl::PointCloud& mesh_cloud, + pcl::PointCloud& path_cloud, + double dist); + +bool insertPointNormals(const pcl::PointCloud::Ptr& mesh_cloud, + pcl::PointCloud& path_cloud); + +void shapeMsgToPclPointXYZ( const shape_msgs::Mesh& mesh, pcl::PointCloud& mesh_cloud); + + +void computeFaceNormals(const shape_msgs::Mesh& mesh, std::vector& face_normals); + +void computeMeshNormals(const shape_msgs::Mesh& mesh, std::vector& face_normals, std::vector& vertex_normals); + +bool alignToVertexNormals(pcl::PointCloud& mesh_cloud, std::vector& vertex_normals); } // namespace tool_path_planner #endif /* INCLUDE_TOOL_PATH_PLANNER_UTILITIES_H_ */ diff --git a/tool_path_planner/src/halfedge_edge_generator.cpp b/tool_path_planner/src/halfedge_edge_generator.cpp index 54fd577c..a0b80205 100644 --- a/tool_path_planner/src/halfedge_edge_generator.cpp +++ b/tool_path_planner/src/halfedge_edge_generator.cpp @@ -155,7 +155,7 @@ bool applyMinPointDistance(const pcl::PointCloud& in, * @param dist The required point spacing distance * @return True on success, False otherwise */ -bool applyEqualDistance(const pcl::PointCloud& in, +bool applyEqualDistance1(const pcl::PointCloud& in, pcl::PointCloud& out, double dist) { @@ -215,7 +215,7 @@ bool applyEqualDistance(const pcl::PointCloud& in, } // add last point if not already there - if ((out.back().getVector3fMap() - in.back().getNormalVector3fMap()).norm() > MIN_POINT_DIST_ALLOWED) + if ((out.back().getVector3fMap() - in.back().getVector3fMap()).norm() > MIN_POINT_DIST_ALLOWED) { out.push_back(in.back()); } @@ -500,9 +500,9 @@ boost::optional HalfedgeEdgeGenerator::generate() break; case PointSpacingMethod::EQUAL_SPACING: - if (!applyEqualDistance(bound_segment_points, decimated_points, config_.point_dist)) + if (!applyEqualDistance1(bound_segment_points, decimated_points, config_.point_dist)) { - CONSOLE_BRIDGE_logError("applyEqualDistance point spacing method failed"); + CONSOLE_BRIDGE_logError("applyEqualDistance1 point spacing method failed"); return boost::none; } break; diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index 1135884f..5711dffd 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -895,9 +895,11 @@ boost::optional PlaneSlicerRasterGenerator::generate() rasters = addExtraWaypoints(rasters, config_.raster_spacing, config_.point_spacing); } - // switch directions of every other raster without rotating - rasters = reverseOddRasters(rasters, config_.raster_style); - + if((config_.raster_style != PROCESS_FORWARD_DIRECTION_ONLY)) + { + // switch directions of every other raster, using either flipping orientation or not depending on style selected + rasters = reverseOddRasters(rasters, config_.raster_style); + } return rasters; } diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index 387f5355..0b87dd91 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -46,6 +46,9 @@ typedef std::tuple wp_tuple; typedef std::list tlist; typedef std::list::iterator tlist_it; +static const double MIN_POINT_DIST_ALLOWED = 1e-8; +static const double EPSILON = 1e-3; + void flipPointOrder(ToolPath& path) { // Reverse the order of the segments @@ -196,6 +199,7 @@ bool toPlaneSlicerConfigMsg(noether_msgs::PlaneSlicerRasterGeneratorConfig& conf config_msg.search_radius = config.search_radius; config_msg.interleave_rasters = config.interleave_rasters; config_msg.smooth_rasters = config.smooth_rasters; + config_msg.raster_style = config.raster_style; config_msg.generate_extra_rasters = config.generate_extra_rasters; config_msg.raster_wrt_global_axes = config.raster_wrt_global_axes; config_msg.raster_direction.x = config.raster_direction.x(); @@ -262,6 +266,7 @@ bool toPlaneSlicerConfig(PlaneSlicerRasterGenerator::Config& config, config.search_radius = config_msg.search_radius; config.interleave_rasters = config_msg.interleave_rasters; config.smooth_rasters = config_msg.smooth_rasters; + config.raster_style = static_cast(config_msg.raster_style); config.generate_extra_rasters = config_msg.generate_extra_rasters; config.raster_wrt_global_axes = config_msg.raster_wrt_global_axes; @@ -289,16 +294,19 @@ bool createToolPathSegment(const pcl::PointCloud& cloud_normal Vector3d x_dir, z_dir, y_dir; std::vector cloud_indices; if (indices.empty()) - { - cloud_indices.resize(cloud_normals.size()); - std::iota(cloud_indices.begin(), cloud_indices.end(), 0); - } + { + cloud_indices.reserve(cloud_normals.size()); + for(int i=0; i& cloud_normal p.translation().y() = cloud_normals[cloud_indices.back()].y; p.translation().z() = cloud_normals[cloud_indices.back()].z; segment.push_back(p); - return true; } @@ -902,5 +909,263 @@ ToolPaths addExtraWaypoints(const ToolPaths& tool_paths, double raster_spacing, return new_tool_paths; } +// computes the normal vectors to a mesh at every vertex +pcl::PointCloud computeMLSMeshNormals(const pcl::PointCloud::Ptr& mesh_cloud, double normal_search_radius) +{ + // initialize the MLSNE object + pcl::MovingLeastSquares MLS; + MLS.setInputCloud(mesh_cloud); + MLS.setComputeNormals(true); + pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); + MLS.setSearchMethod(tree); + MLS.setPolynomialOrder(2); + MLS.setSearchRadius(normal_search_radius); + MLS.setUpsamplingMethod(pcl::MovingLeastSquares::UpsamplingMethod::NONE); + MLS.setCacheMLSResults(true); + // NONE, DISTINCT_CLOUD, SAMPLE_LOCAL_PLANE, RANDOM_UNIFORM_DENSITY, VOXEL_GRID_DILATION + // unused initialization methods here to help future developers + // MLS.setUpsamplingRadius(), MLS.setUpsamplingStepSize(), MLS.setDistinctCloud() + + // process the cloud + pcl::PointCloud mls_mesh_cloud_normals; + MLS.process(mls_mesh_cloud_normals); + std::vector R = MLS.getMLSResults(); + + pcl::PointCloud mesh_cloud_normals; + mesh_cloud_normals.reserve(mesh_cloud->points.size()); + for(size_t i=0; ipoints.size(); i++) + { + pcl::PointNormal new_pn; + new_pn.x = mesh_cloud->points[i].x; + new_pn.y = mesh_cloud->points[i].y; + new_pn.z = mesh_cloud->points[i].z; + new_pn.normal_x = R[i].plane_normal.x(); + new_pn.normal_y = R[i].plane_normal.y(); + new_pn.normal_z = R[i].plane_normal.z(); + mesh_cloud_normals.push_back(new_pn); + } + return(mesh_cloud_normals); +} + +/** + * @brief forces the required point spacing by computing new points along the curve + * @param in The input cloud + * @param out The output cloud + * @param dist The required point spacing distance + * @return True on success, False otherwise + */ +bool applyEqualDistance(const std::vector& pnt_indices, + const pcl::PointCloud& mesh_cloud, + pcl::PointCloud& path_cloud, + double dist) +{ + using namespace pcl; + using namespace Eigen; + + PointNormal new_pt; + PointXYZ p_start, p_mid, p_end; + Vector3f v_1, v_2, unitv_2, v_next; + if(pnt_indices.size()<3) + { + CONSOLE_BRIDGE_logError("pnt_indices must have at least 3"); + return false; + } + PointXYZ pt = mesh_cloud.points[pnt_indices[0]]; + p_start.x = pt.x; + p_start.y = pt.y; + p_start.z = pt.z; + pt = mesh_cloud.points[pnt_indices[1]]; + p_mid.x = pt.x; + p_mid.y = pt.y; + p_mid.z = pt.z; + v_1 = p_mid.getVector3fMap() - p_start.getVector3fMap(); + new_pt.x = mesh_cloud[pnt_indices[0]].x; + new_pt.y = mesh_cloud[pnt_indices[0]].y; + new_pt.z = mesh_cloud[pnt_indices[0]].z; + path_cloud.push_back(new_pt); // add first point + for (std::size_t i = 2; i < pnt_indices.size(); i++) + { + pt = mesh_cloud[pnt_indices[i]]; + p_end.x = pt.x; + p_end.y = pt.y; + p_end.z = pt.z; + while (dist < v_1.norm()) + { + p_start.getVector3fMap() = p_start.getVector3fMap() + dist * v_1.normalized(); + std::tie(new_pt.x, new_pt.y, new_pt.z) = std::make_tuple(p_start.x, p_start.y, p_start.z); + path_cloud.push_back(new_pt); + v_1 = p_mid.getVector3fMap() - p_start.getVector3fMap(); + } + + v_2 = p_end.getVector3fMap() - p_mid.getVector3fMap(); + if (dist < (v_1 + v_2).norm()) + { + // solve for x such that " x^2 + 2 * x * dot(v_1, unitv_2) + norm(v_1)^2 - d^2 = 0" + unitv_2 = v_2.normalized(); + double b = 2 * v_1.dot(unitv_2); + double c = std::pow(v_1.norm(), 2) - std::pow(dist, 2); + double sqrt_term = std::sqrt(std::pow(b, 2) - 4 * c); + double x = 0.5 * (-b + sqrt_term); + x = (x > 0.0 && x < dist) ? x : 0.5 * (-b - sqrt_term); + if (x > dist) + { + return false; + } + v_next = v_1 + x * unitv_2; + + // computing next point at distance "dist" from previous that lies on the curve + p_start.getVector3fMap() = p_start.getVector3fMap() + v_next; + std::tie(new_pt.x, new_pt.y, new_pt.z) = std::make_tuple(p_start.x, p_start.y, p_start.z); + path_cloud.push_back(new_pt); + } + + // computing values for next iter + p_mid = p_end; + v_1 = p_mid.getVector3fMap() - p_start.getVector3fMap(); + } + + // add last point if not already there + if ((path_cloud.back().getVector3fMap() - mesh_cloud[pnt_indices.back()].getVector3fMap()).norm() > MIN_POINT_DIST_ALLOWED) + { + new_pt.x = mesh_cloud[pnt_indices[pnt_indices.size()-1]].x; + new_pt.y = mesh_cloud[pnt_indices[pnt_indices.size()-1]].y; + new_pt.z = mesh_cloud[pnt_indices[pnt_indices.size()-1]].z; + path_cloud.push_back(new_pt); + } + + if (path_cloud.size() < 2) + { + CONSOLE_BRIDGE_logError("Points in curve segment are too close together"); + return false; + } + + return true; +} + +bool insertPointNormals(const pcl::PointCloud::Ptr& mesh_cloud, + pcl::PointCloud& path_cloud) +{ + //recovering normals using kd-treek + pcl::PointCloud::Ptr in_points (new pcl::PointCloud ()); + for(pcl::PointNormal p : mesh_cloud->points) + { + pcl::PointXYZ pt(p.x, p.y,p.z); + in_points->points.push_back(pt); + } + pcl::KdTreeFLANN kdtree; + kdtree.setInputCloud(in_points); + kdtree.setEpsilon(EPSILON); + + std::vector k_indices; + std::vector k_sqr_distances; + pcl::PointXYZ query_point; + for (auto& p : path_cloud) + { + k_indices.resize(1); + k_sqr_distances.resize(1); + std::tie(query_point.x, query_point.y, query_point.z) = std::make_tuple(p.x, p.y, p.z); + if (kdtree.nearestKSearch(query_point, 1, k_indices, k_sqr_distances) < 1) + { + CONSOLE_BRIDGE_logError("No nearest points found"); + return false; + } + + int idx = k_indices[0]; + if(idx < mesh_cloud->points.size()) + { + std::tie(p.normal_x, p.normal_y, p.normal_z) = + std::make_tuple(mesh_cloud->points[idx].normal_x, mesh_cloud->points[idx].normal_y, mesh_cloud->points[idx].normal_z); + } + else + { + ROS_ERROR("what is wrong %ld", idx); + } + } + + return true; +} + +void shapeMsgToPclPointXYZ( const shape_msgs::Mesh& mesh, pcl::PointCloud& mesh_cloud) +{ + mesh_cloud.points.clear(); + for(int i=0; i& face_normals) +{ + face_normals.clear(); + face_normals.reserve(mesh.triangles.size()); + for(size_t i=0; i& face_normals, std::vector& vertex_normals) +{ + computeFaceNormals(mesh, face_normals); + // populate a structure for each vertex containing a list of adjoining faces + std::vector vertex_faces[mesh.vertices.size()]; + for(size_t i=0; i& mesh_cloud, std::vector& vertex_normals) +{ + if(mesh_cloud.points.size() != vertex_normals.size()) + { + ROS_ERROR("mesh_cloud and vertex_normals have different number of points %ld vs %ld", mesh_cloud.points.size(), vertex_normals.size()); + return(false); + } + int num_flipped=0; + for(size_t i=0; i Date: Fri, 23 Jul 2021 14:19:06 -0500 Subject: [PATCH 52/64] updated method for setting x-direction of pose --- tool_path_planner/src/utilities.cpp | 41 +++++++++++++++++++++++------ 1 file changed, 33 insertions(+), 8 deletions(-) diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index 0b87dd91..fb6fe5da 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -306,8 +306,28 @@ bool createToolPathSegment(const pcl::PointCloud& cloud_normal cloud_indices.assign(indices.begin(), indices.end()); } - for (std::size_t i = 0; i < cloud_indices.size() - 2; i++) + if(cloud_indices.size() < 3) + { + // TODO, covering the 2 point case is straight forward + ROS_ERROR("A valid path must have at least 3 points"); + return false; + } + + // compute first pose with x-axis using vector from first point to third point + const PointNormal& p1 = cloud_normals[cloud_indices[0]]; + const PointNormal& p2 = cloud_normals[cloud_indices[2]]; + x_dir = (p2.getVector3fMap() - p1.getVector3fMap()).normalized().cast(); + z_dir = Vector3d(p1.normal_x, p1.normal_y, p1.normal_z).normalized(); + x_dir = (x_dir - x_dir.dot(z_dir)*z_dir).normalized();// remove component of x along z + y_dir = z_dir.cross(x_dir).normalized(); + + pose = Translation3d(p1.getVector3fMap().cast()); + pose.matrix().block<3, 3>(0, 0) = tool_path_planner::toRotationMatrix(x_dir, y_dir, z_dir); + segment.push_back(pose); + + for (std::size_t i = 1; i < cloud_indices.size() - 2; i++) { + std::size_t idx_prev = cloud_indices[i - 1]; std::size_t idx_current = cloud_indices[i]; std::size_t idx_next = cloud_indices[i + 1]; if (idx_current >= cloud_normals.size() || idx_next >= cloud_normals.size()) @@ -316,10 +336,12 @@ bool createToolPathSegment(const pcl::PointCloud& cloud_normal "Invalid indices (current: %lu, next: %lu) for point cloud were passed", idx_current, idx_next); return false; } + const PointNormal& p0 = cloud_normals[idx_prev]; const PointNormal& p1 = cloud_normals[idx_current]; const PointNormal& p2 = cloud_normals[idx_next]; - x_dir = (p2.getVector3fMap() - p1.getVector3fMap()).normalized().cast(); + x_dir = (p2.getVector3fMap() - p0.getVector3fMap()).normalized().cast(); z_dir = Vector3d(p1.normal_x, p1.normal_y, p1.normal_z).normalized(); + x_dir = (x_dir - x_dir.dot(z_dir)*z_dir).normalized();// remove component of x along z y_dir = z_dir.cross(x_dir).normalized(); pose = Translation3d(p1.getVector3fMap().cast()); @@ -327,12 +349,15 @@ bool createToolPathSegment(const pcl::PointCloud& cloud_normal segment.push_back(pose); } - // last pose - Eigen::Isometry3d p = segment.back(); - p.translation().x() = cloud_normals[cloud_indices.back()].x; - p.translation().y() = cloud_normals[cloud_indices.back()].y; - p.translation().z() = cloud_normals[cloud_indices.back()].z; - segment.push_back(p); + // compute last pose with last computed x-axis (corrected for change in z + const PointNormal& p_last = cloud_normals[cloud_indices[cloud_indices.size() -1]]; + z_dir = Vector3d(p_last.normal_x, p_last.normal_y, p_last.normal_z).normalized(); + x_dir = (x_dir - x_dir.dot(z_dir)*z_dir).normalized(); + y_dir = z_dir.cross(x_dir).normalized(); + pose = Translation3d(p_last.getVector3fMap().cast()); + pose.matrix().block<3, 3>(0, 0) = tool_path_planner::toRotationMatrix(x_dir, y_dir, z_dir); + segment.push_back(pose); + return true; } From caad679687773273ac9446e8a1256f0691f1b04c Mon Sep 17 00:00:00 2001 From: Chris Lewis Date: Wed, 28 Jul 2021 17:18:43 -0500 Subject: [PATCH 53/64] added MLS normal estimation and other utility functions(e.g. compute face and vertex normals from mesh either as shape_msg or pcl::Polygonmesh. Modified plane slicer to use MLS for its normal estimation. --- .../plane_slicer_raster_generator.h | 22 +- .../include/tool_path_planner/utilities.h | 1 + .../src/plane_slicer_raster_generator.cpp | 219 +++++++++--------- tool_path_planner/src/utilities.cpp | 89 ++++++- 4 files changed, 205 insertions(+), 126 deletions(-) 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 96da1482..2ff8f209 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 @@ -175,6 +175,14 @@ class PlaneSlicerRasterGenerator : public PathGenerator } }; + using PolyDataPtr = vtkSmartPointer; + + struct RasterConstructData + { + std::vector raster_segments; + std::vector segment_lengths; + }; + PlaneSlicerRasterGenerator() = default; /** @@ -184,11 +192,8 @@ class PlaneSlicerRasterGenerator : public PathGenerator */ void setConfiguration(const Config& config); - void setInput(pcl::PolygonMesh::ConstPtr mesh) override; - - void setInput(vtkSmartPointer mesh) override; - void setInput(const shape_msgs::Mesh& mesh) override; + void setInput(pcl::PolygonMesh::ConstPtr mesh) override; vtkSmartPointer getInput() override; @@ -197,12 +202,21 @@ class PlaneSlicerRasterGenerator : public PathGenerator std::string getName() const override; private: + + void setInput(vtkSmartPointer mesh) override; + bool insertNormals(const double search_radius, vtkSmartPointer& data); + tool_path_planner::ToolPaths convertToPoses(const std::vector& rasters_data); + void computePoseData(const PolyDataPtr& polydata, int idx, Eigen::Vector3d& p, Eigen::Vector3d& vx, Eigen::Vector3d& vy, Eigen::Vector3d& vz); + vtkSmartPointer mesh_data_; vtkSmartPointer kd_tree_; vtkSmartPointer cell_locator_; Config config_; + boost::shared_ptr > mls_mesh_normals_ptr_; + std::vector vertex_normals_; + std::vector face_normals_; }; } /* namespace tool_path_planner */ diff --git a/tool_path_planner/include/tool_path_planner/utilities.h b/tool_path_planner/include/tool_path_planner/utilities.h index 6112d8fd..6e8751c7 100644 --- a/tool_path_planner/include/tool_path_planner/utilities.h +++ b/tool_path_planner/include/tool_path_planner/utilities.h @@ -177,6 +177,7 @@ void shapeMsgToPclPointXYZ( const shape_msgs::Mesh& mesh, pcl::PointCloud& face_normals); void computeMeshNormals(const shape_msgs::Mesh& mesh, std::vector& face_normals, std::vector& vertex_normals); +void computePCLMeshNormals(pcl::PolygonMesh::ConstPtr& mesh, std::vector& face_normals, std::vector& vertex_normals); bool alignToVertexNormals(pcl::PointCloud& mesh_cloud, std::vector& vertex_normals); } // namespace tool_path_planner diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index 5711dffd..2653a5f2 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -56,11 +56,6 @@ static const double EPSILON = 1e-6; using PolyDataPtr = vtkSmartPointer; -struct RasterConstructData -{ - std::vector raster_segments; - std::vector segment_lengths; -}; static Eigen::Vector3d getSegDir(PolyDataPtr seg) { @@ -86,7 +81,7 @@ static bool compare_ds_pair(std::pair& first, std::pairGetPoints()->GetNumberOfPoints() <= 1) { @@ -97,7 +92,7 @@ static RasterConstructData alignRasterCD(RasterConstructData& rcd, Eigen::Vector rcd.raster_segments[0]->GetPoint(0, raster_start.data()); // determine location and direction of each successive segement, reverse any mis-directed segments - RasterConstructData temp_rcd; + tool_path_planner::PlaneSlicerRasterGenerator::RasterConstructData temp_rcd; std::list > seg_order; // once sorted this list will be the order of the segments for (size_t i = 0; i < rcd.raster_segments.size(); i++) { @@ -128,7 +123,7 @@ static RasterConstructData alignRasterCD(RasterConstructData& rcd, Eigen::Vector { seg_order.sort(compare_ds_pair); } - RasterConstructData new_rcd; + tool_path_planner::PlaneSlicerRasterGenerator::RasterConstructData new_rcd; for (std::pair p : seg_order) { size_t seg_index = std::get<1>(p); @@ -404,28 +399,109 @@ static void rectifyDirection(const vtkSmartPointer& points, } } -static void computePoseData(const PolyDataPtr& polydata, int idx, Eigen::Vector3d& p, Eigen::Vector3d& vx, Eigen::Vector3d& vy, Eigen::Vector3d& vz) + +namespace tool_path_planner +{ +void PlaneSlicerRasterGenerator::setConfiguration(const PlaneSlicerRasterGenerator::Config& config) +{ + config_ = config; +} + +void PlaneSlicerRasterGenerator::setInput(vtkSmartPointer mesh) +{ + if (!mesh_data_) + mesh_data_ = vtkSmartPointer::New(); + + mesh_data_->DeepCopy(mesh); + + if (mesh_data_->GetPointData()->GetNormals() && mesh_data_->GetCellData()->GetNormals()) + { + CONSOLE_BRIDGE_logInform("Normal data is available", getName().c_str()); + } + else + { + vtkSmartPointer normal_generator = vtkSmartPointer::New(); + normal_generator->SetInputData(mesh_data_); + normal_generator->ComputePointNormalsOn(); + normal_generator->SetComputeCellNormals(!mesh_data_->GetCellData()->GetNormals()); + normal_generator->SetFeatureAngle(M_PI_2); + normal_generator->SetSplitting(true); + normal_generator->SetConsistency(true); + normal_generator->SetAutoOrientNormals(false); + normal_generator->SetFlipNormals(false); + normal_generator->SetNonManifoldTraversal(false); + normal_generator->Update(); + + if (!mesh_data_->GetPointData()->GetNormals()) + { + mesh_data_->GetPointData()->SetNormals(normal_generator->GetOutput()->GetPointData()->GetNormals()); + } + + if (!mesh_data_->GetCellData()->GetNormals()) + { + mesh_data_->GetCellData()->SetNormals(normal_generator->GetOutput()->GetCellData()->GetNormals()); + } + } +} + + +void PlaneSlicerRasterGenerator::setInput(pcl::PolygonMesh::ConstPtr mesh) +{ + + auto mesh_data = vtkSmartPointer::New(); + pcl::VTKUtils::mesh2vtk(*mesh, mesh_data); + mesh_data->BuildLinks(); + mesh_data->BuildCells(); + + // compute face and vertex using polygon info + tool_path_planner::computePCLMeshNormals(mesh, face_normals_, vertex_normals_); + + // compute vertex normals using Moving Least Squares + pcl::PointCloud::Ptr mesh_cloud_ptr(new pcl::PointCloud() ); + pcl::fromPCLPointCloud2(mesh->cloud, *mesh_cloud_ptr); + mls_mesh_normals_ptr_ = boost::make_shared > (tool_path_planner::computeMLSMeshNormals(mesh_cloud_ptr, config_.search_radius)); + + + // align mls_vertex_normals to vertex_normals + if(!tool_path_planner::alignToVertexNormals(*mls_mesh_normals_ptr_, vertex_normals_)) + { + ROS_ERROR("alignToVertexNormals failed"); + } + + setInput(mesh_data); +} + + +void PlaneSlicerRasterGenerator::setInput(const shape_msgs::Mesh& mesh) +{ + pcl::PolygonMesh::Ptr pcl_mesh = boost::make_shared(); + noether_conversions::convertToPCLMesh(mesh, *pcl_mesh); + setInput(pcl_mesh); +} + +vtkSmartPointer PlaneSlicerRasterGenerator::getInput() { return mesh_data_; } +void PlaneSlicerRasterGenerator::computePoseData(const PolyDataPtr& polydata, int idx, Eigen::Vector3d& p, Eigen::Vector3d& vx, Eigen::Vector3d& vy, Eigen::Vector3d& vz) { Eigen::Vector3d p_next; Eigen::Vector3d p_start; - // polydata->GetPoint(idx, p.data()); + + // polydata->GetPoint(idx, p.data()); // use this and next point to determine x direction // polydata->GetPoint(idx + 1, p_next.data()); - polydata->GetPoint(0, p_start.data()); + polydata->GetPoint(0, p_start.data()); // use the first and last point to determine x direction polydata->GetPoint(idx, p.data()); polydata->GetPoint(polydata->GetNumberOfPoints()-1, p_next.data()); polydata->GetPointData()->GetNormals()->GetTuple(idx, vz.data()); vz = vz.normalized(); - // vx = p_next - p; vx = p_next - p_start; vx = (vx - vx.dot(vz)*vz).normalized(); vy = vz.cross(vx).normalized(); } -static tool_path_planner::ToolPaths convertToPoses(const std::vector& rasters_data) +tool_path_planner::ToolPaths PlaneSlicerRasterGenerator::convertToPoses(const std::vector& rasters_data) { using namespace Eigen; tool_path_planner::ToolPaths rasters_array; - for (const RasterConstructData& rd : rasters_data) // for every raster + for (const tool_path_planner::PlaneSlicerRasterGenerator::RasterConstructData& rd : rasters_data) // for every raster { tool_path_planner::ToolPath raster_path; std::vector raster_segments; @@ -459,7 +535,7 @@ static tool_path_planner::ToolPaths convertToPoses(const std::vectorGetPoint(indices.size()-1, p.data()); pose = Translation3d(p) * AngleAxisd(computeRotation(vx, vy, vz)); raster_path_segment.push_back(pose); q++; - // ROS_ERROR("vx: %6.3lf %6.3lf %6.3lf p(%d, :) = [ %6.3lf %6.3lf %6.3lf ];", vx.x(), vx.y(), vx.z(), q, p.x(), p.y(), p.z()); } - // ROS_ERROR("raster has %d poses", q); raster_path.push_back(raster_path_segment); } // end for every segment rasters_array.push_back(raster_path); @@ -496,67 +568,6 @@ static tool_path_planner::ToolPaths convertToPoses(const std::vector::New(); - pcl::VTKUtils::mesh2vtk(*mesh, mesh_data); - mesh_data->BuildLinks(); - mesh_data->BuildCells(); - setInput(mesh_data); -} - -void PlaneSlicerRasterGenerator::setInput(vtkSmartPointer mesh) -{ - if (!mesh_data_) - mesh_data_ = vtkSmartPointer::New(); - - mesh_data_->DeepCopy(mesh); - - if (mesh_data_->GetPointData()->GetNormals() && mesh_data_->GetCellData()->GetNormals()) - { - CONSOLE_BRIDGE_logInform("Normal data is available", getName().c_str()); - } - else - { - vtkSmartPointer normal_generator = vtkSmartPointer::New(); - normal_generator->SetInputData(mesh_data_); - normal_generator->ComputePointNormalsOn(); - normal_generator->SetComputeCellNormals(!mesh_data_->GetCellData()->GetNormals()); - normal_generator->SetFeatureAngle(M_PI_2); - normal_generator->SetSplitting(true); - normal_generator->SetConsistency(true); - normal_generator->SetAutoOrientNormals(false); - normal_generator->SetFlipNormals(false); - normal_generator->SetNonManifoldTraversal(false); - normal_generator->Update(); - - if (!mesh_data_->GetPointData()->GetNormals()) - { - mesh_data_->GetPointData()->SetNormals(normal_generator->GetOutput()->GetPointData()->GetNormals()); - } - - if (!mesh_data_->GetCellData()->GetNormals()) - { - mesh_data_->GetCellData()->SetNormals(normal_generator->GetOutput()->GetCellData()->GetNormals()); - } - } -} - -void PlaneSlicerRasterGenerator::setInput(const shape_msgs::Mesh& mesh) -{ - pcl::PolygonMesh::Ptr pcl_mesh = boost::make_shared(); - noether_conversions::convertToPCLMesh(mesh, *pcl_mesh); - setInput(pcl_mesh); -} - -vtkSmartPointer PlaneSlicerRasterGenerator::getInput() { return mesh_data_; } boost::optional PlaneSlicerRasterGenerator::generate() { @@ -733,12 +744,12 @@ boost::optional PlaneSlicerRasterGenerator::generate() // collect rasters and set direction raster_data->Update(); vtkIdType num_slices = raster_data->GetTotalNumberOfInputConnections(); - std::vector rasters_data_vec; + std::vector rasters_data_vec; std::vector raster_ids; boost::optional ref_dir; for (std::size_t i = 0; i < num_slices; i++) { - RasterConstructData r; + tool_path_planner::PlaneSlicerRasterGenerator::RasterConstructData r; // collecting raster segments based on min hole size vtkSmartPointer raster_lines = raster_data->GetInput(i); @@ -862,14 +873,14 @@ boost::optional PlaneSlicerRasterGenerator::generate() } // make sure every raster has its segments ordered and aligned correctly Eigen::Vector3d raster_direction = getSegDir(rasters_data_vec[0].raster_segments[0]); - for (RasterConstructData rcd : rasters_data_vec) + for (tool_path_planner::PlaneSlicerRasterGenerator::RasterConstructData rcd : rasters_data_vec) { rcd = alignRasterCD(rcd, raster_direction); } if(config_.interleave_rasters) { - std::vector tmp_rasters_data_vec; + std::vector tmp_rasters_data_vec; // evens for (size_t i=0; i PlaneSlicerRasterGenerator::generate() rasters_data_vec.push_back(tmp_rasters_data_vec[i]); } } + // converting to poses msg ToolPaths rasters = convertToPoses(rasters_data_vec); @@ -926,46 +938,31 @@ bool PlaneSlicerRasterGenerator::insertNormals(const double search_radius, vtkSm Eigen::Vector3d query_point; vtkSmartPointer id_list = vtkSmartPointer::New(); data->GetPoints()->GetPoint(i, query_point.data()); - kd_tree_->FindPointsWithinRadius(search_radius, query_point.data(), id_list); + kd_tree_->FindClosestNPoints(1, query_point.data(), id_list); if (id_list->GetNumberOfIds() < 1) - { - CONSOLE_BRIDGE_logWarn("%s FindPointsWithinRadius found no points for normal averaging, using closest", - getName().c_str()); - kd_tree_->FindClosestNPoints(1, query_point.data(), id_list); - - if (id_list->GetNumberOfIds() < 1) { CONSOLE_BRIDGE_logError("%s failed to find closest for normal computation", getName().c_str()); return false; } - } // compute normal average normal_vect = Eigen::Vector3d::Zero(); - std::size_t num_normals = 0; - for (auto p = 0; p < id_list->GetNumberOfIds(); p++) - { - Eigen::Vector3d temp_normal, query_point, closest_point; - vtkIdType p_id = id_list->GetId(p); - - if (p_id < 0) + vtkIdType p_id = id_list->GetId(0); + if (p_id < 0) { - CONSOLE_BRIDGE_logError("%s point id is invalid", getName().c_str()); - continue; + CONSOLE_BRIDGE_logError("%s point id is invalid", getName().c_str()); + continue; } - - // get normal and add it to average - normal_data->GetTuple(p_id, temp_normal.data()); - normal_vect += temp_normal.normalized(); - num_normals++; - } - - normal_vect /= num_normals; + + normal_vect.x() = mls_mesh_normals_ptr_->points[p_id].normal_x; + normal_vect.y() = mls_mesh_normals_ptr_->points[p_id].normal_y; + normal_vect.z() = mls_mesh_normals_ptr_->points[p_id].normal_z; normal_vect.normalize(); - + // save normal new_normals->SetTuple3(i, normal_vect(0), normal_vect(1), normal_vect(2)); - } + }// end for every point + data->GetPointData()->SetNormals(new_normals); return true; } diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index fb6fe5da..7ab85078 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -46,7 +46,6 @@ typedef std::tuple wp_tuple; typedef std::list tlist; typedef std::list::iterator tlist_it; -static const double MIN_POINT_DIST_ALLOWED = 1e-8; static const double EPSILON = 1e-3; void flipPointOrder(ToolPath& path) @@ -943,10 +942,11 @@ pcl::PointCloud computeMLSMeshNormals(const pcl::PointCloud::Ptr tree (new pcl::search::KdTree); MLS.setSearchMethod(tree); - MLS.setPolynomialOrder(2); + MLS.setPolynomialOrder(3); MLS.setSearchRadius(normal_search_radius); MLS.setUpsamplingMethod(pcl::MovingLeastSquares::UpsamplingMethod::NONE); MLS.setCacheMLSResults(true); + MLS.setProjectionMethod(pcl::MLSResult::ProjectionMethod::ORTHOGONAL); // NONE, DISTINCT_CLOUD, SAMPLE_LOCAL_PLANE, RANDOM_UNIFORM_DENSITY, VOXEL_GRID_DILATION // unused initialization methods here to help future developers // MLS.setUpsamplingRadius(), MLS.setUpsamplingStepSize(), MLS.setDistinctCloud() @@ -964,9 +964,9 @@ pcl::PointCloud computeMLSMeshNormals(const pcl::PointCloudpoints[i].x; new_pn.y = mesh_cloud->points[i].y; new_pn.z = mesh_cloud->points[i].z; - new_pn.normal_x = R[i].plane_normal.x(); - new_pn.normal_y = R[i].plane_normal.y(); - new_pn.normal_z = R[i].plane_normal.z(); + new_pn.normal_x = -R[i].plane_normal.x(); + new_pn.normal_y = -R[i].plane_normal.y(); + new_pn.normal_z = -R[i].plane_normal.z(); mesh_cloud_normals.push_back(new_pn); } return(mesh_cloud_normals); @@ -1050,7 +1050,7 @@ bool applyEqualDistance(const std::vector& pnt_indices, } // add last point if not already there - if ((path_cloud.back().getVector3fMap() - mesh_cloud[pnt_indices.back()].getVector3fMap()).norm() > MIN_POINT_DIST_ALLOWED) + if ((path_cloud.back().getVector3fMap() - mesh_cloud[pnt_indices.back()].getVector3fMap()).norm() > dist/3.0) { new_pt.x = mesh_cloud[pnt_indices[pnt_indices.size()-1]].x; new_pt.y = mesh_cloud[pnt_indices[pnt_indices.size()-1]].y; @@ -1103,13 +1103,15 @@ bool insertPointNormals(const pcl::PointCloud::Ptr& mesh_cloud } else { - ROS_ERROR("what is wrong %ld", idx); + ROS_ERROR("what is wrong %d", idx); } } return true; } +// TODO, why is this here when noether_conversions::convertToPCLMesh(const shape_msgs::Mesh& mesh_msg, pcl::PolygonMesh& mesh) +// returns a pcl::PointCloud<> with mesh.cloud being of type pcl::PointCloud void shapeMsgToPclPointXYZ( const shape_msgs::Mesh& mesh, pcl::PointCloud& mesh_cloud) { mesh_cloud.points.clear(); @@ -1128,7 +1130,7 @@ void computeFaceNormals(const shape_msgs::Mesh& mesh, std::vector& face_normals) +{ + face_normals.clear(); + face_normals.reserve(mesh->polygons.size()); + for(size_t i=0; ipolygons.size(); i++) + { + size_t idx1 = mesh->polygons[i].vertices[0]*mesh->cloud.point_step; + size_t idx2 = mesh->polygons[i].vertices[1]*mesh->cloud.point_step; + size_t idx3 = mesh->polygons[i].vertices[2]*mesh->cloud.point_step; + size_t xo = mesh->cloud.fields[0].offset; + size_t yo = mesh->cloud.fields[1].offset; + size_t zo = mesh->cloud.fields[2].offset; + float* x = (float *)(&mesh->cloud.data[idx1+xo]); + float* y = (float *)(&mesh->cloud.data[idx1+yo]); + float* z = (float *)(&mesh->cloud.data[idx1+zo]); + Eigen::Vector3d v1(*x,*y,*z); + x = (float *)(&mesh->cloud.data[idx2+xo]); + y = (float *)(&mesh->cloud.data[idx2+yo]); + z = (float *)(&mesh->cloud.data[idx2+zo]); + Eigen::Vector3d v2(*x,*y,*z); + x = (float *)(&mesh->cloud.data[idx3+xo]); + y = (float *)(&mesh->cloud.data[idx3+yo]); + z = (float *)(&mesh->cloud.data[idx3+zo]); + Eigen::Vector3d v3(*x,*y,*z); + Eigen::Vector3d normal = v1.cross(v2) + v2.cross(v3) + v3.cross(v1); + normal.normalize(); + face_normals.push_back(normal); + } +} + void computeMeshNormals(const shape_msgs::Mesh& mesh, std::vector& face_normals, std::vector& vertex_normals) { computeFaceNormals(mesh, face_normals); @@ -1153,7 +1185,7 @@ void computeMeshNormals(const shape_msgs::Mesh& mesh, std::vector& face_normals, std::vector& vertex_normals) +{ + computeFaceNormals(mesh, face_normals); + int num_pts = mesh->cloud.height*mesh->cloud.width; + + // populate a structure for each vertex containing a list of adjoining faces + std::vector vertex_faces[num_pts]; + for(size_t i=0; ipolygons.size(); i++) // find every face adjoining each vertex + { + vertex_faces[mesh->polygons[i].vertices[0]].push_back(i); + vertex_faces[mesh->polygons[i].vertices[1]].push_back(i); + vertex_faces[mesh->polygons[i].vertices[2]].push_back(i); + } + // average the adjoining face normals + vertex_normals.clear(); + vertex_normals.reserve(num_pts); + for(size_t i=-0; i& mesh_cloud, std::vector& vertex_normals) { if(mesh_cloud.points.size() != vertex_normals.size()) @@ -1179,7 +1248,6 @@ bool alignToVertexNormals(pcl::PointCloud& mesh_cloud, std::ve ROS_ERROR("mesh_cloud and vertex_normals have different number of points %ld vs %ld", mesh_cloud.points.size(), vertex_normals.size()); return(false); } - int num_flipped=0; for(size_t i=0; i& mesh_cloud, std::ve mesh_cloud.points[i].normal_x = -mesh_cloud.points[i].normal_x; mesh_cloud.points[i].normal_y = -mesh_cloud.points[i].normal_y; mesh_cloud.points[i].normal_z = -mesh_cloud.points[i].normal_z; - num_flipped++; } } return true; From d6a59f18a49efa00b43b2829ba252118ca591d43 Mon Sep 17 00:00:00 2001 From: Chris Lewis Date: Thu, 29 Jul 2021 09:19:52 -0500 Subject: [PATCH 54/64] removed include file no longer necessary --- noether/src/tool_path_planner_server.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/noether/src/tool_path_planner_server.cpp b/noether/src/tool_path_planner_server.cpp index 8d0d8b65..10a5f2c8 100644 --- a/noether/src/tool_path_planner_server.cpp +++ b/noether/src/tool_path_planner_server.cpp @@ -12,7 +12,6 @@ #include #include #include -#include static const double FEEDBACK_PUBLISH_PERIOD = 1.0; // seconds static const std::string GENERATE_TOOL_PATHS_ACTION = "generate_tool_paths"; From 189eef29d25e88196adbfc740a9c8ded34736323 Mon Sep 17 00:00:00 2001 From: Chris Lewis Date: Thu, 29 Jul 2021 10:28:30 -0500 Subject: [PATCH 55/64] clang, removed unnecessary and complex if(), renamed a static function in half-edge code. --- noether/src/tool_path_planner_server.cpp | 29 +- .../tool_path_planner/path_generator.h | 6 +- .../plane_slicer_raster_generator.h | 17 +- .../include/tool_path_planner/utilities.h | 20 +- .../src/halfedge_edge_generator.cpp | 8 +- .../src/plane_slicer_raster_generator.cpp | 197 +++++----- tool_path_planner/src/utilities.cpp | 347 +++++++++--------- 7 files changed, 321 insertions(+), 303 deletions(-) diff --git a/noether/src/tool_path_planner_server.cpp b/noether/src/tool_path_planner_server.cpp index 10a5f2c8..96722a0d 100644 --- a/noether/src/tool_path_planner_server.cpp +++ b/noether/src/tool_path_planner_server.cpp @@ -107,8 +107,8 @@ class TppServer tool_path_planner::toSurfaceWalkConfig(path_config, config.surface_walk_generator); path_gen->setConfiguration(path_config); generator = path_gen; - pt_spacing = path_config.point_spacing; - raster_spacing = path_config.raster_spacing; + pt_spacing = path_config.point_spacing; + raster_spacing = path_config.raster_spacing; } else if (config.type == noether_msgs::ToolPathConfig::PLANE_SLICER_RASTER_GENERATOR) { @@ -117,9 +117,9 @@ class TppServer tool_path_planner::toPlaneSlicerConfig(path_config, config.plane_slicer_generator); path_gen->setConfiguration(path_config); generator = path_gen; - pt_spacing = path_config.point_spacing; - raster_spacing = path_config.raster_spacing; - smooth_pose_arrays_ = path_config.smooth_rasters; + pt_spacing = path_config.point_spacing; + raster_spacing = path_config.raster_spacing; + smooth_pose_arrays_ = path_config.smooth_rasters; } else if (config.type == noether_msgs::ToolPathConfig::EIGEN_VALUE_EDGE_GENERATOR) { @@ -128,7 +128,7 @@ class TppServer tool_path_planner::toEigenValueConfig(path_config, config.eigen_value_generator); path_gen->setConfiguration(path_config); generator = path_gen; - pt_spacing = path_config.merge_dist; + pt_spacing = path_config.merge_dist; } else if (config.type == noether_msgs::ToolPathConfig::HALFEDGE_EDGE_GENERATOR) { @@ -137,7 +137,7 @@ class TppServer tool_path_planner::toHalfedgeConfig(path_config, config.halfedge_generator); path_gen->setConfiguration(path_config); generator = path_gen; - pt_spacing = path_config.point_dist; + pt_spacing = path_config.point_dist; } else { @@ -150,7 +150,7 @@ class TppServer generator->setInput(mesh); tool_paths = generator->generate(); - + if (tool_paths.is_initialized()) { for (const auto& tool_path : tool_paths.get()) @@ -165,14 +165,13 @@ class TppServer tf::poseEigenToMsg(p, pose); seg.poses.push_back(pose); } - if(smooth_pose_arrays_) - { - smooth_pose_traj::SmoothPoseTraj smoother(seg, pt_spacing, true); - smoother.process(seg); - } + if (smooth_pose_arrays_) + { + smooth_pose_traj::SmoothPoseTraj smoother(seg, pt_spacing, true); + smoother.process(seg); + } tp.segments.push_back(seg); } - result.tool_paths[i].paths.push_back(tp); } @@ -258,5 +257,5 @@ int main(int argc, char** argv) tpp_path_gen::TppServer tpp_server(nh, GENERATE_TOOL_PATHS_ACTION); tpp_server.start(); ros::waitForShutdown(); - return 0; + return 0; } diff --git a/tool_path_planner/include/tool_path_planner/path_generator.h b/tool_path_planner/include/tool_path_planner/path_generator.h index 0f25ecbd..910e5cfe 100644 --- a/tool_path_planner/include/tool_path_planner/path_generator.h +++ b/tool_path_planner/include/tool_path_planner/path_generator.h @@ -15,9 +15,9 @@ namespace tool_path_planner { enum RasterStyle : int { - KEEP_ORIENTATION_ON_REVERSE_STROKES = 0, // painting style, back and fourth without flipping - FLIP_ORIENTATION_ON_REVERSE_STROKES = 1, // mowing style, back and fourth, but flip orientation at turns - PROCESS_FORWARD_DIRECTION_ONLY = 2, // reading style left to right and return + KEEP_ORIENTATION_ON_REVERSE_STROKES = 0, // painting style, back and fourth without flipping + FLIP_ORIENTATION_ON_REVERSE_STROKES = 1, // mowing style, back and fourth, but flip orientation at turns + PROCESS_FORWARD_DIRECTION_ONLY = 2, // reading style left to right and return }; /** @brief A set of contiguous waypoints that lie on the same line created by a "slice" through a surface */ 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 2ff8f209..7df14ef3 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 @@ -117,10 +117,11 @@ class PlaneSlicerRasterGenerator : public PathGenerator DEFAULT_MIN_SEGMENT_SIZE; search_radius = validate(jv, "search_radius", Json::ValueType::realValue) ? jv["search_radius"].asDouble() : DEFAULT_SEARCH_RADIUS; - interleave_rasters = validate(jv, "interleave_rasters", Json::ValueType::booleanValue) ? jv["interleave_rasters"].asBool() : - DEFAULT_INTERLEAVE_RASTERS; + interleave_rasters = validate(jv, "interleave_rasters", Json::ValueType::booleanValue) ? + jv["interleave_rasters"].asBool() : + DEFAULT_INTERLEAVE_RASTERS; smooth_rasters = validate(jv, "smooth_rasters", Json::ValueType::booleanValue) ? jv["smooth_rasters"].asBool() : - DEFAULT_SMOOTH_RASTERS; + DEFAULT_SMOOTH_RASTERS; 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) ? @@ -202,14 +203,18 @@ class PlaneSlicerRasterGenerator : public PathGenerator std::string getName() const override; private: - void setInput(vtkSmartPointer mesh) override; bool insertNormals(const double search_radius, vtkSmartPointer& data); tool_path_planner::ToolPaths convertToPoses(const std::vector& rasters_data); - void computePoseData(const PolyDataPtr& polydata, int idx, Eigen::Vector3d& p, Eigen::Vector3d& vx, Eigen::Vector3d& vy, Eigen::Vector3d& vz); - + void computePoseData(const PolyDataPtr& polydata, + int idx, + Eigen::Vector3d& p, + Eigen::Vector3d& vx, + Eigen::Vector3d& vy, + Eigen::Vector3d& vz); + vtkSmartPointer mesh_data_; vtkSmartPointer kd_tree_; vtkSmartPointer cell_locator_; diff --git a/tool_path_planner/include/tool_path_planner/utilities.h b/tool_path_planner/include/tool_path_planner/utilities.h index 6e8751c7..59ffb071 100644 --- a/tool_path_planner/include/tool_path_planner/utilities.h +++ b/tool_path_planner/include/tool_path_planner/utilities.h @@ -161,23 +161,27 @@ ToolPaths splitByAxes(const ToolPaths& tool_paths, const Eigen::Vector3f& axis_1 */ void InterleavePoseTraj(noether_msgs::ToolPaths& tool_paths, double raster_spacing); -pcl::PointCloud computeMLSMeshNormals(const pcl::PointCloud::Ptr& mesh_cloud, double normal_search_radius); - +pcl::PointCloud computeMLSMeshNormals(const pcl::PointCloud::Ptr& mesh_cloud, + double normal_search_radius); + bool applyEqualDistance(const std::vector& pnt_indices, - const pcl::PointCloud& mesh_cloud, + const pcl::PointCloud& mesh_cloud, pcl::PointCloud& path_cloud, double dist); bool insertPointNormals(const pcl::PointCloud::Ptr& mesh_cloud, - pcl::PointCloud& path_cloud); - -void shapeMsgToPclPointXYZ( const shape_msgs::Mesh& mesh, pcl::PointCloud& mesh_cloud); + pcl::PointCloud& path_cloud); +void shapeMsgToPclPointXYZ(const shape_msgs::Mesh& mesh, pcl::PointCloud& mesh_cloud); void computeFaceNormals(const shape_msgs::Mesh& mesh, std::vector& face_normals); -void computeMeshNormals(const shape_msgs::Mesh& mesh, std::vector& face_normals, std::vector& vertex_normals); -void computePCLMeshNormals(pcl::PolygonMesh::ConstPtr& mesh, std::vector& face_normals, std::vector& vertex_normals); +void computeMeshNormals(const shape_msgs::Mesh& mesh, + std::vector& face_normals, + std::vector& vertex_normals); +void computePCLMeshNormals(pcl::PolygonMesh::ConstPtr& mesh, + std::vector& face_normals, + std::vector& vertex_normals); bool alignToVertexNormals(pcl::PointCloud& mesh_cloud, std::vector& vertex_normals); } // namespace tool_path_planner diff --git a/tool_path_planner/src/halfedge_edge_generator.cpp b/tool_path_planner/src/halfedge_edge_generator.cpp index a0b80205..57e790b4 100644 --- a/tool_path_planner/src/halfedge_edge_generator.cpp +++ b/tool_path_planner/src/halfedge_edge_generator.cpp @@ -155,9 +155,9 @@ bool applyMinPointDistance(const pcl::PointCloud& in, * @param dist The required point spacing distance * @return True on success, False otherwise */ -bool applyEqualDistance1(const pcl::PointCloud& in, - pcl::PointCloud& out, - double dist) +static bool applyEqualDistancePathCloud(const pcl::PointCloud& in, + pcl::PointCloud& out, + double dist) { using namespace pcl; using namespace Eigen; @@ -500,7 +500,7 @@ boost::optional HalfedgeEdgeGenerator::generate() break; case PointSpacingMethod::EQUAL_SPACING: - if (!applyEqualDistance1(bound_segment_points, decimated_points, config_.point_dist)) + if (!applyEqualDistancePathCloud(bound_segment_points, decimated_points, config_.point_dist)) { CONSOLE_BRIDGE_logError("applyEqualDistance1 point spacing method failed"); return boost::none; diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index 2653a5f2..cfcb17ea 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -66,7 +66,7 @@ static Eigen::Vector3d getSegDir(PolyDataPtr seg) v.x() = 1.0; v.y() = 0.0; v.z() = 0.0; - return(v); + return (v); } Eigen::Vector3d seg_start, seg_end; seg->GetPoint(0, seg_start.data()); @@ -81,7 +81,9 @@ static bool compare_ds_pair(std::pair& first, std::pairGetPoints()->GetNumberOfPoints() <= 1) { @@ -119,10 +121,10 @@ static tool_path_planner::PlaneSlicerRasterGenerator::RasterConstructData alignR seg_order.push_back(p); } // sort the segments by location - if(seg_order.size()>=1) - { - seg_order.sort(compare_ds_pair); - } + if (seg_order.size() >= 1) + { + seg_order.sort(compare_ds_pair); + } tool_path_planner::PlaneSlicerRasterGenerator::RasterConstructData new_rcd; for (std::pair p : seg_order) { @@ -391,7 +393,7 @@ static void rectifyDirection(const vtkSmartPointer& points, bool reverse = (ref_point - p0).norm() > (ref_point - pf).norm(); if (reverse) { - for (auto& s : points_lists) // reverse points in segments + for (auto& s : points_lists) // reverse points in segments { std::reverse(s.begin(), s.end()); } @@ -399,7 +401,6 @@ static void rectifyDirection(const vtkSmartPointer& points, } } - namespace tool_path_planner { void PlaneSlicerRasterGenerator::setConfiguration(const PlaneSlicerRasterGenerator::Config& config) @@ -444,10 +445,8 @@ void PlaneSlicerRasterGenerator::setInput(vtkSmartPointer mesh) } } - void PlaneSlicerRasterGenerator::setInput(pcl::PolygonMesh::ConstPtr mesh) { - auto mesh_data = vtkSmartPointer::New(); pcl::VTKUtils::mesh2vtk(*mesh, mesh_data); mesh_data->BuildLinks(); @@ -455,23 +454,22 @@ void PlaneSlicerRasterGenerator::setInput(pcl::PolygonMesh::ConstPtr mesh) // compute face and vertex using polygon info tool_path_planner::computePCLMeshNormals(mesh, face_normals_, vertex_normals_); - + // compute vertex normals using Moving Least Squares - pcl::PointCloud::Ptr mesh_cloud_ptr(new pcl::PointCloud() ); + pcl::PointCloud::Ptr mesh_cloud_ptr(new pcl::PointCloud()); pcl::fromPCLPointCloud2(mesh->cloud, *mesh_cloud_ptr); - mls_mesh_normals_ptr_ = boost::make_shared > (tool_path_planner::computeMLSMeshNormals(mesh_cloud_ptr, config_.search_radius)); - + mls_mesh_normals_ptr_ = boost::make_shared >( + tool_path_planner::computeMLSMeshNormals(mesh_cloud_ptr, config_.search_radius)); // align mls_vertex_normals to vertex_normals - if(!tool_path_planner::alignToVertexNormals(*mls_mesh_normals_ptr_, vertex_normals_)) - { - ROS_ERROR("alignToVertexNormals failed"); - } + if (!tool_path_planner::alignToVertexNormals(*mls_mesh_normals_ptr_, vertex_normals_)) + { + ROS_ERROR("alignToVertexNormals failed"); + } setInput(mesh_data); } - void PlaneSlicerRasterGenerator::setInput(const shape_msgs::Mesh& mesh) { pcl::PolygonMesh::Ptr pcl_mesh = boost::make_shared(); @@ -480,24 +478,30 @@ void PlaneSlicerRasterGenerator::setInput(const shape_msgs::Mesh& mesh) } vtkSmartPointer PlaneSlicerRasterGenerator::getInput() { return mesh_data_; } -void PlaneSlicerRasterGenerator::computePoseData(const PolyDataPtr& polydata, int idx, Eigen::Vector3d& p, Eigen::Vector3d& vx, Eigen::Vector3d& vy, Eigen::Vector3d& vz) +void PlaneSlicerRasterGenerator::computePoseData(const PolyDataPtr& polydata, + int idx, + Eigen::Vector3d& p, + Eigen::Vector3d& vx, + Eigen::Vector3d& vy, + Eigen::Vector3d& vz) { Eigen::Vector3d p_next; Eigen::Vector3d p_start; - + // polydata->GetPoint(idx, p.data()); // use this and next point to determine x direction // polydata->GetPoint(idx + 1, p_next.data()); - polydata->GetPoint(0, p_start.data()); // use the first and last point to determine x direction + polydata->GetPoint(0, p_start.data()); // use the first and last point to determine x direction polydata->GetPoint(idx, p.data()); - polydata->GetPoint(polydata->GetNumberOfPoints()-1, p_next.data()); + polydata->GetPoint(polydata->GetNumberOfPoints() - 1, p_next.data()); polydata->GetPointData()->GetNormals()->GetTuple(idx, vz.data()); vz = vz.normalized(); vx = p_next - p_start; - vx = (vx - vx.dot(vz)*vz).normalized(); + vx = (vx - vx.dot(vz) * vz).normalized(); vy = vz.cross(vx).normalized(); } -tool_path_planner::ToolPaths PlaneSlicerRasterGenerator::convertToPoses(const std::vector& rasters_data) +tool_path_planner::ToolPaths PlaneSlicerRasterGenerator::convertToPoses( + const std::vector& rasters_data) { using namespace Eigen; tool_path_planner::ToolPaths rasters_array; @@ -526,39 +530,39 @@ tool_path_planner::ToolPaths PlaneSlicerRasterGenerator::convertToPoses(const st prev_vx = vx; prev_vy = vy; prev_vz = vz; - int q=0; + int q = 0; for (std::size_t i = 0; i < indices.size() - 2; i++) { - computePoseData(polydata, i+1, next_p, next_vx, next_vy, next_vz); - vx = (prev_vx + vx + next_vx).normalized(); + computePoseData(polydata, i + 1, next_p, next_vx, next_vy, next_vz); + vx = (prev_vx + vx + next_vx).normalized(); vy = vz.cross(vx).normalized(); pose = Translation3d(p) * AngleAxisd(computeRotation(vx, vy, vz)); raster_path_segment.push_back(pose); - q++; - - prev_vx = vx; - prev_vy = vy; - prev_vz = vz; - vx = next_vx; - vy = next_vy; - vz = next_vz; - p = next_p; + q++; + + prev_vx = vx; + prev_vy = vy; + prev_vz = vz; + vx = next_vx; + vy = next_vy; + vz = next_vz; + p = next_p; } // end for every waypoint - if(indices.size() >= 2) // this throws away short segments + if (indices.size() >= 2) // this throws away short segments { - // adding next to last pose - computePoseData(polydata, indices.size()-2, p, vx, vy, vz); - vx = (prev_vx + vx).normalized(); - pose = Translation3d(p) * AngleAxisd(computeRotation(vx, vy, vz)); - raster_path_segment.push_back(pose); - q++; - - // adding last pose - polydata->GetPoint(indices.size()-1, p.data()); - pose = Translation3d(p) * AngleAxisd(computeRotation(vx, vy, vz)); - raster_path_segment.push_back(pose); - q++; + // adding next to last pose + computePoseData(polydata, indices.size() - 2, p, vx, vy, vz); + vx = (prev_vx + vx).normalized(); + pose = Translation3d(p) * AngleAxisd(computeRotation(vx, vy, vz)); + raster_path_segment.push_back(pose); + q++; + + // adding last pose + polydata->GetPoint(indices.size() - 1, p.data()); + pose = Translation3d(p) * AngleAxisd(computeRotation(vx, vy, vz)); + raster_path_segment.push_back(pose); + q++; } raster_path.push_back(raster_path_segment); } // end for every segment @@ -568,7 +572,6 @@ tool_path_planner::ToolPaths PlaneSlicerRasterGenerator::convertToPoses(const st return rasters_array; } - boost::optional PlaneSlicerRasterGenerator::generate() { using namespace Eigen; @@ -854,23 +857,23 @@ boost::optional PlaneSlicerRasterGenerator::generate() } // saving into raster - if(segment_data->GetPoints()->GetNumberOfPoints()>0) - { - r.raster_segments.push_back(segment_data); - r.segment_lengths.push_back(line_length); - } + if (segment_data->GetPoints()->GetNumberOfPoints() > 0) + { + r.raster_segments.push_back(segment_data); + r.segment_lengths.push_back(line_length); + } } } - if(r.raster_segments.size()>0) + if (r.raster_segments.size() > 0) rasters_data_vec.push_back(r); } - if(rasters_data_vec.size() == 0) - { - ROS_ERROR("no rasters found"); - ToolPaths rasters; - return(rasters); - } + if (rasters_data_vec.size() == 0) + { + ROS_ERROR("no rasters found"); + ToolPaths rasters; + return (rasters); + } // make sure every raster has its segments ordered and aligned correctly Eigen::Vector3d raster_direction = getSegDir(rasters_data_vec[0].raster_segments[0]); for (tool_path_planner::PlaneSlicerRasterGenerator::RasterConstructData rcd : rasters_data_vec) @@ -878,26 +881,26 @@ boost::optional PlaneSlicerRasterGenerator::generate() rcd = alignRasterCD(rcd, raster_direction); } - if(config_.interleave_rasters) + if (config_.interleave_rasters) + { + std::vector tmp_rasters_data_vec; + // evens + for (size_t i = 0; i < rasters_data_vec.size(); i += 2) + { + tmp_rasters_data_vec.push_back(rasters_data_vec[i]); + } + // odds + for (size_t i = 1; i < rasters_data_vec.size(); i += 2) + { + tmp_rasters_data_vec.push_back(rasters_data_vec[i]); + } + // clear and copy new order + rasters_data_vec.clear(); + for (size_t i = 0; i < tmp_rasters_data_vec.size(); i++) { - std::vector tmp_rasters_data_vec; - // evens - for (size_t i=0; i PlaneSlicerRasterGenerator::generate() rasters = addExtraWaypoints(rasters, config_.raster_spacing, config_.point_spacing); } - if((config_.raster_style != PROCESS_FORWARD_DIRECTION_ONLY)) - { - // switch directions of every other raster, using either flipping orientation or not depending on style selected - rasters = reverseOddRasters(rasters, config_.raster_style); - } + if ((config_.raster_style != PROCESS_FORWARD_DIRECTION_ONLY)) + { + // switch directions of every other raster, using either flipping orientation or not depending on style selected + rasters = reverseOddRasters(rasters, config_.raster_style); + } return rasters; } @@ -940,28 +943,28 @@ bool PlaneSlicerRasterGenerator::insertNormals(const double search_radius, vtkSm data->GetPoints()->GetPoint(i, query_point.data()); kd_tree_->FindClosestNPoints(1, query_point.data(), id_list); if (id_list->GetNumberOfIds() < 1) - { - CONSOLE_BRIDGE_logError("%s failed to find closest for normal computation", getName().c_str()); - return false; - } + { + CONSOLE_BRIDGE_logError("%s failed to find closest for normal computation", getName().c_str()); + return false; + } // compute normal average normal_vect = Eigen::Vector3d::Zero(); vtkIdType p_id = id_list->GetId(0); if (p_id < 0) - { - CONSOLE_BRIDGE_logError("%s point id is invalid", getName().c_str()); - continue; - } - + { + CONSOLE_BRIDGE_logError("%s point id is invalid", getName().c_str()); + continue; + } + normal_vect.x() = mls_mesh_normals_ptr_->points[p_id].normal_x; normal_vect.y() = mls_mesh_normals_ptr_->points[p_id].normal_y; normal_vect.z() = mls_mesh_normals_ptr_->points[p_id].normal_z; normal_vect.normalize(); - + // save normal new_normals->SetTuple3(i, normal_vect(0), normal_vect(1), normal_vect(2)); - }// end for every point + } // end for every point data->GetPointData()->SetNormals(new_normals); return true; diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index 7ab85078..ad9d532c 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -47,7 +47,7 @@ typedef std::list tlist; typedef std::list::iterator tlist_it; static const double EPSILON = 1e-3; - + void flipPointOrder(ToolPath& path) { // Reverse the order of the segments @@ -200,7 +200,7 @@ bool toPlaneSlicerConfigMsg(noether_msgs::PlaneSlicerRasterGeneratorConfig& conf config_msg.smooth_rasters = config.smooth_rasters; config_msg.raster_style = config.raster_style; config_msg.generate_extra_rasters = config.generate_extra_rasters; - config_msg.raster_wrt_global_axes = config.raster_wrt_global_axes; + 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(); @@ -293,37 +293,37 @@ bool createToolPathSegment(const pcl::PointCloud& cloud_normal Vector3d x_dir, z_dir, y_dir; std::vector cloud_indices; if (indices.empty()) + { + cloud_indices.reserve(cloud_normals.size()); + for (int i = 0; i < cloud_normals.points.size(); i++) { - cloud_indices.reserve(cloud_normals.size()); - for(int i=0; i(); z_dir = Vector3d(p1.normal_x, p1.normal_y, p1.normal_z).normalized(); - x_dir = (x_dir - x_dir.dot(z_dir)*z_dir).normalized();// remove component of x along z + x_dir = (x_dir - x_dir.dot(z_dir) * z_dir).normalized(); // remove component of x along z y_dir = z_dir.cross(x_dir).normalized(); - + pose = Translation3d(p1.getVector3fMap().cast()); pose.matrix().block<3, 3>(0, 0) = tool_path_planner::toRotationMatrix(x_dir, y_dir, z_dir); segment.push_back(pose); - + for (std::size_t i = 1; i < cloud_indices.size() - 2; i++) { std::size_t idx_prev = cloud_indices[i - 1]; @@ -340,7 +340,7 @@ bool createToolPathSegment(const pcl::PointCloud& cloud_normal const PointNormal& p2 = cloud_normals[idx_next]; x_dir = (p2.getVector3fMap() - p0.getVector3fMap()).normalized().cast(); z_dir = Vector3d(p1.normal_x, p1.normal_y, p1.normal_z).normalized(); - x_dir = (x_dir - x_dir.dot(z_dir)*z_dir).normalized();// remove component of x along z + x_dir = (x_dir - x_dir.dot(z_dir) * z_dir).normalized(); // remove component of x along z y_dir = z_dir.cross(x_dir).normalized(); pose = Translation3d(p1.getVector3fMap().cast()); @@ -349,9 +349,9 @@ bool createToolPathSegment(const pcl::PointCloud& cloud_normal } // compute last pose with last computed x-axis (corrected for change in z - const PointNormal& p_last = cloud_normals[cloud_indices[cloud_indices.size() -1]]; + const PointNormal& p_last = cloud_normals[cloud_indices[cloud_indices.size() - 1]]; z_dir = Vector3d(p_last.normal_x, p_last.normal_y, p_last.normal_z).normalized(); - x_dir = (x_dir - x_dir.dot(z_dir)*z_dir).normalized(); + x_dir = (x_dir - x_dir.dot(z_dir) * z_dir).normalized(); y_dir = z_dir.cross(x_dir).normalized(); pose = Translation3d(p_last.getVector3fMap().cast()); pose.matrix().block<3, 3>(0, 0) = tool_path_planner::toRotationMatrix(x_dir, y_dir, z_dir); @@ -934,13 +934,14 @@ ToolPaths addExtraWaypoints(const ToolPaths& tool_paths, double raster_spacing, } // computes the normal vectors to a mesh at every vertex -pcl::PointCloud computeMLSMeshNormals(const pcl::PointCloud::Ptr& mesh_cloud, double normal_search_radius) +pcl::PointCloud computeMLSMeshNormals(const pcl::PointCloud::Ptr& mesh_cloud, + double normal_search_radius) { // initialize the MLSNE object pcl::MovingLeastSquares MLS; MLS.setInputCloud(mesh_cloud); MLS.setComputeNormals(true); - pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); MLS.setSearchMethod(tree); MLS.setPolynomialOrder(3); MLS.setSearchRadius(normal_search_radius); @@ -955,21 +956,21 @@ pcl::PointCloud computeMLSMeshNormals(const pcl::PointCloud mls_mesh_cloud_normals; MLS.process(mls_mesh_cloud_normals); std::vector R = MLS.getMLSResults(); - + pcl::PointCloud mesh_cloud_normals; mesh_cloud_normals.reserve(mesh_cloud->points.size()); - for(size_t i=0; ipoints.size(); i++) - { - pcl::PointNormal new_pn; - new_pn.x = mesh_cloud->points[i].x; - new_pn.y = mesh_cloud->points[i].y; - new_pn.z = mesh_cloud->points[i].z; - new_pn.normal_x = -R[i].plane_normal.x(); - new_pn.normal_y = -R[i].plane_normal.y(); - new_pn.normal_z = -R[i].plane_normal.z(); - mesh_cloud_normals.push_back(new_pn); - } - return(mesh_cloud_normals); + for (size_t i = 0; i < mesh_cloud->points.size(); i++) + { + pcl::PointNormal new_pn; + new_pn.x = mesh_cloud->points[i].x; + new_pn.y = mesh_cloud->points[i].y; + new_pn.z = mesh_cloud->points[i].z; + new_pn.normal_x = -R[i].plane_normal.x(); + new_pn.normal_y = -R[i].plane_normal.y(); + new_pn.normal_z = -R[i].plane_normal.z(); + mesh_cloud_normals.push_back(new_pn); + } + return (mesh_cloud_normals); } /** @@ -980,7 +981,7 @@ pcl::PointCloud computeMLSMeshNormals(const pcl::PointCloud& pnt_indices, - const pcl::PointCloud& mesh_cloud, + const pcl::PointCloud& mesh_cloud, pcl::PointCloud& path_cloud, double dist) { @@ -990,11 +991,11 @@ bool applyEqualDistance(const std::vector& pnt_indices, PointNormal new_pt; PointXYZ p_start, p_mid, p_end; Vector3f v_1, v_2, unitv_2, v_next; - if(pnt_indices.size()<3) - { - CONSOLE_BRIDGE_logError("pnt_indices must have at least 3"); - return false; - } + if (pnt_indices.size() < 3) + { + CONSOLE_BRIDGE_logError("pnt_indices must have at least 3"); + return false; + } PointXYZ pt = mesh_cloud.points[pnt_indices[0]]; p_start.x = pt.x; p_start.y = pt.y; @@ -1050,11 +1051,11 @@ bool applyEqualDistance(const std::vector& pnt_indices, } // add last point if not already there - if ((path_cloud.back().getVector3fMap() - mesh_cloud[pnt_indices.back()].getVector3fMap()).norm() > dist/3.0) + if ((path_cloud.back().getVector3fMap() - mesh_cloud[pnt_indices.back()].getVector3fMap()).norm() > dist / 3.0) { - new_pt.x = mesh_cloud[pnt_indices[pnt_indices.size()-1]].x; - new_pt.y = mesh_cloud[pnt_indices[pnt_indices.size()-1]].y; - new_pt.z = mesh_cloud[pnt_indices[pnt_indices.size()-1]].z; + new_pt.x = mesh_cloud[pnt_indices[pnt_indices.size() - 1]].x; + new_pt.y = mesh_cloud[pnt_indices[pnt_indices.size() - 1]].y; + new_pt.z = mesh_cloud[pnt_indices[pnt_indices.size() - 1]].z; path_cloud.push_back(new_pt); } @@ -1068,15 +1069,15 @@ bool applyEqualDistance(const std::vector& pnt_indices, } bool insertPointNormals(const pcl::PointCloud::Ptr& mesh_cloud, - pcl::PointCloud& path_cloud) + pcl::PointCloud& path_cloud) { - //recovering normals using kd-treek - pcl::PointCloud::Ptr in_points (new pcl::PointCloud ()); - for(pcl::PointNormal p : mesh_cloud->points) - { - pcl::PointXYZ pt(p.x, p.y,p.z); - in_points->points.push_back(pt); - } + // recovering normals using kd-treek + pcl::PointCloud::Ptr in_points(new pcl::PointCloud()); + for (pcl::PointNormal p : mesh_cloud->points) + { + pcl::PointXYZ pt(p.x, p.y, p.z); + in_points->points.push_back(pt); + } pcl::KdTreeFLANN kdtree; kdtree.setInputCloud(in_points); kdtree.setEpsilon(EPSILON); @@ -1096,168 +1097,174 @@ bool insertPointNormals(const pcl::PointCloud::Ptr& mesh_cloud } int idx = k_indices[0]; - if(idx < mesh_cloud->points.size()) - { - std::tie(p.normal_x, p.normal_y, p.normal_z) = - std::make_tuple(mesh_cloud->points[idx].normal_x, mesh_cloud->points[idx].normal_y, mesh_cloud->points[idx].normal_z); - } - else - { - ROS_ERROR("what is wrong %d", idx); - } + if (idx < mesh_cloud->points.size()) + { + std::tie(p.normal_x, p.normal_y, p.normal_z) = std::make_tuple( + mesh_cloud->points[idx].normal_x, mesh_cloud->points[idx].normal_y, mesh_cloud->points[idx].normal_z); + } + else + { + ROS_ERROR("what is wrong %d", idx); + } } return true; } -// TODO, why is this here when noether_conversions::convertToPCLMesh(const shape_msgs::Mesh& mesh_msg, pcl::PolygonMesh& mesh) -// returns a pcl::PointCloud<> with mesh.cloud being of type pcl::PointCloud -void shapeMsgToPclPointXYZ( const shape_msgs::Mesh& mesh, pcl::PointCloud& mesh_cloud) +// TODO, why is this here when noether_conversions::convertToPCLMesh(const shape_msgs::Mesh& mesh_msg, pcl::PolygonMesh& +// mesh) returns a pcl::PointCloud<> with mesh.cloud being of type pcl::PointCloud +void shapeMsgToPclPointXYZ(const shape_msgs::Mesh& mesh, pcl::PointCloud& mesh_cloud) { mesh_cloud.points.clear(); - for(int i=0; i& face_normals) { face_normals.clear(); face_normals.reserve(mesh.triangles.size()); - for(size_t i=0; i& face_normals) { face_normals.clear(); face_normals.reserve(mesh->polygons.size()); - for(size_t i=0; ipolygons.size(); i++) - { - size_t idx1 = mesh->polygons[i].vertices[0]*mesh->cloud.point_step; - size_t idx2 = mesh->polygons[i].vertices[1]*mesh->cloud.point_step; - size_t idx3 = mesh->polygons[i].vertices[2]*mesh->cloud.point_step; - size_t xo = mesh->cloud.fields[0].offset; - size_t yo = mesh->cloud.fields[1].offset; - size_t zo = mesh->cloud.fields[2].offset; - float* x = (float *)(&mesh->cloud.data[idx1+xo]); - float* y = (float *)(&mesh->cloud.data[idx1+yo]); - float* z = (float *)(&mesh->cloud.data[idx1+zo]); - Eigen::Vector3d v1(*x,*y,*z); - x = (float *)(&mesh->cloud.data[idx2+xo]); - y = (float *)(&mesh->cloud.data[idx2+yo]); - z = (float *)(&mesh->cloud.data[idx2+zo]); - Eigen::Vector3d v2(*x,*y,*z); - x = (float *)(&mesh->cloud.data[idx3+xo]); - y = (float *)(&mesh->cloud.data[idx3+yo]); - z = (float *)(&mesh->cloud.data[idx3+zo]); - Eigen::Vector3d v3(*x,*y,*z); - Eigen::Vector3d normal = v1.cross(v2) + v2.cross(v3) + v3.cross(v1); - normal.normalize(); - face_normals.push_back(normal); - } + for (size_t i = 0; i < mesh->polygons.size(); i++) + { + size_t idx1 = mesh->polygons[i].vertices[0] * mesh->cloud.point_step; + size_t idx2 = mesh->polygons[i].vertices[1] * mesh->cloud.point_step; + size_t idx3 = mesh->polygons[i].vertices[2] * mesh->cloud.point_step; + size_t xo = mesh->cloud.fields[0].offset; + size_t yo = mesh->cloud.fields[1].offset; + size_t zo = mesh->cloud.fields[2].offset; + float* x = (float*)(&mesh->cloud.data[idx1 + xo]); + float* y = (float*)(&mesh->cloud.data[idx1 + yo]); + float* z = (float*)(&mesh->cloud.data[idx1 + zo]); + Eigen::Vector3d v1(*x, *y, *z); + x = (float*)(&mesh->cloud.data[idx2 + xo]); + y = (float*)(&mesh->cloud.data[idx2 + yo]); + z = (float*)(&mesh->cloud.data[idx2 + zo]); + Eigen::Vector3d v2(*x, *y, *z); + x = (float*)(&mesh->cloud.data[idx3 + xo]); + y = (float*)(&mesh->cloud.data[idx3 + yo]); + z = (float*)(&mesh->cloud.data[idx3 + zo]); + Eigen::Vector3d v3(*x, *y, *z); + Eigen::Vector3d normal = v1.cross(v2) + v2.cross(v3) + v3.cross(v1); + normal.normalize(); + face_normals.push_back(normal); + } } -void computeMeshNormals(const shape_msgs::Mesh& mesh, std::vector& face_normals, std::vector& vertex_normals) +void computeMeshNormals(const shape_msgs::Mesh& mesh, + std::vector& face_normals, + std::vector& vertex_normals) { computeFaceNormals(mesh, face_normals); // populate a structure for each vertex containing a list of adjoining faces std::vector vertex_faces[mesh.vertices.size()]; - for(size_t i=0; i& face_normals, std::vector& vertex_normals) +void computePCLMeshNormals(pcl::PolygonMesh::ConstPtr& mesh, + std::vector& face_normals, + std::vector& vertex_normals) { computeFaceNormals(mesh, face_normals); - int num_pts = mesh->cloud.height*mesh->cloud.width; + int num_pts = mesh->cloud.height * mesh->cloud.width; // populate a structure for each vertex containing a list of adjoining faces std::vector vertex_faces[num_pts]; - for(size_t i=0; ipolygons.size(); i++) // find every face adjoining each vertex - { - vertex_faces[mesh->polygons[i].vertices[0]].push_back(i); - vertex_faces[mesh->polygons[i].vertices[1]].push_back(i); - vertex_faces[mesh->polygons[i].vertices[2]].push_back(i); - } + for (size_t i = 0; i < mesh->polygons.size(); i++) // find every face adjoining each vertex + { + vertex_faces[mesh->polygons[i].vertices[0]].push_back(i); + vertex_faces[mesh->polygons[i].vertices[1]].push_back(i); + vertex_faces[mesh->polygons[i].vertices[2]].push_back(i); + } // average the adjoining face normals vertex_normals.clear(); vertex_normals.reserve(num_pts); - for(size_t i=-0; i& mesh_cloud, std::vector& vertex_normals) { - if(mesh_cloud.points.size() != vertex_normals.size()) - { - ROS_ERROR("mesh_cloud and vertex_normals have different number of points %ld vs %ld", mesh_cloud.points.size(), vertex_normals.size()); - return(false); - } - for(size_t i=0; i Date: Thu, 29 Jul 2021 10:33:52 -0500 Subject: [PATCH 56/64] text of error message corrected --- tool_path_planner/src/halfedge_edge_generator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tool_path_planner/src/halfedge_edge_generator.cpp b/tool_path_planner/src/halfedge_edge_generator.cpp index 57e790b4..1854c490 100644 --- a/tool_path_planner/src/halfedge_edge_generator.cpp +++ b/tool_path_planner/src/halfedge_edge_generator.cpp @@ -502,7 +502,7 @@ boost::optional HalfedgeEdgeGenerator::generate() case PointSpacingMethod::EQUAL_SPACING: if (!applyEqualDistancePathCloud(bound_segment_points, decimated_points, config_.point_dist)) { - CONSOLE_BRIDGE_logError("applyEqualDistance1 point spacing method failed"); + CONSOLE_BRIDGE_logError("applyEqualDistancePathCloud point spacing method failed"); return boost::none; } break; From 4f0a139cbf4cd57e2d26046f5927d28278a5ef4d Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Tue, 1 Feb 2022 10:34:44 -0600 Subject: [PATCH 57/64] Fixed extra raster_directions param that slipped through on rebase --- .../include/tool_path_planner/plane_slicer_raster_generator.h | 1 - 1 file changed, 1 deletion(-) 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 7df14ef3..438bf798 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 @@ -63,7 +63,6 @@ class PlaneSlicerRasterGenerator : public PathGenerator bool interleave_rasters{ DEFAULT_INTERLEAVE_RASTERS }; bool smooth_rasters{ DEFAULT_SMOOTH_RASTERS }; bool raster_wrt_global_axes{ DEFAULT_RASTER_WRT_GLOBAL_AXES }; - Eigen::Vector3d raster_direction{ Eigen::Vector3d::Zero() }; bool generate_extra_rasters{ DEFAULT_GENERATE_EXTRA_RASTERS }; Eigen::Vector3d raster_direction{ Eigen::Vector3d::UnitY() }; RasterStyle raster_style{ DEFAULT_RASTER_STYLE }; From 74246a12916f95b74b21e145c7f68f9b74dcdf89 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Mon, 7 Feb 2022 15:36:19 -0600 Subject: [PATCH 58/64] Fixed default raster direction to be zero vector rather than unit Y so that it defaults to longest side of bounding box --- .../include/tool_path_planner/plane_slicer_raster_generator.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 438bf798..a0f2fa14 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 @@ -64,7 +64,7 @@ class PlaneSlicerRasterGenerator : public PathGenerator bool smooth_rasters{ DEFAULT_SMOOTH_RASTERS }; bool raster_wrt_global_axes{ DEFAULT_RASTER_WRT_GLOBAL_AXES }; bool generate_extra_rasters{ DEFAULT_GENERATE_EXTRA_RASTERS }; - Eigen::Vector3d raster_direction{ Eigen::Vector3d::UnitY() }; + Eigen::Vector3d raster_direction{ Eigen::Vector3d::Zero() }; RasterStyle raster_style{ DEFAULT_RASTER_STYLE }; Json::Value toJson() const { From 5899b4d9390fecd05649146e8d0347da9b7ea9d8 Mon Sep 17 00:00:00 2001 From: "David Merz, Jr" Date: Tue, 8 Mar 2022 07:50:41 -0600 Subject: [PATCH 59/64] Reorganizing and Missing Headers --- .../plane_slicer_raster_generator.h | 34 +++++------ .../src/plane_slicer_raster_generator.cpp | 57 ++++++++++--------- 2 files changed, 47 insertions(+), 44 deletions(-) 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 a0f2fa14..604f112f 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 @@ -22,17 +22,21 @@ #ifndef INCLUDE_PLANE_SLICER_RASTER_GENERATOR_H_ #define INCLUDE_PLANE_SLICER_RASTER_GENERATOR_H_ -#include -#include -#include #include #include +#include +#include +#include #include -#include #include #include -#include -#include +#include +#include +#include + +#include +#include + #include namespace tool_path_planner @@ -91,14 +95,14 @@ class PlaneSlicerRasterGenerator : public PathGenerator { if (jv.isNull()) { - ROS_ERROR("Json value is null"); + CONSOLE_BRIDGE_logError("Json value is null"); return false; } if (jv.type() != Json::ValueType::objectValue) { - ROS_ERROR("Json type %i is invalid, only '%i' is allowed", - static_cast(jv.type()), - static_cast(Json::ValueType::objectValue)); + CONSOLE_BRIDGE_logError("Json type %i is invalid, only '%i' is allowed", + static_cast(jv.type()), + static_cast(Json::ValueType::objectValue)); return false; } auto validate = [](const Json::Value& jv, const std::string& name_, const Json::ValueType& type_) -> bool { @@ -128,7 +132,7 @@ class PlaneSlicerRasterGenerator : public PathGenerator 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"); + CONSOLE_BRIDGE_logError("Malformed Raster Direction in Json value"); return false; } if (validate(jv["raster_direction"], "x", Json::ValueType::objectValue) && @@ -175,11 +179,9 @@ class PlaneSlicerRasterGenerator : public PathGenerator } }; - using PolyDataPtr = vtkSmartPointer; - struct RasterConstructData { - std::vector raster_segments; + std::vector> raster_segments; std::vector segment_lengths; }; @@ -207,7 +209,7 @@ class PlaneSlicerRasterGenerator : public PathGenerator bool insertNormals(const double search_radius, vtkSmartPointer& data); tool_path_planner::ToolPaths convertToPoses(const std::vector& rasters_data); - void computePoseData(const PolyDataPtr& polydata, + void computePoseData(const vtkSmartPointer& polydata, int idx, Eigen::Vector3d& p, Eigen::Vector3d& vx, @@ -218,7 +220,7 @@ class PlaneSlicerRasterGenerator : public PathGenerator vtkSmartPointer kd_tree_; vtkSmartPointer cell_locator_; Config config_; - boost::shared_ptr > mls_mesh_normals_ptr_; + boost::shared_ptr> mls_mesh_normals_ptr_; std::vector vertex_normals_; std::vector face_normals_; }; diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index cfcb17ea..f7a7c282 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -19,49 +19,50 @@ * limitations under the License. */ -#include +#include + +#include + +#include +#include +#include +#include #include +#include #include #include #include #include +#include +#include +#include +#include +#include +#include +#include #include #include #include #include -#include -#include +#include +#include #include #include -#include -#include -#include -#include #include -#include -#include -#include -#include -#include -#include -#include #include -#include #include + #include #include -#include static const double EPSILON = 1e-6; -using PolyDataPtr = vtkSmartPointer; - -static Eigen::Vector3d getSegDir(PolyDataPtr seg) +static Eigen::Vector3d getSegDir(vtkSmartPointer seg) { if (seg->GetPoints()->GetNumberOfPoints() < 1) { - ROS_ERROR("can't get direction from a segment with fewer than 2 points"); + CONSOLE_BRIDGE_logError("can't get direction from a segment with fewer than 2 points"); Eigen::Vector3d v; v.x() = 1.0; v.y() = 0.0; @@ -87,7 +88,7 @@ alignRasterCD(tool_path_planner::PlaneSlicerRasterGenerator::RasterConstructData { if (rcd.raster_segments[0]->GetPoints()->GetNumberOfPoints() <= 1) { - ROS_ERROR("first raster segment has 0 or 1 points, unable to alignRasterCD()"); + CONSOLE_BRIDGE_logError("first raster segment has 0 or 1 points, unable to alignRasterCD()"); return (rcd); } Eigen::Vector3d raster_start; @@ -464,7 +465,7 @@ void PlaneSlicerRasterGenerator::setInput(pcl::PolygonMesh::ConstPtr mesh) // align mls_vertex_normals to vertex_normals if (!tool_path_planner::alignToVertexNormals(*mls_mesh_normals_ptr_, vertex_normals_)) { - ROS_ERROR("alignToVertexNormals failed"); + CONSOLE_BRIDGE_logError("alignToVertexNormals failed"); } setInput(mesh_data); @@ -478,7 +479,7 @@ void PlaneSlicerRasterGenerator::setInput(const shape_msgs::Mesh& mesh) } vtkSmartPointer PlaneSlicerRasterGenerator::getInput() { return mesh_data_; } -void PlaneSlicerRasterGenerator::computePoseData(const PolyDataPtr& polydata, +void PlaneSlicerRasterGenerator::computePoseData(const vtkSmartPointer& polydata, int idx, Eigen::Vector3d& p, Eigen::Vector3d& vx, @@ -508,9 +509,9 @@ tool_path_planner::ToolPaths PlaneSlicerRasterGenerator::convertToPoses( for (const tool_path_planner::PlaneSlicerRasterGenerator::RasterConstructData& rd : rasters_data) // for every raster { tool_path_planner::ToolPath raster_path; - std::vector raster_segments; + std::vector> raster_segments; raster_segments.assign(rd.raster_segments.begin(), rd.raster_segments.end()); - for (const PolyDataPtr& polydata : raster_segments) // for every segment + for (const vtkSmartPointer& polydata : raster_segments) // for every segment { tool_path_planner::ToolPathSegment raster_path_segment; std::size_t num_points = polydata->GetNumberOfPoints(); @@ -669,7 +670,7 @@ boost::optional PlaneSlicerRasterGenerator::generate() Vector3d raster_dir; if (config_.raster_direction.isApprox(Eigen::Vector3d::Zero())) { - ROS_ERROR("APPROX ZERO"); + CONSOLE_BRIDGE_logError("APPROX ZERO"); // If no direction was specified, use the middle dimension of the bounding box raster_dir = Eigen::Vector3d::UnitY(); raster_dir = (rotation_offset * raster_dir).normalized(); @@ -836,7 +837,7 @@ boost::optional PlaneSlicerRasterGenerator::generate() decltype(points) new_points = applyParametricSpline(points, line_length, config_.point_spacing); // add points to segment now - PolyDataPtr segment_data = PolyDataPtr::New(); + vtkSmartPointer segment_data = vtkSmartPointer::New(); segment_data->SetPoints(new_points); // transforming to original coordinate system @@ -870,7 +871,7 @@ boost::optional PlaneSlicerRasterGenerator::generate() } if (rasters_data_vec.size() == 0) { - ROS_ERROR("no rasters found"); + CONSOLE_BRIDGE_logError("no rasters found"); ToolPaths rasters; return (rasters); } From 41a65ae0652d59bb1e570dc40544decea270ad4c Mon Sep 17 00:00:00 2001 From: "David Merz, Jr" Date: Fri, 11 Mar 2022 14:49:58 -0600 Subject: [PATCH 60/64] Sequencer: Guard against disallowed front() back() --- .../src/simple_path_sequence_planner.cpp | 32 +++++++++++++------ 1 file changed, 23 insertions(+), 9 deletions(-) diff --git a/path_sequence_planner/src/simple_path_sequence_planner.cpp b/path_sequence_planner/src/simple_path_sequence_planner.cpp index 72780f02..b0b271a1 100644 --- a/path_sequence_planner/src/simple_path_sequence_planner.cpp +++ b/path_sequence_planner/src/simple_path_sequence_planner.cpp @@ -120,7 +120,10 @@ long SimplePathSequencePlanner::findNextNearestPath(tool_path_planner::ToolPaths bool front) { Eigen::Isometry3d last_pt; + // find next nearest point + // This should technically be guarded against last_path or its components being empty, but an + // empty path should not have been previously selected. if (front) last_pt = paths[last_path].front().front(); else @@ -136,17 +139,28 @@ long SimplePathSequencePlanner::findNextNearestPath(tool_path_planner::ToolPaths continue; // get first and last point of line j - Eigen::Isometry3d pt1 = paths[j].front().front(); - double dist1 = (pt1.translation() - last_pt.translation()).norm(); + if (!paths[j].empty()) + { + double dist1 = std::numeric_limits::max(); + if (!paths[j].front().empty()) + { + Eigen::Isometry3d pt1 = paths[j].front().front(); + dist1 = (pt1.translation() - last_pt.translation()).norm(); + } - // find distance between last point and the end points of the next line - Eigen::Isometry3d pt2 = paths[j].back().back(); - double dist2 = (pt2.translation() - last_pt.translation()).norm(); + // find distance between last point and the end points of the next line + double dist2 = std::numeric_limits::max(); + if (!paths[j].back().empty()) + { + Eigen::Isometry3d pt2 = paths[j].back().back(); + double dist2 = (pt2.translation() - last_pt.translation()).norm(); + } - if (dist1 < min_dist || dist2 < min_dist) - { - min_index = static_cast(j); - min_dist = (dist1 < dist2 ? dist1 : dist2); + if (dist1 < min_dist || dist2 < min_dist) + { + min_index = static_cast(j); + min_dist = (dist1 < dist2 ? dist1 : dist2); + } } } From e751fc6f357b84b59a30b33459285dfb43e33d45 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Mon, 9 May 2022 23:27:31 -0500 Subject: [PATCH 61/64] Added ability to hop inlets and peninsulas --- .../msg/HalfedgeEdgeGeneratorConfig.msg | 6 + .../halfedge_edge_generator.h | 6 + .../include/tool_path_planner/utilities.h | 2 + .../src/halfedge_edge_generator.cpp | 4 +- tool_path_planner/src/utilities.cpp | 131 ++++++++++++++++++ 5 files changed, 148 insertions(+), 1 deletion(-) diff --git a/noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg b/noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg index 78a25016..932d17d2 100644 --- a/noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg +++ b/noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg @@ -23,3 +23,9 @@ float64 point_dist # flag to break edge paths by diagonal axes bool split_by_axes + +# Size of peninsulas or inlets to skip over +float64 min_allowed_obstacle_width + +# Minimum number of waypoints for a obstacle skip to be considered valid +uint64 min_skip_amount diff --git a/tool_path_planner/include/tool_path_planner/halfedge_edge_generator.h b/tool_path_planner/include/tool_path_planner/halfedge_edge_generator.h index 3544d1e9..2951e801 100644 --- a/tool_path_planner/include/tool_path_planner/halfedge_edge_generator.h +++ b/tool_path_planner/include/tool_path_planner/halfedge_edge_generator.h @@ -69,6 +69,12 @@ class HalfedgeEdgeGenerator : public PathGenerator /** @brief flag indicating whether returned toolpaths should be split along the major and middle axis */ bool split_by_axes = true; + + /** @brief Size of peninsulas or inlets to skip over */ + double min_allowed_obstacle_width = 0.0; + + /** @brief Minimum number of points for a skip over an obstacle to be considered valid */ + std::size_t min_skip_amount = 0; }; HalfedgeEdgeGenerator() = default; diff --git a/tool_path_planner/include/tool_path_planner/utilities.h b/tool_path_planner/include/tool_path_planner/utilities.h index 59ffb071..39148cde 100644 --- a/tool_path_planner/include/tool_path_planner/utilities.h +++ b/tool_path_planner/include/tool_path_planner/utilities.h @@ -154,6 +154,8 @@ ToolPath splitByAxes(const ToolPathSegment& tool_path_segment, const Eigen::Vect ToolPaths splitByAxes(const ToolPaths& tool_paths); ToolPaths splitByAxes(const ToolPaths& tool_paths, const Eigen::Vector3f& axis_1, const Eigen::Vector3f& axis_2); +ToolPathSegment checkForPeninsulaOrInlet(const ToolPathSegment& tool_path, const double& min_allowed_width, const std::size_t& min_skip_amount); + /** * @brief Appends an interleaved set tool_paths to the provided tool_paths * @param tool_paths The input trajectory to interleave diff --git a/tool_path_planner/src/halfedge_edge_generator.cpp b/tool_path_planner/src/halfedge_edge_generator.cpp index 1854c490..867d946f 100644 --- a/tool_path_planner/src/halfedge_edge_generator.cpp +++ b/tool_path_planner/src/halfedge_edge_generator.cpp @@ -556,7 +556,9 @@ boost::optional HalfedgeEdgeGenerator::generate() if (!createToolPathSegment(bound_segment_points, {}, edge_segment)) return boost::none; - edge_paths.push_back({ edge_segment }); + ToolPathSegment cleaned_segment = checkForPeninsulaOrInlet(edge_segment, config_.min_allowed_obstacle_width, config_.min_skip_amount); + + edge_paths.push_back({ cleaned_segment }); CONSOLE_BRIDGE_logInform("Added boundary with %lu points", edge_paths.back()[0].size()); } diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index ad9d532c..54e12654 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -150,6 +150,8 @@ bool toHalfedgeConfigMsg(noether_msgs::HalfedgeEdgeGeneratorConfig& config_msg, config_msg.normal_influence_weight = config.normal_influence_weight; config_msg.point_spacing_method = static_cast(config.point_spacing_method); config_msg.point_dist = config.point_dist; + config_msg.min_allowed_obstacle_width = config.min_allowed_obstacle_width; + config_msg.min_skip_amount = config.min_skip_amount; return true; } @@ -218,6 +220,8 @@ bool toHalfedgeConfig(HalfedgeEdgeGenerator::Config& config, config.point_spacing_method = static_cast(config_msg.point_spacing_method); config.point_dist = config_msg.point_dist; + config.min_allowed_obstacle_width = config_msg.min_allowed_obstacle_width; + config.min_skip_amount = config_msg.min_skip_amount; return true; } @@ -701,6 +705,133 @@ ToolPaths splitByAxes(const ToolPaths& tool_paths, const Eigen::Vector3f& axis_1 return new_tool_paths; } +struct largestJumpResults +{ + bool exists = false; + std::size_t start_i = 0; + std::size_t end_i = 0; + std::size_t largest_jump = 0; + std::vector jump_exists; +}; + +largestJumpResults getLargestJump(const ToolPathSegment& tool_path_seg, const double& min_allowed_width) +{ + largestJumpResults results; + if (min_allowed_width == 0) + return results; + + for (std::size_t i = 0; i < tool_path_seg.size(); i++) + { + std::vector jumps; + for (std::size_t j = 0; j < tool_path_seg.size(); j++) + { + // Compare the distance to every waypoint after the current one + double dist = (tool_path_seg[i].translation() - tool_path_seg[j].translation()).norm(); + + // If the distance is less than the minimum given then add it to the vector keeping track + if (dist < min_allowed_width) + { + std::size_t forward_dist = std::min(j - i, tool_path_seg.size() + j - i); + std::size_t backward_dist = std::min(i - j, tool_path_seg.size() + i - j); + std::size_t min_dist = std::min(forward_dist, backward_dist); + jumps.push_back(min_dist); + if (min_dist > results.largest_jump) + { + results.largest_jump = min_dist; + results.start_i = i; + results.end_i = j; + } + } + } + if (jumps.size() > 0) + { + std::sort(jumps.begin(), jumps.end()); + bool skip_exists = false; + for (std::size_t j = 1; j < jumps.size(); j++) + { + if ((jumps[j] - jumps[j-1]) > 1) + { + results.exists = true; + skip_exists = true; + break; + } + } + results.jump_exists.push_back(skip_exists); + } + } + return results; +} + +std::vector getSideWithLowerSkipPercentage(largestJumpResults jump_results) +{ + double count_inner_skips = 0; + double count_inner_total = 0; + double count_outer_skips = 0; + double count_outer_total = 0; + for (std::size_t i = 0; i < jump_results.jump_exists.size(); i++) + { + if ((i < jump_results.start_i && i < jump_results.end_i) || + (i > jump_results.start_i && i > jump_results.end_i)) + { + if (jump_results.jump_exists[i]) + count_outer_skips++; + count_outer_total++; + } + else if ((i > jump_results.start_i && i < jump_results.end_i) || + (i < jump_results.start_i && i > jump_results.end_i)) + { + if (jump_results.jump_exists[i]) + count_inner_skips++; + count_inner_total++; + } + } + double percent_inner = count_inner_skips / count_inner_total; + double percent_outer = count_outer_skips / count_outer_total; + + std::vector indices_to_keep; + if (count_outer_total > count_inner_total) + { + for (std::size_t i = 0; i <= jump_results.start_i; i++) + indices_to_keep.push_back(i); + for (std::size_t i = jump_results.end_i; i < jump_results.jump_exists.size(); i++) + indices_to_keep.push_back(i); + } + else + { + for (std::size_t i = jump_results.start_i; i <= jump_results.end_i; i++) + indices_to_keep.push_back(i); + } + return indices_to_keep; +} + +ToolPathSegment checkForPeninsulaOrInlet(const ToolPathSegment& tool_path_seg, const double& min_allowed_width, const std::size_t& min_skip_amount) +{ + ToolPathSegment output_segment = tool_path_seg; + bool jumps_exist = true; + std::size_t jumps_cleared = 0; + while (jumps_exist) + { + largestJumpResults jump_results = getLargestJump(output_segment, min_allowed_width); + if (jump_results.exists && jump_results.largest_jump >= min_skip_amount) + { + jumps_cleared++; + std::vector indices_to_keep = getSideWithLowerSkipPercentage(jump_results); + ToolPathSegment temp_output_segment; + for (auto i : indices_to_keep) + temp_output_segment.push_back(output_segment[i]); + output_segment.clear(); + output_segment = temp_output_segment; + } + else + { + jumps_exist = false; + } + if (jumps_cleared > 100) + jumps_exist = false; + } + return output_segment; +} + void testAndMark(tlist_it& item1, tlist_it& item2, double point_spacing) { int mark1 = std::get<2>(*item1); From 958d5840372da422f34d5539874a6ed887fb9475 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Tue, 10 May 2022 09:57:04 -0500 Subject: [PATCH 62/64] Clang formatting and renamed variables to be clearer --- noether_filtering/src/utils.cpp | 77 +++--- .../msg/HalfedgeEdgeGeneratorConfig.msg | 7 +- .../eigen_value_edge_generator.h | 3 +- .../halfedge_edge_generator.h | 5 +- .../include/tool_path_planner/utilities.h | 7 +- .../src/halfedge_edge_generator.cpp | 9 +- .../src/plane_slicer_raster_generator.cpp | 12 +- tool_path_planner/src/utilities.cpp | 232 +++++++++--------- tool_path_planner/test/utest.cpp | 62 ++--- 9 files changed, 197 insertions(+), 217 deletions(-) diff --git a/noether_filtering/src/utils.cpp b/noether_filtering/src/utils.cpp index e6dc4962..7193161a 100644 --- a/noether_filtering/src/utils.cpp +++ b/noether_filtering/src/utils.cpp @@ -10,99 +10,96 @@ namespace noether_filtering { namespace utils { - int vtk2TriangleMesh(const vtkSmartPointer& poly_data, pcl::PolygonMesh& mesh) { - mesh.polygons.clear (); - mesh.cloud.data.clear (); + mesh.polygons.clear(); + mesh.cloud.data.clear(); mesh.cloud.width = mesh.cloud.height = 0; mesh.cloud.is_dense = true; - vtkSmartPointer mesh_points = poly_data->GetPoints (); - vtkIdType nr_points = mesh_points->GetNumberOfPoints (); - vtkIdType nr_polygons = poly_data->GetNumberOfPolys (); + vtkSmartPointer mesh_points = poly_data->GetPoints(); + vtkIdType nr_points = mesh_points->GetNumberOfPoints(); + vtkIdType nr_polygons = poly_data->GetNumberOfPolys(); if (nr_points == 0) return 0; vtkUnsignedCharArray* poly_colors = nullptr; if (poly_data->GetPointData() != nullptr) - poly_colors = vtkUnsignedCharArray::SafeDownCast (poly_data->GetPointData ()->GetScalars ("Colors")); + poly_colors = vtkUnsignedCharArray::SafeDownCast(poly_data->GetPointData()->GetScalars("Colors")); // Some applications do not save the name of scalars (including PCL's native vtk_io) if (!poly_colors) - poly_colors = vtkUnsignedCharArray::SafeDownCast (poly_data->GetPointData ()->GetScalars ("scalars")); + poly_colors = vtkUnsignedCharArray::SafeDownCast(poly_data->GetPointData()->GetScalars("scalars")); // TODO: currently only handles rgb values with 3 components - if (poly_colors && (poly_colors->GetNumberOfComponents () == 3)) + if (poly_colors && (poly_colors->GetNumberOfComponents() == 3)) { - pcl::PointCloud::Ptr cloud_temp (new pcl::PointCloud()); - cloud_temp->points.resize (nr_points); + pcl::PointCloud::Ptr cloud_temp(new pcl::PointCloud()); + cloud_temp->points.resize(nr_points); double point_xyz[3]; unsigned char point_color[3]; - for (vtkIdType i = 0; i < mesh_points->GetNumberOfPoints (); ++i) + for (vtkIdType i = 0; i < mesh_points->GetNumberOfPoints(); ++i) { - mesh_points->GetPoint (i, &point_xyz[0]); - (*cloud_temp)[i].x = static_cast (point_xyz[0]); - (*cloud_temp)[i].y = static_cast (point_xyz[1]); - (*cloud_temp)[i].z = static_cast (point_xyz[2]); + mesh_points->GetPoint(i, &point_xyz[0]); + (*cloud_temp)[i].x = static_cast(point_xyz[0]); + (*cloud_temp)[i].y = static_cast(point_xyz[1]); + (*cloud_temp)[i].z = static_cast(point_xyz[2]); - poly_colors->GetTupleValue (i, &point_color[0]); + poly_colors->GetTupleValue(i, &point_color[0]); (*cloud_temp)[i].r = point_color[0]; (*cloud_temp)[i].g = point_color[1]; (*cloud_temp)[i].b = point_color[2]; } - cloud_temp->width = cloud_temp->size (); + cloud_temp->width = cloud_temp->size(); cloud_temp->height = 1; cloud_temp->is_dense = true; - pcl::toPCLPointCloud2 (*cloud_temp, mesh.cloud); + pcl::toPCLPointCloud2(*cloud_temp, mesh.cloud); } - else // in case points do not have color information: + else // in case points do not have color information: { - pcl::PointCloud::Ptr cloud_temp (new pcl::PointCloud ()); - cloud_temp->points.resize (nr_points); + pcl::PointCloud::Ptr cloud_temp(new pcl::PointCloud()); + cloud_temp->points.resize(nr_points); double point_xyz[3]; - for (vtkIdType i = 0; i < mesh_points->GetNumberOfPoints (); ++i) + for (vtkIdType i = 0; i < mesh_points->GetNumberOfPoints(); ++i) { - mesh_points->GetPoint (i, &point_xyz[0]); - (*cloud_temp)[i].x = static_cast (point_xyz[0]); - (*cloud_temp)[i].y = static_cast (point_xyz[1]); - (*cloud_temp)[i].z = static_cast (point_xyz[2]); + mesh_points->GetPoint(i, &point_xyz[0]); + (*cloud_temp)[i].x = static_cast(point_xyz[0]); + (*cloud_temp)[i].y = static_cast(point_xyz[1]); + (*cloud_temp)[i].z = static_cast(point_xyz[2]); } - cloud_temp->width = cloud_temp->size (); + cloud_temp->width = cloud_temp->size(); cloud_temp->height = 1; cloud_temp->is_dense = true; - pcl::toPCLPointCloud2 (*cloud_temp, mesh.cloud); + pcl::toPCLPointCloud2(*cloud_temp, mesh.cloud); } - mesh.polygons.reserve (nr_polygons); + mesh.polygons.reserve(nr_polygons); #ifdef VTK_CELL_ARRAY_V2 - vtkIdType const *cell_points; + vtkIdType const* cell_points; #else vtkIdType* cell_points; #endif vtkIdType nr_cell_points; - vtkCellArray * mesh_polygons = poly_data->GetPolys (); - mesh_polygons->InitTraversal (); + vtkCellArray* mesh_polygons = poly_data->GetPolys(); + mesh_polygons->InitTraversal(); int id_poly = 0; - while (mesh_polygons->GetNextCell (nr_cell_points, cell_points)) + while (mesh_polygons->GetNextCell(nr_cell_points, cell_points)) { - if (nr_cell_points == 3 && - cell_points[0] != cell_points[1] && - cell_points[1] != cell_points[2] && + if (nr_cell_points == 3 && cell_points[0] != cell_points[1] && cell_points[1] != cell_points[2] && cell_points[2] != cell_points[0]) { mesh.polygons.emplace_back(pcl::Vertices()); - mesh.polygons[id_poly].vertices.resize (nr_cell_points); + mesh.polygons[id_poly].vertices.resize(nr_cell_points); for (vtkIdType i = 0; i < nr_cell_points; ++i) - mesh.polygons[id_poly].vertices[i] = static_cast (cell_points[i]); + mesh.polygons[id_poly].vertices[i] = static_cast(cell_points[i]); ++id_poly; } } - return (static_cast (nr_points)); + return (static_cast(nr_points)); } } // namespace utils diff --git a/noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg b/noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg index 932d17d2..38eef274 100644 --- a/noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg +++ b/noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg @@ -24,8 +24,5 @@ float64 point_dist # flag to break edge paths by diagonal axes bool split_by_axes -# Size of peninsulas or inlets to skip over -float64 min_allowed_obstacle_width - -# Minimum number of waypoints for a obstacle skip to be considered valid -uint64 min_skip_amount +# Size of peninsulas or inlets to skip over, default of 0 will not bridge any gaps, effectively off +float64 max_bridge_distance diff --git a/tool_path_planner/include/tool_path_planner/eigen_value_edge_generator.h b/tool_path_planner/include/tool_path_planner/eigen_value_edge_generator.h index 3cc30cfb..d217edff 100644 --- a/tool_path_planner/include/tool_path_planner/eigen_value_edge_generator.h +++ b/tool_path_planner/include/tool_path_planner/eigen_value_edge_generator.h @@ -100,7 +100,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 */ - bool split_by_axes = true; /** @brief flag indicating whether returned toolpaths should be split along the major and middle axis */ + bool split_by_axes = + true; /** @brief flag indicating whether returned toolpaths should be split along the major and middle axis */ }; EigenValueEdgeGenerator() = default; diff --git a/tool_path_planner/include/tool_path_planner/halfedge_edge_generator.h b/tool_path_planner/include/tool_path_planner/halfedge_edge_generator.h index 2951e801..a8b415b0 100644 --- a/tool_path_planner/include/tool_path_planner/halfedge_edge_generator.h +++ b/tool_path_planner/include/tool_path_planner/halfedge_edge_generator.h @@ -71,10 +71,7 @@ class HalfedgeEdgeGenerator : public PathGenerator bool split_by_axes = true; /** @brief Size of peninsulas or inlets to skip over */ - double min_allowed_obstacle_width = 0.0; - - /** @brief Minimum number of points for a skip over an obstacle to be considered valid */ - std::size_t min_skip_amount = 0; + double max_bridge_distance = 0.0; }; HalfedgeEdgeGenerator() = default; diff --git a/tool_path_planner/include/tool_path_planner/utilities.h b/tool_path_planner/include/tool_path_planner/utilities.h index 39148cde..6b463b14 100644 --- a/tool_path_planner/include/tool_path_planner/utilities.h +++ b/tool_path_planner/include/tool_path_planner/utilities.h @@ -150,11 +150,14 @@ ToolPaths reverseOddRasters(const ToolPaths& tool_paths, RasterStyle raster_styl double computeOffsetSign(const ToolPathSegment& adjusted_segment, const ToolPathSegment& away_from_segment); ToolPath splitByAxes(const ToolPathSegment& tool_path_segment); -ToolPath splitByAxes(const ToolPathSegment& tool_path_segment, const Eigen::Vector3f& axis_1, const Eigen::Vector3f& axis_2); +ToolPath splitByAxes(const ToolPathSegment& tool_path_segment, + const Eigen::Vector3f& axis_1, + const Eigen::Vector3f& axis_2); ToolPaths splitByAxes(const ToolPaths& tool_paths); ToolPaths splitByAxes(const ToolPaths& tool_paths, const Eigen::Vector3f& axis_1, const Eigen::Vector3f& axis_2); -ToolPathSegment checkForPeninsulaOrInlet(const ToolPathSegment& tool_path, const double& min_allowed_width, const std::size_t& min_skip_amount); +ToolPathSegment checkForPeninsulaOrInlet(const ToolPathSegment& tool_path, + const double& min_allowed_width); /** * @brief Appends an interleaved set tool_paths to the provided tool_paths diff --git a/tool_path_planner/src/halfedge_edge_generator.cpp b/tool_path_planner/src/halfedge_edge_generator.cpp index 867d946f..ab6979ea 100644 --- a/tool_path_planner/src/halfedge_edge_generator.cpp +++ b/tool_path_planner/src/halfedge_edge_generator.cpp @@ -156,8 +156,8 @@ bool applyMinPointDistance(const pcl::PointCloud& in, * @return True on success, False otherwise */ static bool applyEqualDistancePathCloud(const pcl::PointCloud& in, - pcl::PointCloud& out, - double dist) + pcl::PointCloud& out, + double dist) { using namespace pcl; using namespace Eigen; @@ -556,10 +556,11 @@ boost::optional HalfedgeEdgeGenerator::generate() if (!createToolPathSegment(bound_segment_points, {}, edge_segment)) return boost::none; - ToolPathSegment cleaned_segment = checkForPeninsulaOrInlet(edge_segment, config_.min_allowed_obstacle_width, config_.min_skip_amount); + ToolPathSegment cleaned_segment = + checkForPeninsulaOrInlet(edge_segment, config_.max_bridge_distance); edge_paths.push_back({ cleaned_segment }); - CONSOLE_BRIDGE_logInform("Added boundary with %lu points", edge_paths.back()[0].size()); + CONSOLE_BRIDGE_logDebug("Added boundary with %lu points", edge_paths.back()[0].size()); } // sorting diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index f7a7c282..dcb39e3d 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -96,7 +96,7 @@ alignRasterCD(tool_path_planner::PlaneSlicerRasterGenerator::RasterConstructData // determine location and direction of each successive segement, reverse any mis-directed segments tool_path_planner::PlaneSlicerRasterGenerator::RasterConstructData temp_rcd; - std::list > seg_order; // once sorted this list will be the order of the segments + std::list> seg_order; // once sorted this list will be the order of the segments for (size_t i = 0; i < rcd.raster_segments.size(); i++) { // determine i'th segments direction @@ -236,7 +236,7 @@ static vtkSmartPointer applyParametricSpline(const vtkSmartPointer >& points_lists) +static void removeRedundant(std::vector>& points_lists) { using IdList = std::vector; if (points_lists.size() < 2) @@ -244,7 +244,7 @@ static void removeRedundant(std::vector >& points_lists) return; } - std::vector > new_points_lists; + std::vector> new_points_lists; new_points_lists.push_back(points_lists.front()); for (std::size_t i = 1; i < points_lists.size(); i++) { @@ -280,7 +280,7 @@ static void removeRedundant(std::vector >& points_lists) static void mergeRasterSegments(const vtkSmartPointer& points, double merge_dist, - std::vector >& points_lists) + std::vector>& points_lists) { using namespace Eigen; using IdList = std::vector; @@ -378,7 +378,7 @@ static void mergeRasterSegments(const vtkSmartPointer& points, static void rectifyDirection(const vtkSmartPointer& points, const Eigen::Vector3d& ref_point, - std::vector >& points_lists) + std::vector>& points_lists) { using namespace Eigen; Vector3d p0, pf; @@ -459,7 +459,7 @@ void PlaneSlicerRasterGenerator::setInput(pcl::PolygonMesh::ConstPtr mesh) // compute vertex normals using Moving Least Squares pcl::PointCloud::Ptr mesh_cloud_ptr(new pcl::PointCloud()); pcl::fromPCLPointCloud2(mesh->cloud, *mesh_cloud_ptr); - mls_mesh_normals_ptr_ = boost::make_shared >( + mls_mesh_normals_ptr_ = boost::make_shared>( tool_path_planner::computeMLSMeshNormals(mesh_cloud_ptr, config_.search_radius)); // align mls_vertex_normals to vertex_normals diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index 54e12654..7b36d7df 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -10,7 +10,7 @@ #include #include #include -#include // std::sort +#include // std::sort #include #include @@ -63,7 +63,8 @@ void flipPointOrder(ToolPath& path) p *= Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ()); } -tool_path_planner::ToolPathSegmentData toToolPathSegmentData(const tool_path_planner::ToolPathSegment& tool_path_segment) +tool_path_planner::ToolPathSegmentData +toToolPathSegmentData(const tool_path_planner::ToolPathSegment& tool_path_segment) { using namespace Eigen; @@ -150,8 +151,7 @@ bool toHalfedgeConfigMsg(noether_msgs::HalfedgeEdgeGeneratorConfig& config_msg, config_msg.normal_influence_weight = config.normal_influence_weight; config_msg.point_spacing_method = static_cast(config.point_spacing_method); config_msg.point_dist = config.point_dist; - config_msg.min_allowed_obstacle_width = config.min_allowed_obstacle_width; - config_msg.min_skip_amount = config.min_skip_amount; + config_msg.max_bridge_distance = config.max_bridge_distance; return true; } @@ -220,8 +220,7 @@ bool toHalfedgeConfig(HalfedgeEdgeGenerator::Config& config, config.point_spacing_method = static_cast(config_msg.point_spacing_method); config.point_dist = config_msg.point_dist; - config.min_allowed_obstacle_width = config_msg.min_allowed_obstacle_width; - config.min_skip_amount = config_msg.min_skip_amount; + config.max_bridge_distance = config_msg.max_bridge_distance; return true; } @@ -558,7 +557,7 @@ ToolPath splitByAxes(const ToolPathSegment& tool_path_segment) // Get major and middle axis (we don't care about minor axis) pcl::PointCloud::Ptr cloud = boost::make_shared>(); - for (Eigen::Isometry3d p: tool_path_segment) + for (Eigen::Isometry3d p : tool_path_segment) { pcl::PointXYZ point(float(p.translation().x()), float(p.translation().y()), float(p.translation().z())); cloud->push_back(point); @@ -567,7 +566,7 @@ ToolPath splitByAxes(const ToolPathSegment& tool_path_segment) moment.setInputCloud(cloud); moment.compute(); Eigen::Vector3f major_axis, middle_axis, minor_axis; - if(!moment.getEigenVectors(major_axis, middle_axis, minor_axis)) + if (!moment.getEigenVectors(major_axis, middle_axis, minor_axis)) { ROS_ERROR_STREAM("Could not compute Eigen Vectors."); } @@ -580,7 +579,9 @@ ToolPath splitByAxes(const ToolPathSegment& tool_path_segment) return splitByAxes(tool_path_segment, major_axis, perp_axis); } -ToolPath splitByAxes(const ToolPathSegment& tool_path_segment, const Eigen::Vector3f& axis_1, const Eigen::Vector3f& axis_2) +ToolPath splitByAxes(const ToolPathSegment& tool_path_segment, + const Eigen::Vector3f& axis_1, + const Eigen::Vector3f& axis_2) { // Sanity check - tool path segment must not be empty if (tool_path_segment.size() == 0) @@ -593,10 +594,7 @@ ToolPath splitByAxes(const ToolPathSegment& tool_path_segment, const Eigen::Vect ToolPath new_tool_path; std::vector vectors = { - -1 * axis_1 + -1 * axis_2, - -1 * axis_1 + axis_2, - axis_1 + axis_2, - axis_1 + -1 * axis_2 + -1 * axis_1 + -1 * axis_2, -1 * axis_1 + axis_2, axis_1 + axis_2, axis_1 + -1 * axis_2 }; // Get indices to cut tool path at std::set cut_indices; @@ -619,7 +617,9 @@ ToolPath splitByAxes(const ToolPathSegment& tool_path_segment, const Eigen::Vect for (std::size_t index = 0; index < tool_path_segment.size(); ++index) { Eigen::Isometry3d p = tool_path_segment[index]; - Eigen::Vector3f v(float(p.translation().x() - path_center[0]), float(p.translation().y() - path_center[1]), float(p.translation().z() - path_center[2])); + Eigen::Vector3f v(float(p.translation().x() - path_center[0]), + float(p.translation().y() - path_center[1]), + float(p.translation().z() - path_center[2])); float dot = vector.dot(v); if (dot > max_dot) { @@ -707,129 +707,129 @@ ToolPaths splitByAxes(const ToolPaths& tool_paths, const Eigen::Vector3f& axis_1 struct largestJumpResults { - bool exists = false; - std::size_t start_i = 0; - std::size_t end_i = 0; - std::size_t largest_jump = 0; - std::vector jump_exists; + bool exists = false; + std::size_t start_i = 0; + std::size_t end_i = 0; + std::size_t largest_jump = 0; + std::vector jump_exists; }; largestJumpResults getLargestJump(const ToolPathSegment& tool_path_seg, const double& min_allowed_width) { - largestJumpResults results; - if (min_allowed_width == 0) - return results; + largestJumpResults results; + if (min_allowed_width == 0) + return results; - for (std::size_t i = 0; i < tool_path_seg.size(); i++) + for (std::size_t i = 0; i < tool_path_seg.size(); i++) + { + std::vector jumps; + for (std::size_t j = 0; j < tool_path_seg.size(); j++) { - std::vector jumps; - for (std::size_t j = 0; j < tool_path_seg.size(); j++) + // Compare the distance to every waypoint after the current one + double dist = (tool_path_seg[i].translation() - tool_path_seg[j].translation()).norm(); + + // If the distance is less than the minimum given then add it to the vector keeping track + if (dist < min_allowed_width) + { + std::size_t forward_dist = std::min(j - i, tool_path_seg.size() + j - i); + std::size_t backward_dist = std::min(i - j, tool_path_seg.size() + i - j); + std::size_t min_dist = std::min(forward_dist, backward_dist); + jumps.push_back(min_dist); + if (min_dist > results.largest_jump) { - // Compare the distance to every waypoint after the current one - double dist = (tool_path_seg[i].translation() - tool_path_seg[j].translation()).norm(); - - // If the distance is less than the minimum given then add it to the vector keeping track - if (dist < min_allowed_width) - { - std::size_t forward_dist = std::min(j - i, tool_path_seg.size() + j - i); - std::size_t backward_dist = std::min(i - j, tool_path_seg.size() + i - j); - std::size_t min_dist = std::min(forward_dist, backward_dist); - jumps.push_back(min_dist); - if (min_dist > results.largest_jump) - { - results.largest_jump = min_dist; - results.start_i = i; - results.end_i = j; - } - } + results.largest_jump = min_dist; + results.start_i = i; + results.end_i = j; } - if (jumps.size() > 0) + } + } + if (jumps.size() > 0) + { + std::sort(jumps.begin(), jumps.end()); + bool skip_exists = false; + for (std::size_t j = 1; j < jumps.size(); j++) + { + if ((jumps[j] - jumps[j - 1]) > 1) { - std::sort(jumps.begin(), jumps.end()); - bool skip_exists = false; - for (std::size_t j = 1; j < jumps.size(); j++) - { - if ((jumps[j] - jumps[j-1]) > 1) - { - results.exists = true; - skip_exists = true; - break; - } - } - results.jump_exists.push_back(skip_exists); + results.exists = true; + skip_exists = true; + break; } + } + results.jump_exists.push_back(skip_exists); } - return results; + } + return results; } std::vector getSideWithLowerSkipPercentage(largestJumpResults jump_results) { - double count_inner_skips = 0; - double count_inner_total = 0; - double count_outer_skips = 0; - double count_outer_total = 0; - for (std::size_t i = 0; i < jump_results.jump_exists.size(); i++) - { - if ((i < jump_results.start_i && i < jump_results.end_i) || - (i > jump_results.start_i && i > jump_results.end_i)) - { - if (jump_results.jump_exists[i]) - count_outer_skips++; - count_outer_total++; - } - else if ((i > jump_results.start_i && i < jump_results.end_i) || - (i < jump_results.start_i && i > jump_results.end_i)) - { - if (jump_results.jump_exists[i]) - count_inner_skips++; - count_inner_total++; - } - } - double percent_inner = count_inner_skips / count_inner_total; - double percent_outer = count_outer_skips / count_outer_total; - - std::vector indices_to_keep; - if (count_outer_total > count_inner_total) + double count_inner_skips = 0; + double count_inner_total = 0; + double count_outer_skips = 0; + double count_outer_total = 0; + for (std::size_t i = 0; i < jump_results.jump_exists.size(); i++) + { + if ((i < jump_results.start_i && i < jump_results.end_i) || (i > jump_results.start_i && i > jump_results.end_i)) { - for (std::size_t i = 0; i <= jump_results.start_i; i++) - indices_to_keep.push_back(i); - for (std::size_t i = jump_results.end_i; i < jump_results.jump_exists.size(); i++) - indices_to_keep.push_back(i); + if (jump_results.jump_exists[i]) + count_outer_skips++; + count_outer_total++; } - else + else if ((i > jump_results.start_i && i < jump_results.end_i) || + (i < jump_results.start_i && i > jump_results.end_i)) { - for (std::size_t i = jump_results.start_i; i <= jump_results.end_i; i++) - indices_to_keep.push_back(i); + if (jump_results.jump_exists[i]) + count_inner_skips++; + count_inner_total++; } - return indices_to_keep; + } + double percent_inner = count_inner_skips / count_inner_total; + double percent_outer = count_outer_skips / count_outer_total; + + std::vector indices_to_keep; + if (count_outer_total > count_inner_total) + { + for (std::size_t i = 0; i <= jump_results.start_i; i++) + indices_to_keep.push_back(i); + for (std::size_t i = jump_results.end_i; i < jump_results.jump_exists.size(); i++) + indices_to_keep.push_back(i); + } + else + { + for (std::size_t i = jump_results.start_i; i <= jump_results.end_i; i++) + indices_to_keep.push_back(i); + } + return indices_to_keep; } -ToolPathSegment checkForPeninsulaOrInlet(const ToolPathSegment& tool_path_seg, const double& min_allowed_width, const std::size_t& min_skip_amount) +ToolPathSegment checkForPeninsulaOrInlet(const ToolPathSegment& tool_path_seg, + const double& min_allowed_width) { - ToolPathSegment output_segment = tool_path_seg; - bool jumps_exist = true; - std::size_t jumps_cleared = 0; - while (jumps_exist) + ToolPathSegment output_segment = tool_path_seg; + bool jumps_exist = true; + std::size_t jumps_cleared = 0; + while (jumps_exist) + { + largestJumpResults jump_results = getLargestJump(output_segment, min_allowed_width); + if (jump_results.exists) { - largestJumpResults jump_results = getLargestJump(output_segment, min_allowed_width); - if (jump_results.exists && jump_results.largest_jump >= min_skip_amount) - { - jumps_cleared++; - std::vector indices_to_keep = getSideWithLowerSkipPercentage(jump_results); - ToolPathSegment temp_output_segment; - for (auto i : indices_to_keep) - temp_output_segment.push_back(output_segment[i]); - output_segment.clear(); - output_segment = temp_output_segment; - } - else - { - jumps_exist = false; - } - if (jumps_cleared > 100) - jumps_exist = false; + jumps_cleared++; + std::vector indices_to_keep = getSideWithLowerSkipPercentage(jump_results); + ToolPathSegment temp_output_segment; + for (auto i : indices_to_keep) + temp_output_segment.push_back(output_segment[i]); + output_segment.clear(); + output_segment = temp_output_segment; } - return output_segment; + else + { + jumps_exist = false; + } + if (jumps_cleared > 100) + jumps_exist = false; + } + return output_segment; } void testAndMark(tlist_it& item1, tlist_it& item2, double point_spacing) @@ -883,7 +883,7 @@ tlist pruneList(tlist& waypoint_list, double point_spacing) return (new_list); } -ToolPath sortAndSegment(std::list >& waypoint_list, double point_spacing) +ToolPath sortAndSegment(std::list>& waypoint_list, double point_spacing) { ToolPath new_tool_path; ToolPathSegment seg; @@ -971,8 +971,8 @@ ToolPaths addExtraWaypoints(const ToolPaths& tool_paths, double raster_spacing, // end the old and start a new segment when the new point is significantly more than the point_spacing from previous // waypoint ToolPath tool_path = tool_paths[i]; - std::list > waypoint_list; // once sorted this list will be the order of - // the segments + std::list> waypoint_list; // once sorted this list will be the order of + // the segments Eigen::Vector3d path_dir = computePathDirection(tool_path); ToolPathSegment sseg = tool_path[0]; Eigen::Vector3d path_start = sseg[0].translation(); diff --git a/tool_path_planner/test/utest.cpp b/tool_path_planner/test/utest.cpp index 0f942516..e10b6837 100644 --- a/tool_path_planner/test/utest.cpp +++ b/tool_path_planner/test/utest.cpp @@ -410,7 +410,8 @@ void runExtraRasterTest(tool_path_planner::PathGenerator& planner, } void runSegmentByAxesTest(const tool_path_planner::ToolPathSegment& tool_path_segment, - const Eigen::Vector3f& axis_1, const Eigen::Vector3f& axis_2) + const Eigen::Vector3f& axis_1, + const Eigen::Vector3f& axis_2) { vtk_viewer::VTKViewer viz; tool_path_planner::ToolPath tool_path = tool_path_planner::splitByAxes(tool_path_segment, axis_1, axis_2); @@ -419,10 +420,10 @@ void runSegmentByAxesTest(const tool_path_planner::ToolPathSegment& tool_path_se double scale = 1.0; std::vector> colors = { - {1, 0, 0}, - {0, 1, 0}, - {0, 0, 1}, - {1, 1, 0}, + { 1, 0, 0 }, + { 0, 1, 0 }, + { 0, 0, 1 }, + { 1, 1, 0 }, }; // Display surface normals @@ -469,9 +470,7 @@ tool_path_planner::ToolPathSegment toSegment(std::vector points * @param angle_inc Increment degree for generated points around the circle * @return Points of circle */ -std::vector getCircle( - double radius = 1.0, - double angle_inc = DEG2RAD(5)) +std::vector getCircle(double radius = 1.0, double angle_inc = DEG2RAD(5)) { std::vector points; @@ -536,9 +535,7 @@ std::vector getCircle( * @param inc Increment length for interpolated points * @return Points of polygon */ -std::vector getPolygon( - std::vector vertices, - double inc = 0.05) +std::vector getPolygon(std::vector vertices, double inc = 0.05) { std::vector points; for (std::size_t point_index = 0; point_index < vertices.size(); ++point_index) @@ -546,21 +543,17 @@ std::vector getPolygon( double x1 = vertices[point_index].x(); double y1 = vertices[point_index].y(); double z1 = vertices[point_index].z(); - double x2 = vertices[(point_index+1)%vertices.size()].x(); - double y2 = vertices[(point_index+1)%vertices.size()].y(); - double z2 = vertices[(point_index+1)%vertices.size()].z(); + double x2 = vertices[(point_index + 1) % vertices.size()].x(); + double y2 = vertices[(point_index + 1) % vertices.size()].y(); + double z2 = vertices[(point_index + 1) % vertices.size()].z(); double dist = std::sqrt(std::pow(x1 - x2, 2) + std::pow(y1 - y2, 2) + std::pow(z1 - z2, 2)); int n = int(std::round(dist / inc)); - double x_inc = (x2 - x1)/n; - double y_inc = (y2 - y1)/n; - double z_inc = (z2 - z1)/n; + double x_inc = (x2 - x1) / n; + double y_inc = (y2 - y1) / n; + double z_inc = (z2 - z1) / n; for (int i = 0; i < n; ++i) { - Eigen::Vector3d point( - x1 + (i * x_inc), - y1 + (i * y_inc), - z1 + (i * z_inc) - ); + Eigen::Vector3d point(x1 + (i * x_inc), y1 + (i * y_inc), z1 + (i * z_inc)); points.push_back(point); } } @@ -573,20 +566,13 @@ std::vector getPolygon( * @param width Width of rectangle * @return Points of rectangle */ -std::vector getRectangle( - double length=1.0, - double width=2.0, - double inc=0.05) +std::vector getRectangle(double length = 1.0, double width = 2.0, double inc = 0.05) { - return getPolygon( - { - Eigen::Vector3d(-length/2, -width/2, 0), - Eigen::Vector3d(-length/2, width/2, 0), - Eigen::Vector3d( length/2, width/2, 0), - Eigen::Vector3d( length/2, -width/2, 0) - }, - inc - ); + return getPolygon({ Eigen::Vector3d(-length / 2, -width / 2, 0), + Eigen::Vector3d(-length / 2, width / 2, 0), + Eigen::Vector3d(length / 2, width / 2, 0), + Eigen::Vector3d(length / 2, -width / 2, 0) }, + inc); } /** @@ -595,9 +581,7 @@ std::vector getRectangle( * @param plane_coeff Coefficients of homogeneous plane equation: for 0 = Ax + By + Cz * @return Projected points */ -std::vector projectZToPlane( - std::vector points_in, - Eigen::Vector3d plane_coeff) +std::vector projectZToPlane(std::vector points_in, Eigen::Vector3d plane_coeff) { std::vector points_out; for (auto p : points_in) @@ -605,7 +589,7 @@ std::vector projectZToPlane( double x = plane_coeff.x(); double y = plane_coeff.y(); // Project z value onto plane --> z = (-Ax - By) / C - p.z() = (-1*x*p.x() + -1*y*p.y())/p.z(); + p.z() = (-1 * x * p.x() + -1 * y * p.y()) / p.z(); points_out.push_back(p); } return points_out; From f033397c1fba3d6422e82ce03d2694e7b89cbefd Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Tue, 10 May 2022 11:00:34 -0500 Subject: [PATCH 63/64] Clarified param units and better infinite loop check --- noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg | 2 +- tool_path_planner/src/utilities.cpp | 5 ++++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg b/noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg index 38eef274..e4f72346 100644 --- a/noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg +++ b/noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg @@ -24,5 +24,5 @@ float64 point_dist # flag to break edge paths by diagonal axes bool split_by_axes -# Size of peninsulas or inlets to skip over, default of 0 will not bridge any gaps, effectively off +# Size (meters) of peninsulas or inlets to skip over, default of 0 will not bridge any gaps, effectively off float64 max_bridge_distance diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index 7b36d7df..b4c82253 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -811,6 +811,7 @@ ToolPathSegment checkForPeninsulaOrInlet(const ToolPathSegment& tool_path_seg, std::size_t jumps_cleared = 0; while (jumps_exist) { + std::size_t prev_size = output_segment.size(); largestJumpResults jump_results = getLargestJump(output_segment, min_allowed_width); if (jump_results.exists) { @@ -826,7 +827,9 @@ ToolPathSegment checkForPeninsulaOrInlet(const ToolPathSegment& tool_path_seg, { jumps_exist = false; } - if (jumps_cleared > 100) + + // Check to ensure that points were actually removed, if not you will be stuck in infinite loop + if (prev_size == output_segment.size() || output_segment.size() == 0) jumps_exist = false; } return output_segment; From 67131418239b780ed45786a97047dae82c36ec5b Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Tue, 10 May 2022 11:18:07 -0500 Subject: [PATCH 64/64] Ensures monotonically decreasing toolpath sizes --- tool_path_planner/src/utilities.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index b4c82253..ecf77a09 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -829,7 +829,7 @@ ToolPathSegment checkForPeninsulaOrInlet(const ToolPathSegment& tool_path_seg, } // Check to ensure that points were actually removed, if not you will be stuck in infinite loop - if (prev_size == output_segment.size() || output_segment.size() == 0) + if (prev_size <= output_segment.size() || output_segment.size() == 0) jumps_exist = false; } return output_segment;