Added viewer to TPP pipeline application (ros-industrial#191)

* Added viewer to TPP pipeline application

* Added CLI argument parser for GUI application to populate the mesh file and/or TPP config file

noether_gui/CMakeLists.txt

@@ -7,6 +7,7 @@ extract_package_metadata(pkg)
 project(${pkg_extracted_name} VERSION ${pkg_extracted_version} LANGUAGES CXX)
 
 find_package(Eigen3 REQUIRED)
+find_package(Boost REQUIRED COMPONENTS program_options)
 find_package(boost_plugin_loader REQUIRED)
 find_package(noether_tpp REQUIRED)
 find_package(PCL REQUIRED COMPONENTS io surface)
@@ -122,7 +123,7 @@ target_code_coverage(${PROJECT_NAME}_plugins INTERFACE ALL ENABLE ${NOETHER_ENAB
 
 # TPP Application
 add_executable(${PROJECT_NAME}_app src/tpp_app.cpp)
-target_link_libraries(${PROJECT_NAME}_app PRIVATE ${PROJECT_NAME})
+target_link_libraries(${PROJECT_NAME}_app PRIVATE ${PROJECT_NAME} Boost::program_options)
 target_compile_definitions(${PROJECT_NAME}_app PRIVATE NOETHER_GUI_PLUGINS="${PROJECT_NAME}_plugins")
 
 # Install headers

noether_gui/include/noether_gui/widgets/tpp_widget.h

@@ -3,6 +3,14 @@
 #include <noether_tpp/core/types.h>
 #include <QWidget>
 
+class QVTKWidget;
+class vtkActor;
+class vtkPolyDataMapper;
+class vtkProp;
+class vtkRenderer;
+class vtkAxes;
+class vtkTubeFilter;
+
 namespace boost_plugin_loader
 {
 class PluginLoader;
@@ -27,6 +35,7 @@ class TPPWidget : public QWidget
   Q_OBJECT
 public:
   TPPWidget(boost_plugin_loader::PluginLoader loader, QWidget* parent = nullptr);
+  virtual ~TPPWidget();
 
   /**
    * @brief Get the planned tool paths
@@ -35,6 +44,9 @@ class TPPWidget : public QWidget
    */
   std::vector<ToolPaths> getToolPaths();
 
+  void setMeshFile(const QString& file);
+  void setConfigurationFile(const QString& file);
+
 private:
   void onLoadMesh(const bool /*checked*/);
   void onLoadConfiguration(const bool /*checked*/);
@@ -43,6 +55,16 @@ class TPPWidget : public QWidget
 
   Ui::TPP* ui_;
   TPPPipelineWidget* pipeline_widget_;
+
+  // Viewer rendering
+  QVTKWidget* render_widget_;
+  vtkRenderer* renderer_;
+  vtkPolyDataMapper* mesh_mapper_;
+  vtkActor* mesh_actor_;
+  std::vector<vtkProp*> tool_path_actors_;
+  vtkAxes* axes_;
+  vtkTubeFilter* tube_filter_;
+
   std::vector<ToolPaths> tool_paths_;
 };
 

noether_gui/src/tpp_app.cpp

@@ -1,13 +1,32 @@
 #include <noether_gui/widgets/tpp_widget.h>
 
+#include <boost/program_options.hpp>
 #include <boost_plugin_loader/plugin_loader.h>
 #include <QApplication>
+#include <QMainWindow>
 #include <signal.h>
 
 void handleSignal(int /*sig*/) { QApplication::instance()->quit(); }
 
 int main(int argc, char** argv)
 {
+  namespace po = boost::program_options;
+
+  std::string mesh_file;
+  std::string config_file;
+
+  po::options_description desc("options");
+  // clang-format off
+  desc.add_options()
+      ("help", "produce help message")
+      ("mesh,m", po::value<std::string>(&mesh_file), "Input mesh file")
+      ("config,c", po::value<std::string>(&config_file), "Tool path planning pipeline configuration file")
+  ;
+  // clang-format on
+  po::variables_map vm;
+  po::store(po::parse_command_line(argc, argv, desc), vm);
+  po::notify(vm);
+
   QApplication app(argc, argv);
 
   signal(SIGINT, handleSignal);
@@ -18,7 +37,18 @@ int main(int argc, char** argv)
   loader.search_libraries_env = NOETHER_GUI_PLUGIN_LIBS_ENV;
   loader.search_paths_env = NOETHER_GUI_PLUGIN_PATHS_ENV;
 
-  noether::TPPWidget w(std::move(loader));
+  // Create the main window
+  QMainWindow w;
+
+  // Create (and optionally configure) the TPP widget
+  auto* widget = new noether::TPPWidget(std::move(loader), &w);
+  if (!mesh_file.empty())
+    widget->setMeshFile(QString::fromStdString(mesh_file));
+  if (!config_file.empty())
+    widget->setConfigurationFile(QString::fromStdString(config_file));
+
+  // Set the TPP widget as the central widget and show
+  w.setCentralWidget(widget);
   w.show();
 
   return app.exec();

noether_gui/src/widgets/tpp_widget.cpp

@@ -5,52 +5,127 @@
 
 #include <fstream>
 #include <pcl/io/vtk_lib_io.h>
+#include <QColorDialog>
 #include <QFileDialog>
 #include <QMessageBox>
 #include <QStandardPaths>
 #include <QTextStream>
 #include <yaml-cpp/yaml.h>
 
+// Rendering includes
+#include <QVTKWidget.h>
+#include <vtkAxesActor.h>
+#include <vtkOpenGLPolyDataMapper.h>
+#include <vtkOpenGLActor.h>
+#include <vtkOpenGLRenderer.h>
+#include <vtkRenderWindow.h>
+#include <vtkPLYReader.h>
+#include <vtkSTLReader.h>
+#include <vtkInteractorStyleTrackballCamera.h>
+#include <vtkAxes.h>
+#include <vtkTransformFilter.h>
+#include <vtkTransform.h>
+#include <vtkTubeFilter.h>
+
 namespace noether
 {
 TPPWidget::TPPWidget(boost_plugin_loader::PluginLoader loader, QWidget* parent)
-  : QWidget(parent), ui_(new Ui::TPP()), pipeline_widget_(new TPPPipelineWidget(std::move(loader), this))
+  : QWidget(parent)
+  , ui_(new Ui::TPP())
+  , pipeline_widget_(new TPPPipelineWidget(std::move(loader), this))
+  , render_widget_(new QVTKWidget(this))
+  , renderer_(vtkOpenGLRenderer::New())
+  , mesh_mapper_(vtkOpenGLPolyDataMapper::New())
+  , mesh_actor_(vtkOpenGLActor::New())
+  , axes_(vtkAxes::New())
+  , tube_filter_(vtkTubeFilter::New())
 {
   ui_->setupUi(this);
 
   ui_->scroll_area->setWidget(pipeline_widget_);
+  ui_->splitter->addWidget(render_widget_);
+
+  // Set up the VTK objects
+  mesh_actor_->SetMapper(mesh_mapper_);
+  renderer_->AddActor(mesh_actor_);
+  renderer_->SetBackground(0.2, 0.2, 0.2);
+  axes_->SetScaleFactor(ui_->double_spin_box_axis_size->value());
+  tube_filter_->SetInputConnection(axes_->GetOutputPort());
+  tube_filter_->SetRadius(axes_->GetScaleFactor() / 10.0);
+  tube_filter_->SetNumberOfSides(10);
+  tube_filter_->CappingOn();
+
+  vtkRenderWindow* window = render_widget_->GetRenderWindow();
+  window->AddRenderer(renderer_);
+  render_widget_->GetInteractor()->SetInteractorStyle(vtkSmartPointer<vtkInteractorStyleTrackballCamera>::New());
+  render_widget_->setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding);
 
+  // Connect signals
   connect(ui_->push_button_load_mesh, &QPushButton::clicked, this, &TPPWidget::onLoadMesh);
   connect(ui_->push_button_load_configuration, &QPushButton::clicked, this, &TPPWidget::onLoadConfiguration);
   connect(ui_->push_button_save_configuration, &QPushButton::clicked, this, &TPPWidget::onSaveConfiguration);
   connect(ui_->push_button_plan, &QPushButton::clicked, this, &TPPWidget::onPlan);
+  connect(ui_->double_spin_box_axis_size, &QDoubleSpinBox::editingFinished, this, [this]() {
+    axes_->SetScaleFactor(ui_->double_spin_box_axis_size->value());
+    tube_filter_->SetRadius(axes_->GetScaleFactor() / 10.0);
+    render_widget_->GetRenderWindow()->Render();
+    render_widget_->GetRenderWindow()->Render();
+  });
+}
+
+TPPWidget::~TPPWidget()
+{
+  renderer_->Delete();
+  mesh_mapper_->Delete();
+  mesh_actor_->Delete();
+  axes_->Delete();
+  tube_filter_->Delete();
 }
 
 std::vector<ToolPaths> TPPWidget::getToolPaths() { return tool_paths_; }
 
+void TPPWidget::setMeshFile(const QString& file)
+{
+  ui_->line_edit_mesh->setText(file);
+
+  // Render the mesh
+  vtkSmartPointer<vtkAbstractPolyDataReader> reader;
+  if (file.endsWith(".ply"))
+    reader = vtkSmartPointer<vtkPLYReader>::New();
+  else if (file.endsWith(".stl"))
+    reader = vtkSmartPointer<vtkSTLReader>::New();
+  else
+    return;
+
+  reader->SetFileName(file.toStdString().c_str());
+  reader->Update();
+
+  // Update the mapper input data
+  mesh_mapper_->SetInputData(reader->GetOutput());
+
+  // Zoom out to the extents
+  renderer_->ResetCamera();
+
+  // Call render twice
+  render_widget_->GetRenderWindow()->Render();
+  render_widget_->GetRenderWindow()->Render();
+}
+
 void TPPWidget::onLoadMesh(const bool /*checked*/)
 {
   const QString home = QStandardPaths::standardLocations(QStandardPaths::HomeLocation).at(0);
   const QString file = QFileDialog::getOpenFileName(this, "Load mesh file", home, "Mesh files (*.ply *.stl)");
-  ui_->line_edit_mesh->setText(file);
+  if (!file.isNull())
+    setMeshFile(file);
 }
 
-void TPPWidget::onLoadConfiguration(const bool /*checked*/)
+void TPPWidget::setConfigurationFile(const QString& file)
 {
-  QString file = ui_->line_edit_configuration->text();
-  if (file.isEmpty())
-    file = QStandardPaths::standardLocations(QStandardPaths::HomeLocation).at(0);
-
-  file = QFileDialog::getOpenFileName(this, "Load configuration file", file, "YAML files (*.yaml)");
-  if (file.isEmpty())
-    return;
-
   ui_->line_edit_configuration->setText(file);
 
   try
   {
     pipeline_widget_->configure(YAML::LoadFile(file.toStdString()));
-    QMessageBox::information(this, "Configuration", "Successfully loaded tool path planning pipeline configuration");
   }
   catch (const YAML::BadFile&)
   {
@@ -67,6 +142,17 @@ void TPPWidget::onLoadConfiguration(const bool /*checked*/)
   }
 }
 
+void TPPWidget::onLoadConfiguration(const bool /*checked*/)
+{
+  QString file = ui_->line_edit_configuration->text();
+  if (file.isEmpty())
+    file = QStandardPaths::standardLocations(QStandardPaths::HomeLocation).at(0);
+
+  file = QFileDialog::getOpenFileName(this, "Load configuration file", file, "YAML files (*.yaml)");
+  if (!file.isEmpty())
+    setConfigurationFile(file);
+}
+
 void TPPWidget::onSaveConfiguration(const bool /*checked*/)
 {
   try
@@ -97,6 +183,15 @@ void TPPWidget::onSaveConfiguration(const bool /*checked*/)
   }
 }
 
+vtkSmartPointer<vtkTransform> toVTK(const Eigen::Isometry3d& mat)
+{
+  auto t = vtkSmartPointer<vtkTransform>::New();
+  t->Translate(mat.translation().data());
+  Eigen::AngleAxisd aa(mat.rotation());
+  t->RotateWXYZ(aa.angle() * 180.0 / M_PI, aa.axis().data());
+  return t;
+}
+
 void TPPWidget::onPlan(const bool /*checked*/)
 {
   try
@@ -115,13 +210,43 @@ void TPPWidget::onPlan(const bool /*checked*/)
     tool_paths_ = pipeline.plan(mesh);
     QApplication::restoreOverrideCursor();
 
-    QString message;
-    QTextStream ss(&message);
-    for (std::size_t i = 0; i < tool_paths_.size(); ++i)
+    // Render the tool paths
+    std::for_each(tool_path_actors_.begin(), tool_path_actors_.end(), [this](vtkProp* actor) {
+      renderer_->RemoveActor(actor);
+      actor->Delete();
+    });
+    tool_path_actors_.clear();
+
+    for (const ToolPaths& fragment : tool_paths_)
     {
-      ss << "Mesh fragment " << i + 1 << ": created tool path with " << tool_paths_[i].size() << " strokes\n";
+      for (const ToolPath& tool_path : fragment)
+      {
+        for (const ToolPathSegment& segment : tool_path)
+        {
+          for (const Eigen::Isometry3d& w : segment)
+          {
+            auto transform_filter = vtkSmartPointer<vtkTransformFilter>::New();
+            transform_filter->SetTransform(toVTK(w));
+            transform_filter->SetInputConnection(tube_filter_->GetOutputPort());
+
+            auto map = vtkSmartPointer<vtkPolyDataMapper>::New();
+            map->SetInputConnection(transform_filter->GetOutputPort());
+
+            auto actor = vtkActor::New();
+            actor->SetMapper(map);
+
+            tool_path_actors_.push_back(actor);
+          }
+        }
+      }
     }
-    QMessageBox::information(this, "Success", message);
+
+    std::for_each(
+        tool_path_actors_.begin(), tool_path_actors_.end(), [this](vtkProp* actor) { renderer_->AddActor(actor); });
+
+    // Call render twice
+    render_widget_->GetRenderWindow()->Render();
+    render_widget_->GetRenderWindow()->Render();
   }
   catch (const std::exception& ex)
   {