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/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 e7d6229f..96722a0d 100644 --- a/noether/src/tool_path_planner_server.cpp +++ b/noether/src/tool_path_planner_server.cpp @@ -11,6 +11,7 @@ #include #include #include +#include static const double FEEDBACK_PUBLISH_PERIOD = 1.0; // seconds static const std::string GENERATE_TOOL_PATHS_ACTION = "generate_tool_paths"; @@ -97,6 +98,7 @@ class TppServer const noether_msgs::ToolPathConfig& config = goal->path_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 +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; } else if (config.type == noether_msgs::ToolPathConfig::PLANE_SLICER_RASTER_GENERATOR) { @@ -113,6 +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; } 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 { @@ -144,7 +153,6 @@ class TppServer if (tool_paths.is_initialized()) { - noether_msgs::ToolPaths trp; for (const auto& tool_path : tool_paths.get()) { noether_msgs::ToolPath tp; @@ -157,9 +165,15 @@ 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); } - trp.paths.push_back(tp); + + result.tool_paths[i].paths.push_back(tp); } result.tool_path_validities[i] = true; @@ -229,6 +243,7 @@ class TppServer ros::NodeHandle nh_; GenPathActionServer as_; std::mutex goal_process_mutex_; + bool smooth_pose_arrays_; }; } // namespace tpp_path_gen 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..7193161a --- /dev/null +++ b/noether_filtering/src/utils.cpp @@ -0,0 +1,106 @@ +#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/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/EigenValueEdgeGeneratorConfig.msg b/noether_msgs/msg/EigenValueEdgeGeneratorConfig.msg index f4949429..37669d7c 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 + +# flag to break edge paths by diagonal axes +bool split_by_axes diff --git a/noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg b/noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg index 07892f88..e4f72346 100644 --- a/noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg +++ b/noether_msgs/msg/HalfedgeEdgeGeneratorConfig.msg @@ -20,3 +20,9 @@ int32 point_spacing_method # Point distance parameter used in conjunction with the spacing method float64 point_dist + +# flag to break edge paths by diagonal axes +bool split_by_axes + +# 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/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/PlaneSlicerRasterGeneratorConfig.msg b/noether_msgs/msg/PlaneSlicerRasterGeneratorConfig.msg index ca33e269..5a073f8f 100644 --- a/noether_msgs/msg/PlaneSlicerRasterGeneratorConfig.msg +++ b/noether_msgs/msg/PlaneSlicerRasterGeneratorConfig.msg @@ -1,6 +1,21 @@ -float64 raster_spacing # Offset between two rasters -float64 point_spacing # Required spacing between path points -float64 raster_rot_offset # Specifies the direction of the raster path in radians -float64 min_segment_size # The minimum segment size to allow when finding intersections; small segments will be discarded -float64 min_hole_size # A path may pass over holes smaller than this, but must be broken when larger holes are encounterd. -float64 tool_offset # How far off the surface the tool needs to be +float64 raster_spacing # Offset between two rasters +float64 point_spacing # Required spacing between path points +float64 raster_rot_offset # Specifies the direction of the raster path in radians. Rotates about short dimension of bounding box. +float64 min_segment_size # The minimum segment size to allow when finding intersections; small segments will be discarded +float64 min_hole_size # A path may pass over holes smaller than this, but must be broken when larger holes are encounterd. +float64 tool_offset # How far off the surface the tool needs to be +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 +# 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. Specified +# in the frame of the input mesh. Overrides raster_wrt_global_axes +geometry_msgs/Vector3 raster_direction diff --git a/noether_msgs/msg/ToolPathConfig.msg b/noether_msgs/msg/ToolPathConfig.msg index a5723625..49d3d358 100644 --- a/noether_msgs/msg/ToolPathConfig.msg +++ b/noether_msgs/msg/ToolPathConfig.msg @@ -3,10 +3,13 @@ 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 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/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); + } } } diff --git a/tool_path_planner/CMakeLists.txt b/tool_path_planner/CMakeLists.txt index ca38f2c5..3d2e7d25 100644 --- a/tool_path_planner/CMakeLists.txt +++ b/tool_path_planner/CMakeLists.txt @@ -48,6 +48,7 @@ catkin_package( geometry_msgs noether_msgs rosconsole + roscpp shape_msgs DEPENDS EIGEN3 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..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 @@ -99,6 +99,9 @@ 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 */ }; 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..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 @@ -66,6 +66,12 @@ 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 split along the major and middle axis */ + bool split_by_axes = true; + + /** @brief Size of peninsulas or inlets to skip over */ + double max_bridge_distance = 0.0; }; HalfedgeEdgeGenerator() = default; 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..910e5cfe 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, // 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 */ 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 14e22573..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,16 +22,20 @@ #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 @@ -39,12 +43,17 @@ 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; static constexpr double DEFAULT_MIN_SEGMENT_SIZE = 0.01; static constexpr double DEFAULT_SEARCH_RADIUS = 0.01; + static constexpr bool DEFAULT_INTERLEAVE_RASTERS = false; + static constexpr bool DEFAULT_SMOOTH_RASTERS = false; static constexpr double DEFAULT_MIN_HOLE_SIZE = 1e-2; + static constexpr bool DEFAULT_RASTER_WRT_GLOBAL_AXES = false; + static constexpr bool DEFAULT_GENERATE_EXTRA_RASTERS = true; public: struct Config @@ -55,7 +64,12 @@ 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 interleave_rasters{ DEFAULT_INTERLEAVE_RASTERS }; + 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::Zero() }; + RasterStyle raster_style{ DEFAULT_RASTER_STYLE }; Json::Value toJson() const { Json::Value jv(Json::ValueType::objectValue); @@ -64,7 +78,15 @@ class PlaneSlicerRasterGenerator : public PathGenerator jv["raster_rot_offset"] = raster_rot_offset; jv["min_segment_size"] = min_segment_size; jv["search_radius"] = search_radius; + jv["interleave_rasters"] = interleave_rasters; + jv["smooth_rasters"] = smooth_rasters; 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; } @@ -73,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 { @@ -98,8 +120,33 @@ 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; + smooth_rasters = validate(jv, "smooth_rasters", Json::ValueType::booleanValue) ? jv["smooth_rasters"].asBool() : + 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) ? + jv["raster_wrt_global_axes"].asBool() : + DEFAULT_RASTER_WRT_GLOBAL_AXES; + if (jv["raster_direction"].isNull() || jv["raster_direction"].type() != Json::ValueType::objectValue) + { + CONSOLE_BRIDGE_logError("Malformed Raster Direction in Json value"); + return false; + } + if (validate(jv["raster_direction"], "x", Json::ValueType::objectValue) && + validate(jv["raster_direction"], "y", Json::ValueType::objectValue) && + validate(jv["raster_direction"], "z", Json::ValueType::objectValue)) + { + raster_direction.x() = jv["raster_direction"]["x"].asDouble(); + raster_direction.y() = jv["raster_direction"]["y"].asDouble(); + raster_direction.z() = jv["raster_direction"]["z"].asDouble(); + } + else + { + raster_direction = Eigen::Vector3d::UnitY(); + } return true; } @@ -121,10 +168,23 @@ 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 << "interleave_rasters: " << interleave_rasters << std::endl; + ss << "smooth_rasters: " << smooth_rasters << 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(); } }; + struct RasterConstructData + { + std::vector> raster_segments; + std::vector segment_lengths; + }; + PlaneSlicerRasterGenerator() = default; /** @@ -134,11 +194,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; @@ -147,12 +204,25 @@ 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 vtkSmartPointer& 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 d2e38d82..6b463b14 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 @@ -43,6 +44,9 @@ #include #include +#include +#include + namespace tool_path_planner { /** @@ -53,10 +57,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 @@ -118,6 +136,59 @@ static std::string getClassName() return (status == 0) ? res.get() : mangled_name; } +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); + +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); +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); + +/** + * @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); + +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); +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 #endif /* INCLUDE_TOOL_PATH_PLANNER_UTILITIES_H_ */ 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/eigen_value_edge_generator.cpp b/tool_path_planner/src/eigen_value_edge_generator.cpp index d411dde7..3c493933 100644 --- a/tool_path_planner/src/eigen_value_edge_generator.cpp +++ b/tool_path_planner/src/eigen_value_edge_generator.cpp @@ -330,6 +330,14 @@ boost::optional EigenValueEdgeGenerator::generate() edge_paths.push_back({ edge_segment }); } } + + if (config_.split_by_axes) + { + edge_paths = tool_path_planner::splitByAxes(edge_paths); + } + + CONSOLE_BRIDGE_logInform("Found %lu valid edge segments", edge_paths.size()); + return edge_paths; } diff --git a/tool_path_planner/src/halfedge_edge_generator.cpp b/tool_path_planner/src/halfedge_edge_generator.cpp index beeb58b5..ab6979ea 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 applyEqualDistance(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; @@ -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 (!applyEqualDistancePathCloud(bound_segment_points, decimated_points, config_.point_dist)) { - CONSOLE_BRIDGE_logError("applyEqualDistance point spacing method failed"); + CONSOLE_BRIDGE_logError("applyEqualDistancePathCloud point spacing method failed"); return boost::none; } break; @@ -556,8 +556,11 @@ boost::optional HalfedgeEdgeGenerator::generate() if (!createToolPathSegment(bound_segment_points, {}, edge_segment)) return boost::none; - edge_paths.push_back({ edge_segment }); - CONSOLE_BRIDGE_logInform("Added boundary with %lu points", edge_paths.back()[0].size()); + ToolPathSegment cleaned_segment = + checkForPeninsulaOrInlet(edge_segment, config_.max_bridge_distance); + + edge_paths.push_back({ cleaned_segment }); + CONSOLE_BRIDGE_logDebug("Added boundary with %lu points", edge_paths.back()[0].size()); } // sorting @@ -580,7 +583,13 @@ boost::optional HalfedgeEdgeGenerator::generate() return boost::none; } + if (config_.split_by_axes) + { + edge_paths = tool_path_planner::splitByAxes(edge_paths); + } + CONSOLE_BRIDGE_logInform("Found %lu valid edge segments", edge_paths.size()); + return edge_paths; } diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index 8bbbefdb..dcb39e3d 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -19,48 +19,122 @@ * 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; -struct RasterConstructData +static Eigen::Vector3d getSegDir(vtkSmartPointer seg) { - std::vector raster_segments; - std::vector segment_lengths; -}; + if (seg->GetPoints()->GetNumberOfPoints() < 1) + { + 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; + v.z() = 0.0; + return (v); + } + 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 tool_path_planner::PlaneSlicerRasterGenerator::RasterConstructData +alignRasterCD(tool_path_planner::PlaneSlicerRasterGenerator::RasterConstructData& rcd, + Eigen::Vector3d& raster_direction) +{ + if (rcd.raster_segments[0]->GetPoints()->GetNumberOfPoints() <= 1) + { + CONSOLE_BRIDGE_logError("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()); + + // 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 + for (size_t i = 0; i < rcd.raster_segments.size(); i++) + { + // determine i'th segments direction + Eigen::Vector3d seg_dir = getSegDir(rcd.raster_segments[i]); + Eigen::Vector3d seg_start; + rcd.raster_segments[i]->GetPoint(0, seg_start.data()); + // 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 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); + 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::pair p(seg_loc, i); + seg_order.push_back(p); + } + // sort the segments by location + if (seg_order.size() >= 1) + { + seg_order.sort(compare_ds_pair); + } + tool_path_planner::PlaneSlicerRasterGenerator::RasterConstructData new_rcd; + 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]); + 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) { @@ -141,7 +215,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; @@ -160,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) @@ -168,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++) { @@ -204,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; @@ -302,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; @@ -318,7 +394,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()); } @@ -326,60 +402,6 @@ static void rectifyDirection(const vtkSmartPointer& points, } } -static tool_path_planner::ToolPaths convertToPoses(const std::vector& rasters_data) -{ - using namespace Eigen; - tool_path_planner::ToolPaths rasters_array; - bool reverse = true; - for (const RasterConstructData& rd : rasters_data) - { - reverse = !reverse; - tool_path_planner::ToolPath raster_path; - std::vector raster_segments; - raster_segments.assign(rd.raster_segments.begin(), rd.raster_segments.end()); - if (reverse) - { - std::reverse(raster_segments.begin(), raster_segments.end()); - } - - for (const PolyDataPtr& polydata : raster_segments) - { - tool_path_planner::ToolPathSegment raster_path_segment; - std::size_t num_points = polydata->GetNumberOfPoints(); - Vector3d p, p_next, vx, vy, vz; - Isometry3d pose; - 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]; - 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(); - vy = vz.cross(vx).normalized(); - vz = vx.cross(vy).normalized(); - pose = Translation3d(p) * AngleAxisd(computeRotation(vx, vy, vz)); - raster_path_segment.push_back(pose); - } - - // adding last pose - pose.translation() = p_next; // orientation stays the same as previous - raster_path_segment.push_back(pose); - - raster_path.push_back(raster_path_segment); - } - rasters_array.push_back(raster_path); - } - - return rasters_array; -} - namespace tool_path_planner { void PlaneSlicerRasterGenerator::setConfiguration(const PlaneSlicerRasterGenerator::Config& config) @@ -387,15 +409,6 @@ void PlaneSlicerRasterGenerator::setConfiguration(const PlaneSlicerRasterGenerat config_ = config; } -void PlaneSlicerRasterGenerator::setInput(pcl::PolygonMesh::ConstPtr mesh) -{ - auto mesh_data = vtkSmartPointer::New(); - pcl::VTKUtils::mesh2vtk(*mesh, mesh_data); - mesh_data->BuildLinks(); - mesh_data->BuildCells(); - setInput(mesh_data); -} - void PlaneSlicerRasterGenerator::setInput(vtkSmartPointer mesh) { if (!mesh_data_) @@ -409,7 +422,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(); @@ -434,6 +446,31 @@ 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(); + 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_)) + { + CONSOLE_BRIDGE_logError("alignToVertexNormals failed"); + } + + setInput(mesh_data); +} + void PlaneSlicerRasterGenerator::setInput(const shape_msgs::Mesh& mesh) { pcl::PolygonMesh::Ptr pcl_mesh = boost::make_shared(); @@ -442,21 +479,155 @@ void PlaneSlicerRasterGenerator::setInput(const shape_msgs::Mesh& mesh) } vtkSmartPointer PlaneSlicerRasterGenerator::getInput() { return mesh_data_; } +void PlaneSlicerRasterGenerator::computePoseData(const vtkSmartPointer& 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(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_start; + vx = (vx - vx.dot(vz) * vz).normalized(); + vy = vz.cross(vx).normalized(); +} + +tool_path_planner::ToolPaths PlaneSlicerRasterGenerator::convertToPoses( + const std::vector& rasters_data) +{ + using namespace Eigen; + tool_path_planner::ToolPaths rasters_array; + for (const tool_path_planner::PlaneSlicerRasterGenerator::RasterConstructData& rd : rasters_data) // for every raster + { + tool_path_planner::ToolPath raster_path; + std::vector> raster_segments; + raster_segments.assign(rd.raster_segments.begin(), rd.raster_segments.end()); + for (const vtkSmartPointer& polydata : raster_segments) // for every segment + { + tool_path_planner::ToolPathSegment raster_path_segment; + std::size_t num_points = polydata->GetNumberOfPoints(); + 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 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++) + { + 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; + } // end for every waypoint + + 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++; + } + raster_path.push_back(raster_path_segment); + } // end for every segment + rasters_array.push_back(raster_path); + } // end for every raster + + return rasters_array; +} boost::optional PlaneSlicerRasterGenerator::generate() { using namespace Eigen; using IDVec = std::vector; - boost::optional rasters = boost::none; + 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()); } - // 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; + } + + // 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 + { + // 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; @@ -490,12 +661,29 @@ 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; - // 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; + if (config_.raster_direction.isApprox(Eigen::Vector3d::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(); + } + 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) // Raster direction specified by user + .normalized(); + } // Calculate all 8 corners projected onto the raster direction vector Eigen::VectorXd dist(8); @@ -516,7 +704,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++) { @@ -549,7 +736,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_); @@ -562,12 +748,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); @@ -626,7 +812,7 @@ boost::optional PlaneSlicerRasterGenerator::generate() 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 @@ -651,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 @@ -672,16 +858,64 @@ 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) + { + CONSOLE_BRIDGE_logError("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) + { + rcd = alignRasterCD(rcd, raster_direction); } - // converting to poses msg now - rasters = convertToPoses(rasters_data_vec); + 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++) + { + rasters_data_vec.push_back(tmp_rasters_data_vec[i]); + } + } + + // converting to poses msg + ToolPaths rasters = convertToPoses(rasters_data_vec); + + if (config_.generate_extra_rasters) + { + 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); + } return rasters; } @@ -708,46 +942,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 closests", - 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; - } + 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++) + vtkIdType p_id = id_list->GetId(0); + if (p_id < 0) { - Eigen::Vector3d temp_normal, query_point, closest_point; - vtkIdType p_id = id_list->GetId(p); - - if (p_id < 0) - { - 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++; + CONSOLE_BRIDGE_logError("%s point id is invalid", getName().c_str()); + continue; } - 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/surface_walk_raster_generator.cpp b/tool_path_planner/src/surface_walk_raster_generator.cpp index c6a3b50f..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::vector #include #include +#include // std::sort +#include #include #include #include @@ -36,9 +38,16 @@ #include #include #include +#include namespace tool_path_planner { +typedef std::tuple wp_tuple; +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 @@ -54,55 +63,73 @@ 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(); + 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()); - // 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())); + 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); - 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); + return tool_path_segment_data; +} - tp_data.push_back(tps_data); - } - results.push_back(tp_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 results; + + 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 tool_paths_data; } Eigen::Matrix3d toRotationMatrix(const Eigen::Vector3d& vx, const Eigen::Vector3d& vy, const Eigen::Vector3d& vz) @@ -124,6 +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.max_bridge_distance = config.max_bridge_distance; return true; } @@ -139,6 +167,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.split_by_axes = config.split_by_axes; return true; } @@ -165,10 +194,18 @@ 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.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(); + config_msg.raster_direction.y = config.raster_direction.y(); + config_msg.raster_direction.z = config.raster_direction.z(); return true; } @@ -183,6 +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.max_bridge_distance = config_msg.max_bridge_distance; return true; } @@ -212,7 +250,6 @@ bool toSurfaceWalkConfig(SurfaceWalkRasterGenerator::Config& config, config.min_segment_size = config_msg.min_segment_size; config.raster_rot_offset = config_msg.raster_rot_offset; config.generate_extra_rasters = config_msg.generate_extra_rasters; - config.cut_direction[0] = config_msg.cut_direction.x; config.cut_direction[1] = config_msg.cut_direction.y; config.cut_direction[2] = config_msg.cut_direction.z; @@ -225,10 +262,25 @@ 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.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; + + // Check that the raster direction was set; we are not interested in direction [0,0,0] + 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 = test_raster_direction; + } return true; } @@ -245,16 +297,39 @@ bool createToolPathSegment(const pcl::PointCloud& cloud_normal 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_normals.points.size(); i++) + { + cloud_indices.push_back(i); + } } else { cloud_indices.assign(indices.begin(), indices.end()); } - for (std::size_t i = 0; i < cloud_indices.size() - 1; 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()) @@ -263,10 +338,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()); @@ -274,14 +351,1054 @@ 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; +} + +// 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(const ToolPaths& tool_paths, double max_segment_length) +{ + ToolPaths new_tool_paths; + for (ToolPath 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; +} + +ToolPaths reverseOddRasters(const ToolPaths& tool_paths, RasterStyle raster_style) +{ + ToolPaths new_tool_paths; + bool is_odd = false; + 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; + } + return new_tool_paths; +} + +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::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; + 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_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) +{ + 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.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 + for (ToolPathSegment seg : temp_path) + { + 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); + } + 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); + } + + // duplicate last raster with an offset and add as last raster + 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() - 2]; + 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); + } + last_dup_tool_path.push_back(new_segment); + } + new_tool_paths.push_back(last_dup_tool_path); + + return new_tool_paths; +} + +ToolPath splitByAxes(const ToolPathSegment& tool_path_segment) +{ + // Sanity check - tool path segment must not be empty + if (tool_path_segment.size() == 0) + { + 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>(); + 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); + } + 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 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) +{ + // Sanity check - tool path segment must not be empty + if (tool_path_segment.size() == 0) + { + ROS_WARN_STREAM("Tool path segment 0 is empty."); + ToolPath tp; + tp.push_back(tool_path_segment); + return tp; + } + + 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; + + 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(); + + 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() - 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) + { + 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 splitByAxes(const ToolPaths& tool_paths) +{ + if (tool_paths.size() == 0) + { + ROS_WARN_STREAM("Tool paths is empty."); + return tool_paths; + } + + 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]; + ToolPath new_tool_path; + for (ToolPathSegment tool_path_segment : 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; +} + +ToolPaths splitByAxes(const ToolPaths& tool_paths, const Eigen::Vector3f& axis_1, const Eigen::Vector3f& axis_2) +{ + if (tool_paths.size() == 0) + { + ROS_WARN_STREAM("Tool paths is empty."); + return tool_paths; + } + + 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]; + ToolPath new_tool_path; + for (ToolPathSegment tool_path_segment : 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; +} + +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) +{ + ToolPathSegment output_segment = tool_path_seg; + bool jumps_exist = true; + 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) + { + 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; + } + + // 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; +} + +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) + double dist = fabs(std::get<0>(*item1) - std::get<0>(*item2)); + if (dist < 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) +{ + ToolPath new_tool_path; + ToolPathSegment seg; + + 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 (wp_tuple waypoint_tuple : waypoint_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 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 > .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 + } + seg.clear(); + seg.push_back(waypoint); + last_wp = waypoint; + last_dot = std::get<0>(waypoint_tuple); + } + 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) + { + // 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); + } + return (new_tool_path); +} + +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 < 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::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 + 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 + { + for (Eigen::Isometry3d waypoint : seg) + { + Eigen::Vector3d v = waypoint.translation() - path_start; + 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 + 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::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 + } + if (i < tool_paths.size() - 1) + { + 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::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 + } + 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::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 + 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; +} + +// 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(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() + + // 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; 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); +} + +/** + * @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() > 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; + 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 %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(); + for (int i = 0; i < mesh.vertices.size(); i++) + { + pcl::PointXYZ p(mesh.vertices[i].x, mesh.vertices[i].y, mesh.vertices[i].z); + mesh_cloud.points.push_back(p); + } +} + +void computeFaceNormals(const shape_msgs::Mesh& mesh, std::vector& face_normals) +{ + face_normals.clear(); + face_normals.reserve(mesh.triangles.size()); + for (size_t i = 0; i < mesh.triangles.size(); i++) + { + size_t idx1 = mesh.triangles[i].vertex_indices[0]; + size_t idx2 = mesh.triangles[i].vertex_indices[1]; + size_t idx3 = mesh.triangles[i].vertex_indices[2]; + Eigen::Vector3d v1(mesh.vertices[idx1].x, mesh.vertices[idx1].y, mesh.vertices[idx1].z); + Eigen::Vector3d v2(mesh.vertices[idx2].x, mesh.vertices[idx2].y, mesh.vertices[idx2].z); + Eigen::Vector3d v3(mesh.vertices[idx3].x, mesh.vertices[idx3].y, mesh.vertices[idx3].z); + Eigen::Vector3d normal = v1.cross(v2) + v2.cross(v3) + v3.cross(v1); + normal.normalize(); + face_normals.push_back(normal); + } +} + +void computeFaceNormals(pcl::PolygonMesh::ConstPtr& mesh, std::vector& face_normals) +{ + face_normals.clear(); + face_normals.reserve(mesh->polygons.size()); + 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) +{ + 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.triangles.size(); i++) // find every face adjoining each vertex + { + vertex_faces[mesh.triangles[i].vertex_indices[0]].push_back(i); + vertex_faces[mesh.triangles[i].vertex_indices[1]].push_back(i); + vertex_faces[mesh.triangles[i].vertex_indices[2]].push_back(i); + } + // average the adjoining face normals + vertex_normals.clear(); + vertex_normals.reserve(mesh.vertices.size()); + for (size_t i = 0; i < mesh.vertices.size(); i++) // for each vertex averages all adjoining face normals + { + Eigen::Vector3d vertex_normal; + if (vertex_faces[i].size() == 0) + { + vertex_normal.z() = 1; + } + else + { + for (size_t j = 0; j < vertex_faces[i].size(); j++) + { + size_t face_index = vertex_faces[i][j]; + vertex_normal += face_normals[face_index]; + } + } + vertex_normal.normalize(); + vertex_normals.push_back(vertex_normal); + } +} + +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; + + // populate a structure for each vertex containing a list of adjoining faces + std::vector vertex_faces[num_pts]; + 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 < num_pts; i++) // for each vertex averages all adjoining face normals + { + Eigen::Vector3d vertex_normal; + if (vertex_faces[i].size() == 0) + { + vertex_normal.z() = 1; + } + else + { + for (size_t j = 0; j < vertex_faces[i].size(); j++) + { + size_t face_index = vertex_faces[i][j]; + vertex_normal += face_normals[face_index]; + } + } + vertex_normal.normalize(); + vertex_normals.push_back(vertex_normal); + } +} + +bool alignToVertexNormals(pcl::PointCloud& 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 < mesh_cloud.points.size(); i++) + { + Eigen::Vector3d n(mesh_cloud.points[i].normal_x, mesh_cloud.points[i].normal_y, mesh_cloud.points[i].normal_z); + if (vertex_normals[i].dot(n) < 0) + { + 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; + } + } + return true; +} } // namespace tool_path_planner diff --git a/tool_path_planner/test/utest.cpp b/tool_path_planner/test/utest.cpp index b5255792..e10b6837 100644 --- a/tool_path_planner/test/utest.cpp +++ b/tool_path_planner/test/utest.cpp @@ -5,9 +5,11 @@ * */ +#include #include #include #include +#include #include #include #include @@ -20,6 +22,17 @@ #define DISPLAY_DERIVATIVES 1 #define POINT_SPACING 0.5 +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 +268,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); @@ -272,6 +286,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,14 +334,15 @@ 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; // Display mesh results color[0] = 0.9f; color[1] = 0.9f; color[2] = 0.9f; viz.addPolyDataDisplay(mesh, color); + viz2.addPolyDataDisplay(mesh, color); // Display surface normals if (DISPLAY_NORMALS) @@ -336,7 +392,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,15 +400,201 @@ 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); } } } + viz2.renderDisplay(); +#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::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; + + 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(); @@ -398,6 +640,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::UnitX(); + // tool.debug = false; + planner.setConfiguration(tool); + runRasterRotationTest(planner, mesh); +} + TEST(IntersectTest, HalfedgeEdgeGeneratorTestCase0) { vtkSmartPointer mesh = createTestMesh1(); @@ -541,6 +814,92 @@ 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)); +} + +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_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); + + 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) { testing::InitGoogleTest(&argc, argv); 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