Feature/style buttons

dependencies.rosinstall

@@ -4,17 +4,18 @@
     uri: 'https://github.com/ros-perception/perception_pcl.git'
     version: 1.7.1
 
-# Noether PR #114
+# Noether 
 - git:
     local-name: noether
     uri: 'https://github.com/drchrislewis/noether.git'
-    version: 357c1fc3914a7789b1878ab8d0fd1347590bba0e
+    version: b5f33c2d8cafdb73f6e7df4ad01ddab5affea518
 
 # heat_raster current master
 - git:
     local-name: heat_raster
-    uri: 'https://github.com/swri-robotics/heat_raster.git'
-    version: 00cf29bd9d31ba44db634c61eebe216a69245263
+    uri: 'https://github.com/drchrislewis/heat_raster.git'
+    version: 328797aa5d2c21c3078078f96ea478d870ea55c2
+
 
 # RViz Tool Cursor
 - git:

opp_gui/CMakeLists.txt

@@ -20,6 +20,8 @@ find_package(catkin REQUIRED COMPONENTS
   pcl_ros
   rviz
   shape_msgs
+  smooth_pose_traj
+  tool_path_planner
   std_srvs
   visualization_msgs
 )

opp_gui/include/opp_gui/widgets/tool_path_parameters_editor_widget.h

@@ -23,9 +23,10 @@
 #include <noether_msgs/GenerateToolPathsAction.h>
 #include <heat_msgs/GenerateHeatToolPathsAction.h>
 #include <heat_msgs/HeatRasterGeneratorConfig.h>
+#include <smooth_pose_traj/SmoothPoseTrajectory.h>
 #include <ros/ros.h>
-
 #include <opp_msgs/ToolPath.h>
+#include <geometry_msgs/PoseArray.h>
 
 namespace Ui
 {
@@ -68,6 +69,8 @@ class ToolPathParametersEditorWidget : public QWidget
 
   heat_msgs::HeatRasterGeneratorConfig getHeatRasterGeneratorConfig() const;
 
+  geometry_msgs::PoseArray compute_pose_arrays(const std::vector<int> pnt_indices);
+
 Q_SIGNALS:
 
   /**
@@ -109,6 +112,7 @@ public Q_SLOTS:
 
   actionlib::SimpleActionClient<noether_msgs::GenerateToolPathsAction> client_;
   actionlib::SimpleActionClient<heat_msgs::GenerateHeatToolPathsAction> heat_client_;
+  ros::ServiceClient polyline_smooth_client_;
 
   Ui::ToolPathParametersEditor* ui_;
 

opp_gui/package.xml

@@ -36,6 +36,8 @@
   <depend>pcl_ros</depend>
   <depend>rviz</depend>
   <depend>shape_msgs</depend>
+  <depend>smooth_pose_traj</depend>
+  <depend>tool_path_planner</depend>
   <depend>std_srvs</depend>
   <depend>visualization_msgs</depend>
 

opp_gui/src/widgets/tool_path_editor_widget.cpp

@@ -266,12 +266,9 @@ void ToolPathEditorWidget::publishToolPathDisplay(const opp_msgs::ToolPath& tool
 
 void ToolPathEditorWidget::onPolylinePath(const std::vector<int> pnt_indices)
 {
-  // TODO: First find shortest path on surface between each segments same as with pathGen
-  // TODO TODO TODO TODO
-  char old_style[255];
-  sprintf(old_style, "new polyline path in ToolPathEditor has %ld pnts", pnt_indices.size());
-  emit QWarningBox(old_style);
+  emit(editor_->polylinePath(pnt_indices));
 }
+
 void ToolPathEditorWidget::onPolylinePathReset(const std::vector<int> pnt_indices)
 {
   std::string msg("Polyline path in ToolPathEditor was reset");

opp_gui/src/widgets/tool_path_parameters_editor_widget.cpp

@@ -27,9 +27,17 @@
 #include <opp_path_selection/path_selection_artist.h>
 #include "opp_gui/utils.h"
 #include "ui_tool_path_parameters_editor.h"
+#include <smooth_pose_traj/SmoothPoseTrajectory.h>
+#include <geometry_msgs/PoseArray.h>
+#include <pcl/point_types.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/surface/mls.h>
+#include <tool_path_planner/utilities.h>
 
 const static std::string GENERATE_TOOLPATHS_ACTION = "generate_tool_paths";
 const static std::string GENERATE_HEAT_TOOLPATHS_ACTION = "generate_heat_tool_paths";
+const static std::string SMOOTH_POLYLINE_SERVICE = "polyline_smoother";
 
 namespace opp_gui
 {
@@ -66,6 +74,8 @@ ToolPathParametersEditorWidget::ToolPathParametersEditorWidget(ros::NodeHandle&
   connect(
       this, &ToolPathParametersEditorWidget::polylinePathGen, this, &ToolPathParametersEditorWidget::onPolylinePathGen);
   connect(this, &ToolPathParametersEditorWidget::QWarningBox, this, &ToolPathParametersEditorWidget::onQWarningBox);
+  connect(this, &ToolPathParametersEditorWidget::polylinePath, this, &ToolPathParametersEditorWidget::onPolylinePath);
+  polyline_smooth_client_ = nh.serviceClient<smooth_pose_traj::SmoothPoseTrajectory>(SMOOTH_POLYLINE_SERVICE);
 }
 
 void ToolPathParametersEditorWidget::init(const shape_msgs::Mesh& mesh) { mesh_.reset(new shape_msgs::Mesh(mesh)); }
@@ -102,7 +112,17 @@ noether_msgs::ToolPathConfig ToolPathParametersEditorWidget::getToolPathConfig()
   config.plane_slicer_generator.min_hole_size = ui_->double_spin_box_min_hole_size->value();
   config.plane_slicer_generator.tool_offset = ui_->double_spin_box_tool_z_offset->value();
   config.plane_slicer_generator.raster_rot_offset = ui_->double_spin_box_raster_angle->value() * M_PI / 180.0;
-
+  config.plane_slicer_generator.search_radius = ui_->double_spin_box_normal_search_radius->value();
+  ;
+  config.plane_slicer_generator.interleave_rasters = ui_->checkBox_interleave_rasters->isChecked();
+  config.plane_slicer_generator.generate_extra_rasters = ui_->checkBox_generate_extra_rasters->isChecked();
+  config.plane_slicer_generator.smooth_rasters = ui_->checkBox_smooth_rasters->isChecked();
+  if (ui_->radioButton_mow_style->isChecked())
+    config.plane_slicer_generator.raster_style = noether_msgs::ToolPathConfig::MOW_RASTER_STYLE;
+  if (ui_->radioButton_paint_style->isChecked())
+    config.plane_slicer_generator.raster_style = noether_msgs::ToolPathConfig::PAINT_RASTER_STYLE;
+  if (ui_->radioButton_read_style->isChecked())
+    config.plane_slicer_generator.raster_style = noether_msgs::ToolPathConfig::READ_RASTER_STYLE;
   return config;
 }
 
@@ -117,7 +137,7 @@ heat_msgs::HeatRasterGeneratorConfig ToolPathParametersEditorWidget::getHeatRast
   config.min_hole_size = ui_->double_spin_box_min_hole_size->value();
   config.min_segment_size = ui_->double_spin_box_min_segment_length->value();
   config.raster_rot_offset = ui_->double_spin_box_raster_angle->value() * M_PI / 180.0;
-  config.generate_extra_rasters = false;  // No option to set this from GUI at present.
+  config.generate_extra_rasters = ui_->checkBox_generate_extra_rasters->isChecked();
   return config;
 }
 
@@ -178,33 +198,86 @@ void ToolPathParametersEditorWidget::generateToolPath()
   progress_dialog_->show();
 }
 
+geometry_msgs::PoseArray ToolPathParametersEditorWidget::compute_pose_arrays(const std::vector<int> pnt_indices)
+{
+  double point_spacing = ui_->double_spin_box_point_spacing->value();
+  double normal_search_radius = ui_->double_spin_box_normal_search_radius->value();
+  geometry_msgs::PoseArray PA;
+
+  // compute vertex normals
+  std::vector<Eigen::Vector3d> vertex_normals, face_normals;
+  tool_path_planner::computeMeshNormals(*mesh_, face_normals, vertex_normals);
+
+  // convert mesh_ to a pcl::PointCloud<pcl::PointNormal>
+  pcl::PointCloud<pcl::PointXYZ>::Ptr mesh_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>());
+  tool_path_planner::shapeMsgToPclPointXYZ(*mesh_, *mesh_cloud_ptr);
+
+  // compute normals of mesh using Moving Least Squares
+  auto mesh_normals_ptr = boost::make_shared<pcl::PointCloud<pcl::PointNormal> >(
+      tool_path_planner::computeMLSMeshNormals(mesh_cloud_ptr, normal_search_radius));
+
+  // align the mls normals to the vertex normals
+  if (!tool_path_planner::alignToVertexNormals(*mesh_normals_ptr, vertex_normals))
+  {
+    ROS_ERROR("alignToVertexNormals failed");
+  }
+
+  // generate equally spaced points
+  pcl::PointCloud<pcl::PointNormal> equal_spaced_points;
+  if (!tool_path_planner::applyEqualDistance(pnt_indices, *mesh_cloud_ptr, equal_spaced_points, point_spacing))
+  {
+    ROS_ERROR("could not find equal spaced points");
+  }
+  else
+  {
+    tool_path_planner::insertPointNormals(mesh_normals_ptr, equal_spaced_points);
+    tool_path_planner::ToolPathSegment segment;
+    if (!tool_path_planner::createToolPathSegment(equal_spaced_points, {}, segment))
+    {
+      ROS_ERROR("failed to create tool path segment from equally spaced points");
+    }
+    else
+    {
+      for (Eigen::Isometry3d pose : segment)
+      {
+        geometry_msgs::Pose gm_pose;
+        Eigen::Quaterniond r(pose.rotation());
+        gm_pose.position.x = pose.translation().x();
+        gm_pose.position.y = pose.translation().y();
+        gm_pose.position.z = pose.translation().z();
+        gm_pose.orientation.x = r.x();
+        gm_pose.orientation.y = r.y();
+        gm_pose.orientation.z = r.z();
+        gm_pose.orientation.w = r.w();
+        PA.poses.push_back(gm_pose);
+      }
+    }
+  }
+  return (PA);
+}
 void ToolPathParametersEditorWidget::onPolylinePath(const std::vector<int> pnt_indices)
 {
   if (!mesh_)
   {
     emit QWarningBox("Mesh has not yet been specified");
     return;
   }
-
-  // Create an action goal with only one set of sources and configs
-  heat_msgs::GenerateHeatToolPathsGoal goal;
-  // copy pnt_indices into goal as the sources
-  heat_msgs::Source S;
-  for (int i = 0; i < pnt_indices.size(); i++)
-  {
-    S.source_indices.push_back(pnt_indices[i]);
-  }
   if (pnt_indices.size() == 0)
   {
-    emit QWarningBox("No path points selected, using raster angle instead");
+    emit QWarningBox("No polyline points yet selected");
+    return;
   }
-  goal.sources.push_back(S);
-  goal.path_configs.push_back(getHeatRasterGeneratorConfig());
-  goal.surface_meshes.push_back(*mesh_);
-  goal.proceed_on_failure = false;
-
-  heat_client_.sendGoal(goal,
-                        boost::bind(&ToolPathParametersEditorWidget::onGenerateHeatToolPathsComplete, this, _1, _2));
+  geometry_msgs::PoseArray output_poses = compute_pose_arrays(pnt_indices);
+
+  actionlib::SimpleClientGoalState state = actionlib::SimpleClientGoalState::SUCCEEDED;
+  heat_msgs::GenerateHeatToolPathsResult goal_;
+  goal_.success = true;
+  goal_.tool_path_validities.push_back(true);
+  heat_msgs::HeatToolPath tool_path;  // geometry_msgs/PoseArray[] paths
+  noether_msgs::ToolPath ntool_path;  // geometry_msgs/PoseArray[] segments
+  tool_path.paths.push_back(output_poses);
+  goal_.tool_raster_paths.push_back(tool_path);
+  auto goal = boost::make_shared<heat_msgs::GenerateHeatToolPathsResult>(goal_);
 
   progress_dialog_ = new QProgressDialog(this);
   progress_dialog_->setModal(true);
@@ -214,6 +287,8 @@ void ToolPathParametersEditorWidget::onPolylinePath(const std::vector<int> pnt_i
 
   progress_dialog_->setValue(progress_dialog_->minimum());
   progress_dialog_->show();
+
+  ToolPathParametersEditorWidget::onGenerateHeatToolPathsComplete(state, goal);
 }
 
 void ToolPathParametersEditorWidget::onPolylinePathGen(const std::vector<int> pnt_indices)
@@ -305,8 +380,7 @@ void ToolPathParametersEditorWidget::onGenerateToolPathsComplete(
       tp.params.config.surface_walk_generator.min_hole_size = ui_->double_spin_box_min_hole_size->value();
       tp.params.config.surface_walk_generator.min_segment_size = ui_->double_spin_box_min_segment_length->value();
       tp.params.config.surface_walk_generator.generate_extra_rasters =
-          false;  // No option to set this from GUI at present.
-      false;      // No option to set this from GUI at present.
+          ui_->checkBox_generate_extra_rasters->isChecked();
       tp.params.config.surface_walk_generator.intersection_plane_height =
           ui_->double_spin_box_intersecting_plane_height->value();
 
@@ -317,6 +391,17 @@ void ToolPathParametersEditorWidget::onGenerateToolPathsComplete(
           ui_->double_spin_box_raster_angle->value() * M_PI / 180.0;
       tp.params.config.plane_slicer_generator.min_hole_size = ui_->double_spin_box_min_hole_size->value();
       tp.params.config.plane_slicer_generator.min_segment_size = ui_->double_spin_box_min_segment_length->value();
+      tp.params.config.plane_slicer_generator.search_radius = ui_->double_spin_box_normal_search_radius->value();
+      tp.params.config.plane_slicer_generator.interleave_rasters = ui_->checkBox_interleave_rasters->isChecked();
+      tp.params.config.plane_slicer_generator.generate_extra_rasters =
+          ui_->checkBox_generate_extra_rasters->isChecked();
+      tp.params.config.plane_slicer_generator.smooth_rasters = ui_->checkBox_smooth_rasters->isChecked();
+      if (ui_->radioButton_mow_style->isChecked())
+        tp.params.config.plane_slicer_generator.raster_style = noether_msgs::ToolPathConfig::MOW_RASTER_STYLE;
+      if (ui_->radioButton_paint_style->isChecked())
+        tp.params.config.plane_slicer_generator.raster_style = noether_msgs::ToolPathConfig::PAINT_RASTER_STYLE;
+      if (ui_->radioButton_read_style->isChecked())
+        tp.params.config.plane_slicer_generator.raster_style = noether_msgs::ToolPathConfig::READ_RASTER_STYLE;
 
       // Create the tool path offset transform
       // 1. Add z offset
@@ -378,7 +463,7 @@ void ToolPathParametersEditorWidget::onGenerateHeatToolPathsComplete(
       tp.params.config.heat_generator.raster_rot_offset = ui_->double_spin_box_raster_angle->value() * M_PI / 180.0;
       tp.params.config.heat_generator.min_hole_size = ui_->double_spin_box_min_hole_size->value();
       tp.params.config.heat_generator.min_segment_size = ui_->double_spin_box_min_segment_length->value();
-      tp.params.config.heat_generator.generate_extra_rasters = false;  // No option to set this from GUI at present.
+      tp.params.config.heat_generator.generate_extra_rasters = ui_->checkBox_generate_extra_rasters->isChecked();
 
       // Create the tool path offset transform
       // 1. Add z offset