Feature new raster method, plane slicer (#82)

Co-authored-by: Levi Armstrong <[email protected]>

.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

noether_conversions/include/noether_conversions/noether_conversions.h

@@ -85,6 +85,24 @@ namespace noether_conversions {
                                                        const std::tuple<float,float,float,float,float,float>& offset = std::make_tuple(
                                                            0.0, 0.0, 0.0, 0.0, 0.0, 0.0));
 
+  visualization_msgs::MarkerArray convertToArrowMarkers(const std::vector<geometry_msgs::PoseArray>& 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<float, float, float, float, float, float>&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<float, float, float, float, float, float>&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,

noether_conversions/src/noether_conversions.cpp

@@ -272,6 +272,83 @@ visualization_msgs::MarkerArray convertToAxisMarkers(const std::vector<geometry_
   return std::move(markers);
 }
 
+visualization_msgs::MarkerArray convertToArrowMarkers(const noether_msgs::ToolRasterPath& toolpath,
+                                                    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<float, float, float, float, float, float>&offset)
+{
+  return convertToArrowMarkers(toolpath.paths,frame_id, ns, start_id, arrow_diameter, point_size, offset);
+}
+
+visualization_msgs::MarkerArray convertToArrowMarkers(const std::vector<geometry_msgs::PoseArray>& 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<float, float, float, float, float, float>&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<int>(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,

noether_examples/launch/rastering_demo.launch

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="mesh_file" />
+  <node launch-prefix="" name="rastering_demo_node" pkg="noether_examples" type="rastering_demo_node" output="screen"> 
+    <param name="mesh_file" value="$(arg mesh_file)"/>
+    <rosparam ns="plane_slicer_rastering">
+      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
+    </rosparam>
+  </node>
+</launch>

noether_examples/src/rastering_demo_node.cpp

@@ -0,0 +1,221 @@
+/**
+ * @author Jorge Nicho <[email protected]>
+ * @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 <ros/ros.h>
+#include <XmlRpcException.h>
+#include <boost/format.hpp>
+#include <boost/filesystem.hpp>
+#include <noether_conversions/noether_conversions.h>
+#include <tool_path_planner/plane_slicer_raster_generator.h>
+#include <visualization_msgs/MarkerArray.h>
+#include <actionlib/client/simple_action_client.h>
+#include <console_bridge/console.h>
+
+using RGBA = std::tuple<double,double,double,double>;
+
+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<visualization_msgs::MarkerArray>(RASTER_LINES_MARKERS_TOPIC,1);
+    raster_poses_markers_pub_ = nh_.advertise<visualization_msgs::MarkerArray>(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<std::string> 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<double>(cfg[field_names[idx++]]);
+      config.point_spacing = static_cast<double>(cfg[field_names[idx++]]);
+      config.raster_rot_offset = static_cast<double>(cfg[field_names[idx++]]);
+      config.min_hole_size = static_cast<double>(cfg[field_names[idx++]]);
+      config.min_segment_size = static_cast<double>(cfg[field_names[idx++]]);
+      config.search_radius = static_cast<double>(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<noether_msgs::ToolRasterPath> > temp = raster_gen.generate(mesh_msg,config);
+    if(!temp)
+    {
+      ROS_ERROR("Failed to generate rasters");
+      return false;
+    }
+    std::vector<noether_msgs::ToolRasterPath> 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"<<std::endl;
+
+
+        std::string ns = RASTER_PATH_NS + std::to_string(i) + std::string("_s[") + std::to_string(j) + std::string("]") ;
+        visualization_msgs::MarkerArray edge_path_axis_markers = convertToAxisMarkers({raster_paths[i].paths[j]},
+                                                                                 DEFAULT_FRAME_ID,
+                                                                                 ns);
+
+        visualization_msgs::MarkerArray edge_path_line_markers = convertToArrowMarkers({raster_paths[i].paths[j]},
+                                                                                 DEFAULT_FRAME_ID,
+                                                                                 ns);
+
+        poses_markers_.markers.insert( poses_markers_.markers.end(),
+                                 edge_path_axis_markers.markers.begin(), edge_path_axis_markers.markers.end());
+        line_markers_.markers.insert( line_markers_.markers.end(),
+                                 edge_path_line_markers.markers.begin(), edge_path_line_markers.markers.end());
+      }
+    }
+    return true;
+  }
+
+private:
+  ros::NodeHandle nh_;
+  ros::Timer markers_publish_timer_;
+  ros::Publisher raster_lines_markers_pub_;
+  ros::Publisher raster_poses_markers_pub_;
+  visualization_msgs::MarkerArray line_markers_;
+  visualization_msgs::MarkerArray poses_markers_;
+
+};
+
+int main(int argc, char** argv)
+{
+  ros::init(argc,argv,"halfedge_finder");
+  console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG);
+  ros::NodeHandle nh;
+  ros::AsyncSpinner spinner(2);
+  spinner.start();
+  Rasterer edge_finder(nh);
+  if(!edge_finder.run())
+  {
+    return -1;
+  }
+  ros::waitForShutdown();
+  return 0;
+}
+
+
+
+
+
+

tool_path_planner/CMakeLists.txt

@@ -64,6 +64,7 @@ include_directories(
 add_library(${PROJECT_NAME}
   src/raster_path_generator.cpp
   src/raster_tool_path_planner.cpp
+  src/plane_slicer_raster_generator.cpp
   src/edge_path_generator.cpp
   src/halfedge_boundary_finder.cpp
   src/utilities.cpp