diff --git a/noether_gui/CMakeLists.txt b/noether_gui/CMakeLists.txt index 27aa3a45..0502e5ed 100644 --- a/noether_gui/CMakeLists.txt +++ b/noether_gui/CMakeLists.txt @@ -23,6 +23,7 @@ qt5_wrap_cpp(${PROJECT_NAME}_widget_mocs include/${PROJECT_NAME}/widgets/configurable_tpp_pipeline_widget.h include/${PROJECT_NAME}/widgets/tpp_widget.h # Tool Path Planners + include/${PROJECT_NAME}/widgets/tool_path_planners/flat_plane_toolpath_planner_widget.h # Raster # Direction Generator include/${PROJECT_NAME}/widgets/tool_path_planners/raster/direction_generators/fixed_direction_generator_widget.h @@ -49,6 +50,8 @@ qt5_wrap_cpp(${PROJECT_NAME}_widget_mocs qt5_wrap_ui(${PROJECT_NAME}_widget_ui_mocs ui/vector3d_editor_widget.ui + ui/isometry3d_editor_widget.ui + ui/vector2d_editor_widget.ui ui/plugin_loader_widget.ui ui/linear_approach_modifier_widget.ui ui/raster_planner_widget.ui @@ -66,6 +69,7 @@ add_library(${PROJECT_NAME} SHARED src/widgets/configurable_tpp_pipeline_widget.cpp src/widgets/tpp_widget.cpp # Tool Path Planners + src/widgets/tool_path_planners/flat_plane_toolpath_planner_widget.cpp # Raster Planner src/widgets/tool_path_planners/raster/raster_planner_widget.cpp # Direction Generator diff --git a/noether_gui/include/noether_gui/widgets/tool_path_planners/flat_plane_toolpath_planner_widget.h b/noether_gui/include/noether_gui/widgets/tool_path_planners/flat_plane_toolpath_planner_widget.h new file mode 100644 index 00000000..b999df3c --- /dev/null +++ b/noether_gui/include/noether_gui/widgets/tool_path_planners/flat_plane_toolpath_planner_widget.h @@ -0,0 +1,37 @@ +#pragma once + +#include +#include + +class QSpinBox; + +namespace Ui +{ +class Isometry3dEditor; +} + +namespace Ui +{ +class Vector2dEditor; +} + +namespace noether +{ +class FlatPlaneToolPathPlannerWidget : public ToolPathPlannerWidget +{ + Q_OBJECT +public: + FlatPlaneToolPathPlannerWidget(QWidget* parent = nullptr); + + ToolPathPlanner::ConstPtr create() const override; + + void configure(const YAML::Node&) override; + void save(YAML::Node&) const override; + +private: + Ui::Isometry3dEditor* origin_ui_; + Ui::Vector2dEditor* plane_dim_ui_; + Ui::Vector2dEditor* spacing_dim_ui_; +}; + +} // namespace noether diff --git a/noether_gui/src/plugins.cpp b/noether_gui/src/plugins.cpp index fdb95589..a7e812ff 100644 --- a/noether_gui/src/plugins.cpp +++ b/noether_gui/src/plugins.cpp @@ -1,5 +1,6 @@ #include // Tool path planners +#include // Raster #include // Direction Generators @@ -45,6 +46,8 @@ struct WidgetPluginImpl : WidgetPlugin return widget; } }; +// Tool Path Planners +using FlatPlaneToolPathPlannerWidgetPlugin = WidgetPluginImpl; // Direction Generators using FixedDirectionGeneratorWidgetPlugin = @@ -142,4 +145,6 @@ EXPORT_TOOL_PATH_MODIFIER_WIDGET_PLUGIN(noether::CircularLeadOutToolPathModifier EXPORT_TOOL_PATH_MODIFIER_WIDGET_PLUGIN(noether::LinearApproachToolPathModifierWidgetPlugin, LinearApproachModifier) EXPORT_TOOL_PATH_MODIFIER_WIDGET_PLUGIN(noether::LinearDepartureToolPathModifierWidgetPlugin, LinearDepartureModifier) +EXPORT_TPP_WIDGET_PLUGIN(noether::FlatPlaneToolPathPlannerWidgetPlugin, FlatPlaneToolPathPlanner) + EXPORT_TPP_WIDGET_PLUGIN(noether::PlaneSlicerRasterPlannerWidgetPlugin, PlaneSlicerRasterPlanner) diff --git a/noether_gui/src/widgets/tool_path_planners/flat_plane_toolpath_planner_widget.cpp b/noether_gui/src/widgets/tool_path_planners/flat_plane_toolpath_planner_widget.cpp new file mode 100644 index 00000000..9ff85e5d --- /dev/null +++ b/noether_gui/src/widgets/tool_path_planners/flat_plane_toolpath_planner_widget.cpp @@ -0,0 +1,131 @@ +#include +#include + +#include +#include +#include +#include +#include "ui_vector2d_editor_widget.h" +#include "ui_isometry3d_editor_widget.h" + +static const std::string ORIGIN_PX_KEY = "px"; +static const std::string ORIGIN_PY_KEY = "py"; +static const std::string ORIGIN_PZ_KEY = "pz"; +static const std::string ORIGIN_QX_KEY = "qx"; +static const std::string ORIGIN_QY_KEY = "qy"; +static const std::string ORIGIN_QZ_KEY = "qz"; +static const std::string ORIGIN_QW_KEY = "qw"; + +static const std::string PLANE_X_DIM_KEY = "plane_x_dim"; +static const std::string PLANE_Y_DIM_KEY = "plane_y_dim"; +static const std::string X_SPACING_KEY = "x_spacing"; +static const std::string Y_SPACING_KEY = "y_spacing"; + +namespace noether +{ +FlatPlaneToolPathPlannerWidget::FlatPlaneToolPathPlannerWidget(QWidget* parent) + : ToolPathPlannerWidget(parent) + , origin_ui_(new Ui::Isometry3dEditor()) + , plane_dim_ui_(new Ui::Vector2dEditor()) + , spacing_dim_ui_(new Ui::Vector2dEditor()) +{ + // Create a vertical layout for the entire widget + auto mainLayout = new QVBoxLayout(this); + + origin_ui_->setupUi(this); + origin_ui_->group_box->setTitle("Offset (position in meters, rotation in radians)"); + + plane_dim_ui_->setupUi(this); + plane_dim_ui_->group_box->setTitle("Plane Dimensions (m)"); + + spacing_dim_ui_->setupUi(this); + spacing_dim_ui_->group_box->setTitle("Plane Spacing (m)"); + + // Create a container widget to hold the layouts + auto containerWidget = new QWidget; + + // Create a layout for the container widget + auto containerLayout = new QVBoxLayout(containerWidget); + + // Create a horizontal layout for the spin box and label + auto spinBoxLayout = new QHBoxLayout; + + // Add the horizontal layout to the container layout + containerLayout->addLayout(spinBoxLayout); + + // Set the container widget as the layout for the Isometry3dEditor and Vector2dEditor widgets + origin_ui_->group_box->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + containerLayout->addWidget(origin_ui_->group_box); + + plane_dim_ui_->group_box->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + containerLayout->addWidget(plane_dim_ui_->group_box); + + spacing_dim_ui_->group_box->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + containerLayout->addWidget(spacing_dim_ui_->group_box); + + // Add the container widget to the main layout + mainLayout->addWidget(containerWidget); + + // Set the main layout for the entire widget + setLayout(mainLayout); +} + +ToolPathPlanner::ConstPtr FlatPlaneToolPathPlannerWidget::create() const +{ + Eigen::Isometry3d origin = Eigen::Isometry3d::Identity(); + // Create a quaternion from roll, pitch, yaw + Eigen::Quaterniond q(origin_ui_->double_spin_box_qw->value(), + origin_ui_->double_spin_box_qx->value(), + origin_ui_->double_spin_box_qy->value(), + origin_ui_->double_spin_box_qz->value()); + + // Set the translation + origin.translation() = Eigen::Vector3d(origin_ui_->double_spin_box_px->value(), + origin_ui_->double_spin_box_py->value(), + origin_ui_->double_spin_box_pz->value()); + + // Set the rotation + origin.rotate(q); + + Eigen::Vector2d plane_dim(plane_dim_ui_->double_spin_box_x->value(), plane_dim_ui_->double_spin_box_y->value()); + Eigen::Vector2d spacing(spacing_dim_ui_->double_spin_box_x->value(), spacing_dim_ui_->double_spin_box_y->value()); + return std::make_unique(plane_dim, spacing, origin); +} + +void FlatPlaneToolPathPlannerWidget::configure(const YAML::Node& config) +{ + origin_ui_->double_spin_box_px->setValue(getEntry(config, ORIGIN_PX_KEY)); + origin_ui_->double_spin_box_py->setValue(getEntry(config, ORIGIN_PY_KEY)); + origin_ui_->double_spin_box_pz->setValue(getEntry(config, ORIGIN_PZ_KEY)); + origin_ui_->double_spin_box_qx->setValue(getEntry(config, ORIGIN_QX_KEY)); + origin_ui_->double_spin_box_qy->setValue(getEntry(config, ORIGIN_QY_KEY)); + origin_ui_->double_spin_box_qz->setValue(getEntry(config, ORIGIN_QZ_KEY)); + origin_ui_->double_spin_box_qw->setValue(getEntry(config, ORIGIN_QW_KEY)); + + Eigen::Vector2d plane_dim(getEntry(config, PLANE_X_DIM_KEY), getEntry(config, PLANE_Y_DIM_KEY)); + plane_dim_ui_->double_spin_box_x->setValue(getEntry(config, PLANE_X_DIM_KEY)); + plane_dim_ui_->double_spin_box_y->setValue(getEntry(config, PLANE_Y_DIM_KEY)); + + Eigen::Vector2d spacing(getEntry(config, X_SPACING_KEY), getEntry(config, Y_SPACING_KEY)); + spacing_dim_ui_->double_spin_box_x->setValue(getEntry(config, X_SPACING_KEY)); + spacing_dim_ui_->double_spin_box_y->setValue(getEntry(config, Y_SPACING_KEY)); +} + +void FlatPlaneToolPathPlannerWidget::save(YAML::Node& config) const +{ + config[ORIGIN_PX_KEY] = origin_ui_->double_spin_box_px->value(); + config[ORIGIN_PY_KEY] = origin_ui_->double_spin_box_py->value(); + config[ORIGIN_PZ_KEY] = origin_ui_->double_spin_box_pz->value(); + config[ORIGIN_QX_KEY] = origin_ui_->double_spin_box_qx->value(); + config[ORIGIN_QY_KEY] = origin_ui_->double_spin_box_qy->value(); + config[ORIGIN_QZ_KEY] = origin_ui_->double_spin_box_qz->value(); + config[ORIGIN_QW_KEY] = origin_ui_->double_spin_box_qw->value(); + + config[PLANE_X_DIM_KEY] = plane_dim_ui_->double_spin_box_x->value(); + config[PLANE_Y_DIM_KEY] = plane_dim_ui_->double_spin_box_y->value(); + + config[X_SPACING_KEY] = spacing_dim_ui_->double_spin_box_x->value(); + config[Y_SPACING_KEY] = spacing_dim_ui_->double_spin_box_y->value(); +} + +} // namespace noether diff --git a/noether_gui/ui/isometry3d_editor_widget.ui b/noether_gui/ui/isometry3d_editor_widget.ui new file mode 100644 index 00000000..dbf685a8 --- /dev/null +++ b/noether_gui/ui/isometry3d_editor_widget.ui @@ -0,0 +1,189 @@ + + + Isometry3dEditor + + + + 0 + 0 + 236 + 258 + + + + Form + + + + + + Isometry 3d + + + + + + + + px + + + + + + + 3 + + + -100.000000000000000 + + + 100.000000000000000 + + + + + + + py + + + + + + + 3 + + + -100.000000000000000 + + + 100.000000000000000 + + + 0.000000000000000 + + + + + + + pz + + + + + + + 3 + + + -100.000000000000000 + + + 100.000000000000000 + + + + + + + qx + + + + + + + 3 + + + -1.000000000000000 + + + 1.000000000000000 + + + 0.001000000000000 + + + + + + + qy + + + + + + + 3 + + + -1.000000000000000 + + + 1.000000000000000 + + + 0.001000000000000 + + + + + + + qz + + + + + + + 3 + + + -1.000000000000000 + + + 1.000000000000000 + + + 0.001000000000000 + + + + + + + qw + + + + + + + 3 + + + -1.000000000000000 + + + 1.000000000000000 + + + 0.001000000000000 + + + + + + + + + + + + + diff --git a/noether_gui/ui/vector2d_editor_widget.ui b/noether_gui/ui/vector2d_editor_widget.ui new file mode 100644 index 00000000..55139713 --- /dev/null +++ b/noether_gui/ui/vector2d_editor_widget.ui @@ -0,0 +1,77 @@ + + + Vector2dEditor + + + + 0 + 0 + 234 + 167 + + + + Form + + + + + + Vector + + + + + + + + x + + + + + + + 3 + + + -100.000000000000000 + + + 100.000000000000000 + + + + + + + y + + + + + + + 3 + + + -100.000000000000000 + + + 100.000000000000000 + + + 0.000000000000000 + + + + + + + + + + + + + diff --git a/noether_tpp/CMakeLists.txt b/noether_tpp/CMakeLists.txt index 86774bb5..f7894469 100644 --- a/noether_tpp/CMakeLists.txt +++ b/noether_tpp/CMakeLists.txt @@ -9,6 +9,7 @@ project(${pkg_extracted_name} VERSION ${pkg_extracted_version} LANGUAGES C CXX) find_package(Eigen3 REQUIRED) find_package(PCL REQUIRED COMPONENTS common io surface segmentation) + find_package(VTK REQUIRED NO_MODULE) if(VTK_FOUND AND ("${VTK_VERSION}" VERSION_LESS 7.1)) message(FATAL_ERROR "The minimum required version of VTK is 7.1, but found ${VTK_VERSION}") @@ -61,6 +62,7 @@ add_library(${PROJECT_NAME} SHARED src/tool_path_planners/raster/direction_generators/fixed_direction_generator.cpp src/tool_path_planners/raster/direction_generators/principal_axis_direction_generator.cpp src/tool_path_planners/raster/plane_slicer_raster_planner.cpp + src/tool_path_planners/flat_plane_toolpath_planner.cpp ) if(${CMAKE_VERSION} VERSION_GREATER 3.15) diff --git a/noether_tpp/include/noether_tpp/tool_path_planners/flat_plane_toolpath_planner.h b/noether_tpp/include/noether_tpp/tool_path_planners/flat_plane_toolpath_planner.h new file mode 100644 index 00000000..ffacf357 --- /dev/null +++ b/noether_tpp/include/noether_tpp/tool_path_planners/flat_plane_toolpath_planner.h @@ -0,0 +1,43 @@ +/** + * @file plane_slicer_raster_planner.h + * @copyright Copyright (c) 2021, 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. + */ +#pragma once + +#include +#include + +namespace noether +{ +/** + * @brief An implementation of a tool path planner that generates a toolpath on a flat plane. + */ +class FlatPlaneToolPathPlanner : public ToolPathPlanner +{ +public: + FlatPlaneToolPathPlanner(const Eigen::Vector2d& plane_dims, + const Eigen::Vector2d& point_spacing, + Eigen::Isometry3d offset = Eigen::Isometry3d::Identity()); + ToolPaths plan(const pcl::PolygonMesh& mesh) const override final; + +private: + const Eigen::Vector2d plane_dims_; + const Eigen::Vector2d point_spacing_; + const Eigen::Isometry3d offset_; +}; + +} // namespace noether diff --git a/noether_tpp/src/tool_path_planners/flat_plane_toolpath_planner.cpp b/noether_tpp/src/tool_path_planners/flat_plane_toolpath_planner.cpp new file mode 100644 index 00000000..3d0cb36e --- /dev/null +++ b/noether_tpp/src/tool_path_planners/flat_plane_toolpath_planner.cpp @@ -0,0 +1,41 @@ +#include +#include +#include + +namespace noether +{ +FlatPlaneToolPathPlanner::FlatPlaneToolPathPlanner(const Eigen::Vector2d& plane_dims, + const Eigen::Vector2d& point_spacing, + Eigen::Isometry3d offset) + : plane_dims_(plane_dims), point_spacing_(point_spacing), offset_(offset) +{ +} +ToolPaths FlatPlaneToolPathPlanner::plan(const pcl::PolygonMesh& /*mesh*/) const +{ + ToolPathSegment segment; + ToolPath tool_path; + ToolPaths tool_paths; + + Eigen::Isometry3d eigen_pose = offset_; + for (size_t i = 0; plane_dims_[0] - point_spacing_[0] * i >= 0; i++) + { + for (size_t j = 0; plane_dims_[1] - point_spacing_[1] * j >= 0; j++) + { + Eigen::Isometry3d pt = eigen_pose * Eigen::Translation3d(i * point_spacing_[0], j * point_spacing_[1], 0.0); + segment.push_back(pt); + } + if (i % 2 != 0) + { + std::reverse(segment.begin(), segment.end()); + tool_path.push_back(segment); + } + else + { + tool_path.push_back(segment); + } + segment.clear(); + } + tool_paths.push_back(tool_path); + return tool_paths; +} +} // namespace noether