From 3eff1fdc9450c8aa2fa71ca7782be67ddf4c6ab6 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Fri, 15 Sep 2023 13:18:09 -0500 Subject: [PATCH] Linear departure tool path modifier (#202) * Created linear departure tool path modifier * Added widget for linear departure modifier widget --------- Co-authored-by: David Spielman --- noether_gui/CMakeLists.txt | 2 ++ .../linear_approach_modifier_widget.h | 2 +- .../linear_departure_modifier_widget.h | 16 ++++++++++ noether_gui/src/plugins.cpp | 5 ++++ .../linear_departure_modifier_widget.cpp | 27 +++++++++++++++++ noether_tpp/CMakeLists.txt | 1 + .../linear_approach_modifier.h | 2 +- .../linear_departure_modifier.h | 18 ++++++++++++ .../linear_departure_modifier.cpp | 29 +++++++++++++++++++ 9 files changed, 100 insertions(+), 2 deletions(-) create mode 100644 noether_gui/include/noether_gui/widgets/tool_path_modifiers/linear_departure_modifier_widget.h create mode 100644 noether_gui/src/widgets/tool_path_modifiers/linear_departure_modifier_widget.cpp create mode 100644 noether_tpp/include/noether_tpp/tool_path_modifiers/linear_departure_modifier.h create mode 100644 noether_tpp/src/tool_path_modifiers/linear_departure_modifier.cpp diff --git a/noether_gui/CMakeLists.txt b/noether_gui/CMakeLists.txt index d200fbb2..804722f2 100644 --- 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/noether_gui/include/noether_gui/widgets/tool_path_modifiers/linear_approach_modifier_widget.h b/noether_gui/include/noether_gui/widgets/tool_path_modifiers/linear_approach_modifier_widget.h index 1d1cf52a..7d1b381d 100644 --- a/noether_gui/include/noether_gui/widgets/tool_path_modifiers/linear_approach_modifier_widget.h +++ b/noether_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/noether_gui/include/noether_gui/widgets/tool_path_modifiers/linear_departure_modifier_widget.h b/noether_gui/include/noether_gui/widgets/tool_path_modifiers/linear_departure_modifier_widget.h new file mode 100644 index 00000000..7fea2e1d --- /dev/null +++ b/noether_gui/include/noether_gui/widgets/tool_path_modifiers/linear_departure_modifier_widget.h @@ -0,0 +1,16 @@ +#pragma once + +#include + +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 index f39f0776..fdb95589 100644 --- a/noether_gui/src/plugins.cpp +++ b/noether_gui/src/plugins.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -91,6 +92,9 @@ using CircularLeadOutToolPathModifierWidgetPlugin = using LinearApproachToolPathModifierWidgetPlugin = WidgetPluginImpl; +using LinearDepartureToolPathModifierWidgetPlugin = + WidgetPluginImpl; + // 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 new file mode 100644 index 00000000..9416b98d --- /dev/null +++ b/noether_gui/src/widgets/tool_path_modifiers/linear_departure_modifier_widget.cpp @@ -0,0 +1,27 @@ +#include +#include + +#include +#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(ui_->combo_box_axis->currentIndex()); + return std::make_unique( + 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(dir, ui_->spin_box_points->value()); + } +} + +} // namespace noether diff --git a/noether_tpp/CMakeLists.txt b/noether_tpp/CMakeLists.txt index 9fa20d05..53b4aef9 100644 --- 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 index 954b0807..901f8bb6 100644 --- 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 new file mode 100644 index 00000000..7a1e419b --- /dev/null +++ b/noether_tpp/include/noether_tpp/tool_path_modifiers/linear_departure_modifier.h @@ -0,0 +1,18 @@ +#pragma once + +#include + +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 new file mode 100644 index 00000000..ac418c0f --- /dev/null +++ b/noether_tpp/src/tool_path_modifiers/linear_departure_modifier.cpp @@ -0,0 +1,29 @@ +#include + +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