Linear departure tool path modifier (#202)

* Created linear departure tool path modifier * Added widget for linear departure modifier widget --------- Co-authored-by: David Spielman <davidroyspielman@gmail.com>
ros-industrial · Sep 15, 2023 · 3eff1fd · 3eff1fd
1 parent db25b08
commit 3eff1fd
Show file tree

Hide file tree

Showing 9 changed files with 100 additions and 2 deletions.
diff --git a/noether_gui/CMakeLists.txt b/noether_gui/CMakeLists.txt
@@ -42,6 +42,7 @@ qt5_wrap_cpp(${PROJECT_NAME}_widget_mocs
   include/${PROJECT_NAME}/widgets/tool_path_modifiers/snake_organization_modifier_widget.h
   include/${PROJECT_NAME}/widgets/tool_path_modifiers/standard_edge_paths_organization_modifier_widget.h
   include/${PROJECT_NAME}/widgets/tool_path_modifiers/linear_approach_modifier_widget.h
+  include/${PROJECT_NAME}/widgets/tool_path_modifiers/linear_departure_modifier_widget.h
   include/${PROJECT_NAME}/widgets/tool_path_modifiers/tool_drag_orientation_modifier_widget.h
   include/${PROJECT_NAME}/widgets/tool_path_modifiers/uniform_orientation_modifier_widget.h)
 
@@ -85,6 +86,7 @@ add_library(${PROJECT_NAME} SHARED
   src/widgets/tool_path_modifiers/snake_organization_modifier_widget.cpp
   src/widgets/tool_path_modifiers/standard_edge_paths_organization_modifier_widget.cpp
   src/widgets/tool_path_modifiers/linear_approach_modifier_widget.cpp
+  src/widgets/tool_path_modifiers/linear_departure_modifier_widget.cpp
   src/widgets/tool_path_modifiers/tool_drag_orientation_modifier_widget.cpp
   src/widgets/tool_path_modifiers/uniform_orientation_modifier_widget.cpp
   ${${PROJECT_NAME}_widget_mocs}

diff --git a/...her_gui/include/noether_gui/widgets/tool_path_modifiers/linear_approach_modifier_widget.h b/...her_gui/include/noether_gui/widgets/tool_path_modifiers/linear_approach_modifier_widget.h
@@ -22,7 +22,7 @@ class LinearApproachToolPathModifierWidget : public ToolPathModifierWidget
   void configure(const YAML::Node&) override;
   void save(YAML::Node&) const override;
 
-private:
+protected:
   Ui::LinearApproachModifier* ui_;
   Ui::Vector3dEditor* vector_editor_ui_;
 };

diff --git a/...er_gui/include/noether_gui/widgets/tool_path_modifiers/linear_departure_modifier_widget.h b/...er_gui/include/noether_gui/widgets/tool_path_modifiers/linear_departure_modifier_widget.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include <noether_gui/widgets/tool_path_modifiers/linear_approach_modifier_widget.h>
+
+namespace noether
+{
+class LinearDepartureToolPathModifierWidget : public LinearApproachToolPathModifierWidget
+{
+  Q_OBJECT
+public:
+  using LinearApproachToolPathModifierWidget::LinearApproachToolPathModifierWidget;
+
+  ToolPathModifier::ConstPtr create() const override;
+};
+
+}  // namespace noether
diff --git a/noether_gui/src/plugins.cpp b/noether_gui/src/plugins.cpp
@@ -23,6 +23,7 @@
 #include <noether_gui/widgets/tool_path_modifiers/tool_drag_orientation_modifier_widget.h>
 #include <noether_gui/widgets/tool_path_modifiers/uniform_orientation_modifier_widget.h>
 #include <noether_gui/widgets/tool_path_modifiers/linear_approach_modifier_widget.h>
+#include <noether_gui/widgets/tool_path_modifiers/linear_departure_modifier_widget.h>
 
 #include <QWidget>
 #include <QMessageBox>
@@ -91,6 +92,9 @@ using CircularLeadOutToolPathModifierWidgetPlugin =
 using LinearApproachToolPathModifierWidgetPlugin =
     WidgetPluginImpl<LinearApproachToolPathModifierWidget, ToolPathModifierWidget>;
 
+using LinearDepartureToolPathModifierWidgetPlugin =
+    WidgetPluginImpl<LinearDepartureToolPathModifierWidget, ToolPathModifierWidget>;
+
 // Raster Tool Path Planners
 struct PlaneSlicerRasterPlannerWidgetPlugin : ToolPathPlannerWidgetPlugin
 {
@@ -136,5 +140,6 @@ EXPORT_TOOL_PATH_MODIFIER_WIDGET_PLUGIN(noether::ToolDragOrientationToolPathModi
 EXPORT_TOOL_PATH_MODIFIER_WIDGET_PLUGIN(noether::CircularLeadInToolPathModifierWidgetPlugin, CircularLeadInModifier)
 EXPORT_TOOL_PATH_MODIFIER_WIDGET_PLUGIN(noether::CircularLeadOutToolPathModifierWidgetPlugin, CircularLeadOutModifier)
 EXPORT_TOOL_PATH_MODIFIER_WIDGET_PLUGIN(noether::LinearApproachToolPathModifierWidgetPlugin, LinearApproachModifier)
+EXPORT_TOOL_PATH_MODIFIER_WIDGET_PLUGIN(noether::LinearDepartureToolPathModifierWidgetPlugin, LinearDepartureModifier)
 
 EXPORT_TPP_WIDGET_PLUGIN(noether::PlaneSlicerRasterPlannerWidgetPlugin, PlaneSlicerRasterPlanner)
diff --git a/noether_gui/src/widgets/tool_path_modifiers/linear_departure_modifier_widget.cpp b/noether_gui/src/widgets/tool_path_modifiers/linear_departure_modifier_widget.cpp
@@ -0,0 +1,27 @@
+#include <noether_gui/widgets/tool_path_modifiers/linear_departure_modifier_widget.h>
+#include <noether_gui/utils.h>
+
+#include <noether_tpp/tool_path_modifiers/linear_departure_modifier.h>
+#include "ui_vector3d_editor_widget.h"
+#include "ui_linear_approach_modifier_widget.h"
+
+namespace noether
+{
+ToolPathModifier::ConstPtr LinearDepartureToolPathModifierWidget::create() const
+{
+  if (ui_->combo_box_menu->currentIndex() == 0)
+  {
+    auto axis = static_cast<LinearDepartureModifier::Axis>(ui_->combo_box_axis->currentIndex());
+    return std::make_unique<LinearDepartureModifier>(
+        ui_->double_spin_box_offset->value(), axis, ui_->spin_box_points->value());
+  }
+  else
+  {
+    Eigen::Vector3d dir(vector_editor_ui_->double_spin_box_x->value(),
+                        vector_editor_ui_->double_spin_box_y->value(),
+                        vector_editor_ui_->double_spin_box_z->value());
+    return std::make_unique<LinearDepartureModifier>(dir, ui_->spin_box_points->value());
+  }
+}
+
+}  // namespace noether
diff --git a/noether_tpp/CMakeLists.txt b/noether_tpp/CMakeLists.txt
@@ -42,6 +42,7 @@ add_library(${PROJECT_NAME} SHARED
   src/tool_path_modifiers/snake_organization_modifier.cpp
   src/tool_path_modifiers/standard_edge_paths_organization_modifier.cpp
   src/tool_path_modifiers/linear_approach_modifier.cpp
+  src/tool_path_modifiers/linear_departure_modifier.cpp
   src/tool_path_modifiers/tool_drag_orientation_modifier.cpp
   src/tool_path_modifiers/uniform_orientation_modifier.cpp
   # Tool Path Planners

diff --git a/noether_tpp/include/noether_tpp/tool_path_modifiers/linear_approach_modifier.h b/noether_tpp/include/noether_tpp/tool_path_modifiers/linear_approach_modifier.h
@@ -22,7 +22,7 @@ class LinearApproachModifier : public ToolPathModifier
   LinearApproachModifier(Eigen::Vector3d offset, std::size_t n_points);
   LinearApproachModifier(double offset_height, Axis axis, std::size_t n_points);
 
-  ToolPaths modify(ToolPaths tool_paths) const override final;
+  ToolPaths modify(ToolPaths tool_paths) const override;
 
 protected:
   Eigen::Vector3d offset_;

diff --git a/noether_tpp/include/noether_tpp/tool_path_modifiers/linear_departure_modifier.h b/noether_tpp/include/noether_tpp/tool_path_modifiers/linear_departure_modifier.h
@@ -0,0 +1,18 @@
+#pragma once
+
+#include <noether_tpp/tool_path_modifiers/linear_approach_modifier.h>
+
+namespace noether
+{
+/**
+ * @brief Adds a series of waypoints in a linear pattern off the last waypoint in a tool path
+ */
+class LinearDepartureModifier : public LinearApproachModifier
+{
+public:
+  using LinearApproachModifier::LinearApproachModifier;
+
+  ToolPaths modify(ToolPaths tool_paths) const override final;
+};
+
+}  // namespace noether
diff --git a/noether_tpp/src/tool_path_modifiers/linear_departure_modifier.cpp b/noether_tpp/src/tool_path_modifiers/linear_departure_modifier.cpp
@@ -0,0 +1,29 @@
+#include <noether_tpp/tool_path_modifiers/linear_departure_modifier.h>
+
+namespace noether
+{
+ToolPaths LinearDepartureModifier::modify(ToolPaths tool_paths) const
+{
+  for (ToolPath& tool_path : tool_paths)
+  {
+    for (ToolPathSegment& segment : tool_path)
+    {
+      Eigen::Isometry3d offset_point = segment.back() * Eigen::Translation3d(offset_);
+      ToolPathSegment new_segment;
+
+      for (int i = 0; i <= n_points_; i++)
+      {
+        Eigen::Isometry3d pt;
+        pt = offset_point * Eigen::Translation3d(-offset_ + (offset_ / (n_points_)) * i);
+        pt.linear() = segment.back().linear();
+        new_segment.push_back(pt);
+      }
+
+      segment.insert(segment.end(), new_segment.rbegin(), new_segment.rend());
+    }
+  }
+
+  return tool_paths;
+}
+
+}  // namespace noether