diff --git a/.github/workflows/focal_build.yml b/.github/workflows/focal_build.yml index 172492ab..1c565e0a 100644 --- a/.github/workflows/focal_build.yml +++ b/.github/workflows/focal_build.yml @@ -22,7 +22,7 @@ jobs: UPSTREAM_WORKSPACE: 'dependencies_ros1.rosinstall' ROSDEP_SKIP_KEYS: "iwyu cmake_common_scripts" CCACHE_DIR: "/home/runner/work/noether/noether/Focal-Build/.ccache" - TARGET_CMAKE_ARGS: "-DNURBS_FOUND=TRUE" + TARGET_CMAKE_ARGS: "-DNURBS_FOUND=FALSE" DOCKER_IMAGE: "rosindustrial/noether:noetic" steps: - uses: actions/checkout@v2 diff --git a/noether_conversions/include/noether_conversions/noether_conversions.h b/noether_conversions/include/noether_conversions/noether_conversions.h index ba2782f2..c7a8ae68 100644 --- a/noether_conversions/include/noether_conversions/noether_conversions.h +++ b/noether_conversions/include/noether_conversions/noether_conversions.h @@ -85,6 +85,24 @@ namespace noether_conversions { const std::tuple& offset = std::make_tuple( 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + visualization_msgs::MarkerArray convertToArrowMarkers(const std::vector& path, + const std::string& frame_id, + const std::string& ns, + const std::size_t start_id = 1, + const float arrow_diameter = 0.002, + const float point_size = 0.01, + const std::tuple&offset = + std::make_tuple(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + + visualization_msgs::MarkerArray convertToArrowMarkers(const noether_msgs::ToolRasterPath& toolpath, + const std::string& frame_id, + const std::string& ns, + const std::size_t start_id = 1, + const float arrow_diameter = 0.002, + const float point_size = 0.01, + const std::tuple&offset = + std::make_tuple(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + visualization_msgs::MarkerArray convertToDottedLineMarker(const noether_msgs::ToolRasterPath& toolpath, const std::string& frame_id, const std::string& ns, diff --git a/noether_conversions/src/noether_conversions.cpp b/noether_conversions/src/noether_conversions.cpp index cb1631db..e77b85ee 100644 --- a/noether_conversions/src/noether_conversions.cpp +++ b/noether_conversions/src/noether_conversions.cpp @@ -272,6 +272,83 @@ visualization_msgs::MarkerArray convertToAxisMarkers(const std::vector&offset) +{ + return convertToArrowMarkers(toolpath.paths,frame_id, ns, start_id, arrow_diameter, point_size, offset); +} + +visualization_msgs::MarkerArray convertToArrowMarkers(const std::vector& path, + const std::string& frame_id, + const std::string& ns, + const std::size_t start_id, + const float arrow_diameter, + const float point_size, + const std::tuple&offset) +{ + visualization_msgs::MarkerArray markers_msgs; + visualization_msgs::Marker arrow_marker, points_marker; + const geometry_msgs::Pose pose_msg = pose3DtoPoseMsg(offset); + + // configure arrow marker + arrow_marker.action = arrow_marker.ADD; + std::tie(arrow_marker.color.r, arrow_marker.color.g, arrow_marker.color.b, arrow_marker.color.a) = std::make_tuple(1.0, 1.0, 0.2, 1.0); + arrow_marker.header.frame_id = frame_id; + arrow_marker.type = arrow_marker.ARROW; + arrow_marker.id = start_id; + arrow_marker.lifetime = ros::Duration(0); + arrow_marker.ns = ns; + std::tie(arrow_marker.scale.x, arrow_marker.scale.y, arrow_marker.scale.z) = std::make_tuple(arrow_diameter, 4.0 * arrow_diameter, 4.0 * arrow_diameter); + + + // configure point marker + points_marker = arrow_marker; + points_marker.type = points_marker.POINTS; + points_marker.ns = ns; + points_marker.pose = pose_msg; + std::tie(points_marker.color.r, points_marker.color.g, points_marker.color.b, points_marker.color.a) = std::make_tuple(0.1, .8, 0.2, 1.0); + std::tie(points_marker.scale.x, points_marker.scale.y, points_marker.scale.z) = std::make_tuple(point_size,point_size,point_size); + + auto transformPoint = [](const geometry_msgs::Pose& pose_msg, const geometry_msgs::Point& p_msg) -> geometry_msgs::Point{ + auto new_p_msg = p_msg; + Eigen::Isometry3d transform; + Eigen::Vector3d p, new_p; + tf::poseMsgToEigen(pose_msg,transform); + tf::pointMsgToEigen(p_msg, p); + new_p = transform * p; + tf::pointEigenToMsg(new_p, new_p_msg); + return new_p_msg; + }; + + int id_counter = static_cast(start_id); + for(auto& poses : path) + { + points_marker.points.clear(); + points_marker.points.push_back(poses.poses.front().position); + for(std::size_t i = 1; i < poses.poses.size(); i++) + { + arrow_marker.points.clear(); + geometry_msgs::Point p_start = transformPoint(pose_msg, poses.poses[i - 1].position); + geometry_msgs::Point p_end = transformPoint(pose_msg,poses.poses[i].position); + arrow_marker.points.push_back(p_start); + arrow_marker.points.push_back(p_end); + arrow_marker.id = (++id_counter); + markers_msgs.markers.push_back(arrow_marker); + } + points_marker.points.push_back(poses.poses.back().position); + + points_marker.id = (++id_counter); + markers_msgs.markers.push_back(points_marker); + } + + return markers_msgs; +} + visualization_msgs::MarkerArray convertToDottedLineMarker(const noether_msgs::ToolRasterPath& toolpath, const std::string& frame_id, const std::string& ns, diff --git a/noether_examples/launch/rastering_demo.launch b/noether_examples/launch/rastering_demo.launch new file mode 100644 index 00000000..069e1e72 --- /dev/null +++ b/noether_examples/launch/rastering_demo.launch @@ -0,0 +1,15 @@ + + + + + + + raster_spacing: 0.05 + point_spacing: 0.02 + raster_rot_offset: 0.0 + min_hole_size: 0.02 + min_segment_size: 0.01 + search_radius: 0.02 + + + diff --git a/noether_examples/src/rastering_demo_node.cpp b/noether_examples/src/rastering_demo_node.cpp new file mode 100644 index 00000000..5df7b7d0 --- /dev/null +++ b/noether_examples/src/rastering_demo_node.cpp @@ -0,0 +1,221 @@ +/** + * @author Jorge Nicho + * @file rastering_demo_node.cpp + * @date Dec 27, 2019 + * @copyright Copyright (c) 2019, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using RGBA = std::tuple; + +static const std::string DEFAULT_FRAME_ID = "world"; +static const std::string RASTER_LINES_MARKERS_TOPIC ="raster_lines"; +static const std::string RASTER_POSES_MARKERS_TOPIC ="raster_poses"; +static const std::string RASTER_PATH_NS = "raster_"; +static const std::string INPUT_MESH_NS = "input_mesh"; +static const RGBA RAW_MESH_RGBA = std::make_tuple(0.6, 0.6, 1.0, 1.0); + +class Rasterer +{ +public: + + Rasterer(ros::NodeHandle nh): + nh_(nh) + { + raster_lines_markers_pub_ = nh_.advertise(RASTER_LINES_MARKERS_TOPIC,1); + raster_poses_markers_pub_ = nh_.advertise(RASTER_POSES_MARKERS_TOPIC,1); + } + + ~Rasterer() + { + + } + + bool loadConfig(tool_path_planner::PlaneSlicerRasterGenerator::Config& config) + { + ros::NodeHandle ph("~"); + XmlRpc::XmlRpcValue cfg; + if(!ph.getParam("plane_slicer_rastering", cfg)) + { + return false; + } + + /* + * load configuration into the following struct type + struct Config + { + double raster_spacing = 0.04; + double point_spacing = 0.01; + double raster_rot_offset = 0.0; + double min_hole_size = 0.01; + double min_segment_size = 0.01; + double search_radius = 0.01; + }; + */ + + std::vector field_names = {"raster_spacing", + "point_spacing", + "raster_rot_offset", + "min_hole_size", + "min_segment_size", + "search_radius"}; + if(!std::all_of(field_names.begin(), field_names.end(), [&cfg](const std::string& f){ + return cfg.hasMember(f); + })) + { + ROS_ERROR("Failed to find one or more members"); + } + + std::size_t idx = 0; + try + { + config.raster_spacing = static_cast(cfg[field_names[idx++]]); + config.point_spacing = static_cast(cfg[field_names[idx++]]); + config.raster_rot_offset = static_cast(cfg[field_names[idx++]]); + config.min_hole_size = static_cast(cfg[field_names[idx++]]); + config.min_segment_size = static_cast(cfg[field_names[idx++]]); + config.search_radius = static_cast(cfg[field_names[idx++]]); + } + catch(XmlRpc::XmlRpcException& e) + { + ROS_ERROR_STREAM(e.getMessage()); + } + return true; + } + + bool run() + { + using namespace noether_conversions; + + // loading parameters + ros::NodeHandle ph("~"); + std::string mesh_file; + if(!ph.getParam("mesh_file",mesh_file)) + { + ROS_ERROR("Failed to load one or more parameters"); + return false; + } + + ROS_INFO("Got mesh file %s", mesh_file.c_str()); + + // get configuration + tool_path_planner::PlaneSlicerRasterGenerator::Config config; + if(!loadConfig(config)) + { + ROS_WARN("Failed to load configuration, using default parameters"); + } + + markers_publish_timer_ = nh_.createTimer(ros::Duration(0.5),[&](const ros::TimerEvent& e){ + if(!line_markers_.markers.empty()) + { + raster_lines_markers_pub_.publish(line_markers_); + } + + if(!poses_markers_.markers.empty()) + { + raster_poses_markers_pub_.publish(poses_markers_); + } + }); + + shape_msgs::Mesh mesh_msg; + if(!noether_conversions::loadPLYFile(mesh_file,mesh_msg)) + { + ROS_ERROR("Failed to read file %s",mesh_file.c_str()); + return false; + } + line_markers_.markers.push_back(createMeshMarker(mesh_file,INPUT_MESH_NS,DEFAULT_FRAME_ID,RAW_MESH_RGBA)); + + + + // rastering + tool_path_planner::PlaneSlicerRasterGenerator raster_gen; + boost::optional< std::vector > temp = raster_gen.generate(mesh_msg,config); + if(!temp) + { + ROS_ERROR("Failed to generate rasters"); + return false; + } + std::vector raster_paths = *temp; + + ROS_INFO("Found %lu rasters",raster_paths.size()); + + for(std::size_t i = 0; i < raster_paths.size(); i++) + { + ROS_INFO("Raster %lu contains %lu segments",i, raster_paths[i].paths.size()); + for(std::size_t j = 0; j < raster_paths[i].paths.size(); j++) + { + std::cout<<"\tSegment "<< j << " contains "<< raster_paths[i].paths[j].poses.size() << " points"< + * @file plane_slicer_raster_generator.h + * @date Dec 26, 2019 + * @copyright Copyright (c) 2019, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef INCLUDE_PLANE_SLICER_RASTER_GENERATOR_H_ +#define INCLUDE_PLANE_SLICER_RASTER_GENERATOR_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace tool_path_planner +{ +class PlaneSlicerRasterGenerator +{ +public: + + struct Config + { + double raster_spacing = 0.04; + double point_spacing = 0.01; + double raster_rot_offset = 0.0; + double min_segment_size = 0.01; + double search_radius = 0.01; + double min_hole_size = 1e-2; + + std::string str() + { + std::stringstream ss; + ss<<"raster_spacing: "<< raster_spacing< > generate(const PlaneSlicerRasterGenerator::Config& config); + + /** + * @brief Generate the raster paths that follow the contour of the mesh + * @param mesh The input mesh from which raster paths will be generated + * @param config The configuration + * @return An array of raster paths or boost::none when it fails. + */ + boost::optional< std::vector > generate(const shape_msgs::Mesh& mesh, + const PlaneSlicerRasterGenerator::Config& config); + + /** + * @brief Generate the raster paths that follow the contour of the mesh + * @param mesh The input mesh from which raster paths will be generated + * @param config The configuration + * @return An array of raster paths or boost::none when it fails. + */ + boost::optional< std::vector > generate(pcl::PolygonMesh::ConstPtr mesh, + const PlaneSlicerRasterGenerator::Config& config); + /** + * @brief the class name + * @return a string + */ + std::string getName(); + +private: + + bool insertNormals(const double search_radius, vtkSmartPointer& data); + + vtkSmartPointer mesh_data_; + vtkSmartPointer kd_tree_; + vtkSmartPointer cell_locator_; +}; + +} /* namespace tool_path_planner */ + +#endif /* INCLUDE_PLANE_SLICER_RASTER_GENERATOR_H_ */ diff --git a/tool_path_planner/include/tool_path_planner/utilities.h b/tool_path_planner/include/tool_path_planner/utilities.h index e40149f5..d63d5019 100644 --- a/tool_path_planner/include/tool_path_planner/utilities.h +++ b/tool_path_planner/include/tool_path_planner/utilities.h @@ -24,11 +24,13 @@ #define INCLUDE_TOOL_PATH_PLANNER_UTILITIES_H_ #include +#include #include #include #include #include #include +#include #include "tool_path_planner_base.h" @@ -45,6 +47,13 @@ namespace tool_path_planner std::vector convertVTKtoGeometryMsgs( const std::vector& paths); + /** + * @brief conversion function. Not well tested yet + * @param paths The toolpaths within a raster + * @return a vector to process paths + */ + std::vector toNoetherToolpaths(const noether_msgs::ToolRasterPath& paths); + /** * @brief Convenience conversion function * @param paths The tool paths @@ -90,6 +99,20 @@ namespace tool_path_planner bool createPoseArray(const pcl::PointCloud& cloud_normals, const std::vector& indices, geometry_msgs::PoseArray& poses); + template + static std::string getClassName() { + + int status = -4; // some arbitrary value to eliminate the compiler warning + const char* mangled_name = typeid(C).name(); + + // enable c++11 by passing the flag -std=c++11 to g++ + std::unique_ptr res { + abi::__cxa_demangle(mangled_name, NULL, NULL, &status), + std::free + }; + return (status==0) ? res.get() : mangled_name ; + } + } diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp new file mode 100644 index 00000000..4d9f2994 --- /dev/null +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -0,0 +1,757 @@ +/** + * @author Jorge Nicho + * @file plane_slicer_raster_generator.cpp + * @date Dec 26, 2019 + * @copyright Copyright (c) 2019, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * 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 "tool_path_planner/utilities.h" +#include "tool_path_planner/plane_slicer_raster_generator.h" + +static const double EPSILON = 1e-6; + +using PolyDataPtr = vtkSmartPointer; +struct RasterConstructData +{ + std::vector raster_segments; + std::vector segment_lengths; +}; + +static Eigen::Matrix3d computeRotation(const Eigen::Vector3d& vx, const Eigen::Vector3d& vy, const Eigen::Vector3d& vz) +{ + Eigen::Matrix3d m; + m.setIdentity(); + m.col(0) = vx.normalized(); + m.col(1) = vy.normalized(); + m.col(2) = vz.normalized(); + return m; +} + +static vtkSmartPointer toVtkMatrix(const Eigen::Affine3d& t) +{ + vtkSmartPointer vtk_t = vtkSmartPointer::New(); + vtk_t->PreMultiply(); + vtk_t->Identity(); + vtk_t->Translate(t.translation().x(),t.translation().y(), t.translation().z()); + Eigen::Vector3d rpy = t.rotation().eulerAngles(0,1,2); + vtk_t->RotateX(vtkMath::DegreesFromRadians(rpy(0))); + vtk_t->RotateY(vtkMath::DegreesFromRadians(rpy(1))); + vtk_t->RotateZ(vtkMath::DegreesFromRadians(rpy(2))); + vtk_t->Scale(1.0,1.0,1.0); + return vtk_t; +} + +double computeLength(const vtkSmartPointer& points) +{ + std::size_t num_points = points->GetNumberOfPoints(); + double total_length = 0.0; + if(num_points < 2) + { + return total_length; + } + + Eigen::Vector3d p0, pf; + for(std::size_t i = 1; i < num_points; i++) + { + points->GetPoint(i-1,p0.data()); + points->GetPoint(i,pf.data()); + + total_length += (pf - p0).norm(); + } + return total_length; +} + +static vtkSmartPointer applyParametricSpline(const vtkSmartPointer& points,double total_length, + double point_spacing) +{ + vtkSmartPointer new_points = vtkSmartPointer::New(); + + // create spline + vtkSmartPointer spline = vtkSmartPointer::New(); + spline->SetPoints(points); + spline->SetParameterizeByLength(true); + spline->ClosedOff(); + + // adding first point + Eigen::Vector3d pt_prev; + points->GetPoint(0,pt_prev.data()); + new_points->InsertNextPoint(pt_prev.data()); + + // adding remaining points by evaluating spline + double incr = point_spacing/total_length; + std::size_t num_points = std::ceil(total_length/point_spacing) + 1; + double du[9]; + Eigen::Vector3d u, pt; + for(std::size_t i = 1; i < num_points; i++) + { + double interv = static_cast(i)/static_cast(num_points-1); + interv = interv > 1.0 ? 1.0 : interv ; + if(std::abs(interv - 1.0) < EPSILON) + { + break; // reach end + } + + u = interv * Eigen::Vector3d::Ones(); + std::tie(u[0], u[1], u[2]) = std::make_tuple(interv, interv, interv); + spline->Evaluate(u.data(), pt.data(), du); + + // check distance + if((pt - pt_prev).norm() >= point_spacing) + { + new_points->InsertNextPoint(pt.data()); + pt_prev = pt; + } + } + + // add last point + points->GetPoint(points->GetNumberOfPoints() - 1 ,pt_prev.data()); + new_points->InsertNextPoint(pt_prev.data()); + + return new_points; +} + +/** + * @brief removes points that appear in multiple lists such that only one instance of that point + * index remains + * @param points_lists + */ +static void removeRedundant(std::vector< std::vector >& points_lists) +{ + using IdList = std::vector; + if(points_lists.size() < 2) + { + return; + } + + std::vector< std::vector > new_points_lists; + new_points_lists.push_back(points_lists.front()); + for(std::size_t i = 1; i < points_lists.size(); i++) + { + IdList& current_list = points_lists[i]; + IdList new_list; + IdList all_ids; + + // create list of all ids + for(auto& ref_list : new_points_lists) + { + all_ids.insert(all_ids.end(), ref_list.begin(), ref_list.end()); + } + + for(auto& id : current_list) + { + // add if not found in any of the previous lists + if(std::find(all_ids.begin(), all_ids.end(),id) == all_ids.end()) + { + new_list.push_back(id); + } + } + + // add if it has enough points + if(new_list.size() > 0) + { + new_points_lists.push_back(new_list); + } + } + + points_lists.clear(); + points_lists.assign(new_points_lists.begin(), new_points_lists.end()); +} + +static void mergeRasterSegments(const vtkSmartPointer& points, double merge_dist, + std::vector< std::vector >& points_lists) +{ + using namespace Eigen; + using IdList = std::vector; + if(points_lists.size() < 2) + { + return; + } + + std::vector new_points_lists; + IdList merged_list_ids; + IdList merged_list; + + auto do_merge = [&points](const IdList& current_list, const IdList& next_list, double merge_dist, + IdList& merged_list ){ + Vector3d cl_point, nl_point; + + // checking front and back end points respectively + points->GetPoint(current_list.front(),cl_point.data()); + points->GetPoint(next_list.back(),nl_point.data()); + double d = (cl_point - nl_point).norm(); + if(d < merge_dist) + { + merged_list.assign(next_list.begin(), next_list.end()); + merged_list.insert(merged_list.end(),current_list.begin(), current_list.end()); + return true; + } + + // checking back and front end points respectively + points->GetPoint(current_list.back(),cl_point.data()); + points->GetPoint(next_list.front(),nl_point.data()); + d = (cl_point - nl_point).norm(); + if(d < merge_dist) + { + merged_list.assign(current_list.begin(), current_list.end()); + merged_list.insert(merged_list.end(),next_list.begin(), next_list.end()); + return true; + } + return false; + }; + + for(std::size_t i = 0; i < points_lists.size() ; i++) + { + if(std::find(merged_list_ids.begin(), merged_list_ids.end(),i) != merged_list_ids.end()) + { + // already merged + CONSOLE_BRIDGE_logDebug("Segment %i has already been merged, skipping", i); + continue; + } + + IdList current_list = points_lists[i]; + Vector3d cl_point, nl_point; + bool seek_adjacent = true; + while(seek_adjacent) + { + seek_adjacent = false; + for(std::size_t j = i+1; j < points_lists.size() ; j++) + { + if(std::find(merged_list_ids.begin(), merged_list_ids.end(),j) != merged_list_ids.end()) + { + // already merged + CONSOLE_BRIDGE_logDebug("Segment %i has already been merged, skipping", j); + continue; + } + + merged_list.clear(); + IdList next_list = points_lists[j]; + if(do_merge(current_list,next_list,merge_dist,merged_list)) + { + CONSOLE_BRIDGE_logDebug("Merged segment %lu onto segment %lu",j, i); + current_list = merged_list; + merged_list_ids.push_back(j); + seek_adjacent = true; + continue; + } + + std::reverse(next_list.begin(), next_list.end()); + if(do_merge(current_list,next_list,merge_dist,merged_list)) + { + CONSOLE_BRIDGE_logDebug("Merged segment %lu onto segment %lu",j, i); + current_list = merged_list; + merged_list_ids.push_back(j); + seek_adjacent = true; + continue; + } + } + } + new_points_lists.push_back(current_list); + } + points_lists.clear(); + std::copy_if(new_points_lists.begin(), new_points_lists.end(),std::back_inserter(points_lists),[](const IdList& l){ + return l.size() > 1; + }); + CONSOLE_BRIDGE_logDebug("Final raster contains %lu segments", points_lists.size()); + +} + +static void rectifyDirection(const vtkSmartPointer& points, const Eigen::Vector3d& ref_point, + std::vector< std::vector >& points_lists ) +{ + using namespace Eigen; + Vector3d p0, pf; + if(points_lists.empty()) + { + return; + } + + // getting first and last points + points->GetPoint( points_lists.front().front(),p0.data()); + points->GetPoint( points_lists.back().back(),pf.data()); + + bool reverse = (ref_point - p0).norm() > (ref_point - pf).norm(); + if(reverse) + { + for(auto& s: points_lists) + { + std::reverse(s.begin(),s.end()); + } + std::reverse(points_lists.begin(), points_lists.end()); + } +} + +static std::vector convertToPoses(const std::vector& rasters_data) +{ + using namespace Eigen; + std::vector rasters_array; + bool reverse = true; + for(const RasterConstructData& rd : rasters_data) + { + reverse = !reverse; + noether_msgs::ToolRasterPath raster_path_msg; + 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) + { + std::vector > raster_poses; + 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_poses.push_back(pose); + } + + // adding last pose + pose.translation() = p_next; // orientation stays the same as previous + raster_poses.push_back(pose); + + // convert to poses msg + geometry_msgs::PoseArray raster_poses_msg; + std::transform(raster_poses.begin(), raster_poses.end(), + std::back_inserter(raster_poses_msg.poses),[](const Isometry3d& p){ + geometry_msgs::Pose pose_msg; + tf::poseEigenToMsg(p,pose_msg); + return pose_msg; + }); + raster_path_msg.paths.push_back(raster_poses_msg); + + } + rasters_array.push_back(raster_path_msg); + } + + return rasters_array; +} + +namespace tool_path_planner +{ +PlaneSlicerRasterGenerator::PlaneSlicerRasterGenerator() +{ + +} + +PlaneSlicerRasterGenerator::~PlaneSlicerRasterGenerator() +{ + +} + +void PlaneSlicerRasterGenerator::setInput(pcl::PolygonMesh::ConstPtr mesh) +{ + mesh_data_ = vtkSmartPointer::New(); + pcl::VTKUtils::mesh2vtk(*mesh, mesh_data_); + mesh_data_->BuildLinks(); + mesh_data_->BuildCells(); + + if(mesh_data_->GetPointData()->GetNormals() && mesh_data_->GetCellData()->GetNormals() ) + { + return; // normal data is available + } + + CONSOLE_BRIDGE_logWarn("%s generating normal data", getName().c_str()); + vtkSmartPointer normal_generator = vtkSmartPointer::New(); + normal_generator->SetInputData(mesh_data_); + normal_generator->ComputePointNormalsOn(); + normal_generator->SetComputeCellNormals(!mesh_data_->GetCellData()->GetNormals()); + normal_generator->SetFeatureAngle(M_PI_2); + normal_generator->SetSplitting(true); + normal_generator->SetConsistency(true); + normal_generator->SetAutoOrientNormals(false); + normal_generator->SetFlipNormals(false); + normal_generator->SetNonManifoldTraversal(false); + normal_generator->Update(); + + + if ( !mesh_data_->GetPointData()->GetNormals()) + { + mesh_data_->GetPointData()->SetNormals(normal_generator->GetOutput()->GetPointData()->GetNormals()); + } + + if ( !mesh_data_->GetCellData()->GetNormals() ) + { + mesh_data_->GetCellData()->SetNormals(normal_generator->GetOutput()->GetCellData()->GetNormals()); + } + +} + +void PlaneSlicerRasterGenerator::setInput(const shape_msgs::Mesh& mesh) +{ + pcl::PolygonMesh::Ptr pcl_mesh = boost::make_shared(); + noether_conversions::convertToPCLMesh(mesh,*pcl_mesh); + setInput(pcl_mesh); +} + +boost::optional > +PlaneSlicerRasterGenerator::generate(const PlaneSlicerRasterGenerator::Config& config) +{ + using namespace Eigen; + using IDVec = std::vector; + + boost::optional > rasters = boost::none; + if(!mesh_data_) + { + CONSOLE_BRIDGE_logDebug("%s No mesh data has been provided",getName().c_str()); + } + // 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()); + + // Compute the center of mass + Vector3d origin; + vtkSmartPointer cog_filter = vtkSmartPointer::New(); + cog_filter->SetInputData(mesh_data_); + cog_filter->SetUseScalarsAsWeights(false); + cog_filter->Update(); + cog_filter->GetCenter(origin.data()); + + // computing transformation matrix + Affine3d t = Translation3d(origin) * AngleAxisd(computeRotation(x_dir, y_dir, z_dir)); + Affine3d t_inv = t.inverse(); + + // transforming data + vtkSmartPointer vtk_transform = toVtkMatrix(t_inv); + + vtkSmartPointer transform_filter = vtkSmartPointer::New(); + transform_filter->SetInputData(mesh_data_); + transform_filter->SetTransform(vtk_transform); + transform_filter->Update(); + vtkSmartPointer transformed_mesh_data = transform_filter->GetPolyDataOutput(); + + // compute bounds + VectorXd bounds(6), abs_bounds(6); + transformed_mesh_data->GetBounds(bounds.data()); + abs_bounds = bounds.cwiseAbs(); + + // calculating size + sizes.x() = abs_bounds[0] > abs_bounds[1] ? 2.0 * abs_bounds[0] : 2.0 *abs_bounds[1]; + sizes.y() = abs_bounds[2] > abs_bounds[3] ? 2.0 * abs_bounds[2] : 2.0 *abs_bounds[3]; + sizes.z() = abs_bounds[4] > abs_bounds[5] ? 2.0 * abs_bounds[4] : 2.0 *abs_bounds[5]; + + // now cutting the mesh with planes along the y axis + std::size_t num_planes = std::ceil(sizes[1]/config.raster_spacing); + Isometry3d rotation_offset = Isometry3d::Identity() * AngleAxisd(config.raster_rot_offset, Vector3d::UnitZ()); + Vector3d raster_dir = rotation_offset * Vector3d::UnitY(); + Vector3d start_loc = -config.raster_spacing * std::ceil(num_planes * 0.5)* raster_dir + Vector3d::Zero(); + + vtkSmartPointer raster_data = vtkSmartPointer::New(); + for(int i = 0; i < num_planes + 1 ; i++) + { + vtkSmartPointer plane = vtkSmartPointer::New(); + vtkSmartPointer cutter = vtkSmartPointer::New(); + vtkSmartPointer stripper = vtkSmartPointer::New(); + + Vector3d current_loc = start_loc + i * config.raster_spacing * raster_dir; + plane->SetOrigin(current_loc.x(), current_loc.y(), current_loc.z()); + plane->SetNormal(raster_dir.x() ,raster_dir.y(), raster_dir.z()); + + cutter->SetCutFunction(plane); + cutter->SetInputData(transformed_mesh_data); + cutter->SetSortBy(1); + cutter->SetGenerateTriangles(false); + cutter->Update(); + + stripper->SetInputConnection(cutter->GetOutputPort()); + stripper->JoinContiguousSegmentsOn(); + stripper->SetMaximumLength(mesh_data_->GetNumberOfPoints()); + stripper->Update(); + + if(stripper->GetErrorCode() != vtkErrorCode::NoError) + { + continue; + } + + for(int r = 0; r < stripper->GetNumberOfOutputPorts(); r++) + { + 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_); + kd_tree_->BuildLocator(); + + cell_locator_ = vtkSmartPointer::New(); + cell_locator_->SetDataSet(mesh_data_); + cell_locator_->BuildLocator(); + + // collect rasters and set direction + raster_data->Update(); + std::size_t num_slices = raster_data->GetTotalNumberOfInputConnections(); + 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; + + // collecting raster segments based on min hole size + vtkSmartPointer raster_lines = raster_data->GetInput(i); + vtkIdType* indices; + vtkIdType num_points; + vtkIdType num_lines = raster_lines->GetNumberOfLines(); + vtkPoints* points = raster_lines->GetPoints(); + vtkCellArray* cells = raster_lines->GetLines(); + + CONSOLE_BRIDGE_logDebug("%s raster %i has %i lines and %i points",getName().c_str(), i, + raster_lines->GetNumberOfLines(), raster_lines->GetNumberOfPoints()); + + if(num_lines ==0) + { + continue; + } + + raster_ids.clear(); + + unsigned int lineCount = 0; + for (cells->InitTraversal(); cells->GetNextCell(num_points, indices); + lineCount++) + { + IDVec point_ids; + for (vtkIdType i = 0; i < num_points; i++) + { + if(std::find(point_ids.begin(), point_ids.end(), indices[i]) != point_ids.end()) + { + continue; + } + point_ids.push_back(indices[i]); + } + if(point_ids.empty()) + { + continue; + } + + // removing duplicates + auto iter = std::unique(point_ids.begin(), point_ids.end()); + point_ids.erase(iter,point_ids.end()); + + // adding + raster_ids.push_back(point_ids); + } + + if(raster_ids.empty()) + { + continue; + } + + // remove redundant indices + removeRedundant(raster_ids); + + // merging segments + mergeRasterSegments(raster_lines->GetPoints(),config.min_hole_size,raster_ids); + + // rectifying + if(!rasters_data_vec.empty()) + { + Vector3d ref_point; + rasters_data_vec.back().raster_segments.front()->GetPoint(0,ref_point.data()); // first point in previous raster + rectifyDirection(raster_lines->GetPoints(), t_inv * ref_point, raster_ids); + } + + for(auto& rpoint_ids: raster_ids) + { + // Populating with points + vtkSmartPointer points = vtkSmartPointer::New(); + std::for_each(rpoint_ids.begin(), rpoint_ids.end(),[&points, &raster_lines](vtkIdType& id){ + std::array p; + raster_lines->GetPoint(id, p.data()); + points->InsertNextPoint(p.data()); + }); + + // compute length and add points if segment length is greater than threshold + double line_length = computeLength(points); + if(line_length > config.min_segment_size && points->GetNumberOfPoints()> 1) + { + // enforce point spacing + decltype(points) new_points = applyParametricSpline(points,line_length,config.point_spacing); + + // add points to segment now + PolyDataPtr segment_data = PolyDataPtr::New(); + segment_data->SetPoints(new_points); + + // transforming to original coordinate system + transform_filter = vtkSmartPointer::New(); + transform_filter->SetInputData(segment_data); + transform_filter->SetTransform(toVtkMatrix(t)); + transform_filter->Update(); + segment_data = transform_filter->GetPolyDataOutput(); + + // inserting normals + if(!insertNormals(config.search_radius,segment_data)) + { + CONSOLE_BRIDGE_logError("%s failed to insert normals to segment %lu of raster %lu", + getName().c_str(), r.raster_segments.size(), + i); + return boost::none; + } + + // saving into raster + r.raster_segments.push_back(segment_data); + r.segment_lengths.push_back(line_length); + } + } + + rasters_data_vec.push_back(r); + } + + // converting to poses msg now + rasters = convertToPoses(rasters_data_vec); + return rasters; +} + +boost::optional > +PlaneSlicerRasterGenerator::generate(const shape_msgs::Mesh& mesh, const PlaneSlicerRasterGenerator::Config& config) +{ + setInput(mesh); + return generate(config); +} + +boost::optional > +PlaneSlicerRasterGenerator::generate(pcl::PolygonMesh::ConstPtr mesh, const PlaneSlicerRasterGenerator::Config& config) +{ + setInput(mesh); + return generate(config); +} + +bool PlaneSlicerRasterGenerator::insertNormals(const double search_radius, + vtkSmartPointer& data) +{ + // Find closest cell to each point and uses its normal vector + vtkSmartPointer new_normals = vtkSmartPointer::New(); + new_normals->SetNumberOfComponents(3); + new_normals->SetNumberOfTuples(data->GetPoints()->GetNumberOfPoints()); + + // get normal data + vtkSmartPointer normal_data = mesh_data_->GetPointData()->GetNormals(); + + if(!normal_data) + { + CONSOLE_BRIDGE_logError("%s Normal data is not available", getName().c_str()); + return false; + } + + Eigen::Vector3d normal_vect = Eigen::Vector3d::UnitZ(); + for(int i = 0; i < data->GetPoints()->GetNumberOfPoints(); ++i) + { + // locate closest cell + 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); + 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; + } + } + + // compute normal average + normal_vect = Eigen::Vector3d::Zero(); + std::size_t num_normals = 0; + for(std::size_t p = 0; p < id_list->GetNumberOfIds(); p++) + { + Eigen::Vector3d temp_normal, query_point, closest_point; + vtkIdType p_id = id_list->GetId(p); + vtkIdType cell_id; + int sub_index; + double dist; + + 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++; + } + + normal_vect /= num_normals; + normal_vect.normalize(); + + // save normal + new_normals->SetTuple3(i,normal_vect(0),normal_vect(1),normal_vect(2)); + } + data->GetPointData()->SetNormals(new_normals); + return true; +} + +std::string PlaneSlicerRasterGenerator::getName() +{ + return getClassName(); +} + +} /* namespace tool_path_planner */ diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index 6abe85cd..0fde9a5c 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -27,6 +27,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -150,6 +153,53 @@ std::vector convertVTKtoGeometryMsgs( return poseArrayVector; } +std::vector toNoetherToolpaths(const noether_msgs::ToolRasterPath& paths) +{ + using namespace Eigen; + std::vector noether_tp; + + for(auto& s : paths.paths) + { + tool_path_planner::ProcessPath tpp; + tpp.line = vtkSmartPointer::New(); + tpp.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(s.poses.size()); + vtkSmartPointer der_normals = vtkSmartPointer::New();; + der_normals->SetNumberOfComponents(3); //3d normals (ie x,y,z) + der_normals->SetNumberOfTuples(s.poses.size()); + + int idx = 0; + for(auto& pose : s.poses) + { + Isometry3d pose_eigen; + Vector3d point, vx, vy, vz; + tf::poseMsgToEigen(pose, pose_eigen); + point = pose_eigen.translation(); + vx = pose_eigen.linear().col(0); + vx *= -1.0; + vy = pose_eigen.linear().col(1); + vz = pose_eigen.linear().col(2); + points->InsertNextPoint(point.data()); + line_normals->SetTuple(idx,vz.data()); + der_normals->SetTuple(idx,vx.data()); + + idx++; + } + tpp.line->SetPoints(points); + tpp.line->GetPointData()->SetNormals(line_normals); + tpp.derivatives->GetPointData()->SetNormals(der_normals); + tpp.derivatives->SetPoints(points); + + noether_tp.push_back(tpp); + } + return noether_tp; +} + std::vector toPosesMsgs(const std::vector& paths) { std::vector poseArrayVector;