From 23a0129742a749b7018a418073487077c39eeadf Mon Sep 17 00:00:00 2001 From: David Spielman Date: Wed, 22 Nov 2023 11:47:18 -0600 Subject: [PATCH 1/2] Created tool path planner that creates a snakelike pattern on a plane of configurable size, with configurable spacing between waypoints at an offset from a given reference frame. --- noether_gui/CMakeLists.txt | 4 + .../flat_plane_toolpath_planner_widget.h | 37 ++++ noether_gui/src/plugins.cpp | 6 + .../flat_plane_toolpath_planner_widget.cpp | 130 ++++++++++++ noether_gui/ui/isometry3d_editor_widget.ui | 189 ++++++++++++++++++ noether_gui/ui/vector2d_editor_widget.ui | 77 +++++++ noether_tpp/CMakeLists.txt | 2 + .../flat_plane_toolpath_planner.h | 41 ++++ .../flat_plane_toolpath_planner.cpp | 38 ++++ 9 files changed, 524 insertions(+) create mode 100644 noether_gui/include/noether_gui/widgets/tool_path_planners/flat_plane_toolpath_planner_widget.h create mode 100644 noether_gui/src/widgets/tool_path_planners/flat_plane_toolpath_planner_widget.cpp create mode 100644 noether_gui/ui/isometry3d_editor_widget.ui create mode 100644 noether_gui/ui/vector2d_editor_widget.ui create mode 100644 noether_tpp/include/noether_tpp/tool_path_planners/flat_plane_toolpath_planner.h create mode 100644 noether_tpp/src/tool_path_planners/flat_plane_toolpath_planner.cpp 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..7876c0fb 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,9 @@ struct WidgetPluginImpl : WidgetPlugin return widget; } }; +// Tool Path Planners +using FlatPlaneToolPathPlannerWidgetPlugin = + WidgetPluginImpl; // Direction Generators using FixedDirectionGeneratorWidgetPlugin = @@ -142,4 +146,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..31148a5b --- /dev/null +++ b/noether_gui/src/widgets/tool_path_planners/flat_plane_toolpath_planner_widget.cpp @@ -0,0 +1,130 @@ +#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..214cf1cb --- /dev/null +++ b/noether_tpp/include/noether_tpp/tool_path_planners/flat_plane_toolpath_planner.h @@ -0,0 +1,41 @@ +/** + * @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..196f97be --- /dev/null +++ b/noether_tpp/src/tool_path_planners/flat_plane_toolpath_planner.cpp @@ -0,0 +1,38 @@ +#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 From ee2cba179ac85f5ff560e33574af8b70d6493488 Mon Sep 17 00:00:00 2001 From: David Spielman Date: Fri, 5 Jan 2024 14:05:23 -0600 Subject: [PATCH 2/2] Minor formatting changes --- noether_gui/src/plugins.cpp | 3 +-- .../flat_plane_toolpath_planner_widget.cpp | 25 ++++++++++--------- .../flat_plane_toolpath_planner.h | 4 ++- .../flat_plane_toolpath_planner.cpp | 17 +++++++------ 4 files changed, 27 insertions(+), 22 deletions(-) diff --git a/noether_gui/src/plugins.cpp b/noether_gui/src/plugins.cpp index 7876c0fb..a7e812ff 100644 --- a/noether_gui/src/plugins.cpp +++ b/noether_gui/src/plugins.cpp @@ -47,8 +47,7 @@ struct WidgetPluginImpl : WidgetPlugin } }; // Tool Path Planners -using FlatPlaneToolPathPlannerWidgetPlugin = - WidgetPluginImpl; +using FlatPlaneToolPathPlannerWidgetPlugin = WidgetPluginImpl; // Direction Generators using FixedDirectionGeneratorWidgetPlugin = 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 index 31148a5b..9ff85e5d 100644 --- 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 @@ -8,7 +8,6 @@ #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"; @@ -22,12 +21,13 @@ 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()) + : 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); @@ -74,22 +74,24 @@ 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()); + 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()); + 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()); + 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)); @@ -124,7 +126,6 @@ void FlatPlaneToolPathPlannerWidget::save(YAML::Node& config) const 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_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 index 214cf1cb..ffacf357 100644 --- 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 @@ -29,7 +29,9 @@ namespace noether class FlatPlaneToolPathPlanner : public ToolPathPlanner { public: - FlatPlaneToolPathPlanner(const Eigen::Vector2d& plane_dims, const Eigen::Vector2d& point_spacing, Eigen::Isometry3d offset = Eigen::Isometry3d::Identity()); + 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: 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 index 196f97be..3d0cb36e 100644 --- a/noether_tpp/src/tool_path_planners/flat_plane_toolpath_planner.cpp +++ b/noether_tpp/src/tool_path_planners/flat_plane_toolpath_planner.cpp @@ -4,7 +4,9 @@ namespace noether { -FlatPlaneToolPathPlanner::FlatPlaneToolPathPlanner(const Eigen::Vector2d& plane_dims, const Eigen::Vector2d& point_spacing, Eigen::Isometry3d offset) +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) { } @@ -15,24 +17,25 @@ ToolPaths FlatPlaneToolPathPlanner::plan(const pcl::PolygonMesh& /*mesh*/) const 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 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++) + 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); + 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) { + if (i % 2 != 0) + { std::reverse(segment.begin(), segment.end()); tool_path.push_back(segment); } - else{ + else + { tool_path.push_back(segment); } segment.clear(); } tool_paths.push_back(tool_path); return tool_paths; - } } // namespace noether