diff --git a/crs_application/CMakeLists.txt b/crs_application/CMakeLists.txt
index b3bfdc4e..3eae2df4 100644
--- a/crs_application/CMakeLists.txt
+++ b/crs_application/CMakeLists.txt
@@ -8,7 +8,7 @@ endif()
 
 # Default to C++14
 if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD 17)
 endif()
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
diff --git a/crs_area_selection/CMakeLists.txt b/crs_area_selection/CMakeLists.txt
index 362cf73b..9764d102 100644
--- a/crs_area_selection/CMakeLists.txt
+++ b/crs_area_selection/CMakeLists.txt
@@ -9,7 +9,7 @@ find_package(tf2_ros REQUIRED)
 find_package(tf2_eigen REQUIRED)
 
 find_package(Eigen3 REQUIRED)
-find_package(PCL 1.8 REQUIRED COMPONENTS sample_consensus segmentation surface)
+find_package(PCL 1.8 REQUIRED COMPONENTS sample_consensus segmentation surface io)
 
 find_package(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
@@ -21,7 +21,7 @@ find_package(crs_msgs REQUIRED)
 # Targets
 # Library containing tools for specifying regions and determining inlying points
 add_library(${PROJECT_NAME}_area_selection SHARED src/area_selector.cpp src/selection_artist.cpp)
-set_target_properties(${PROJECT_NAME}_area_selection PROPERTIES CXX_STANDARD 14)
+set_target_properties(${PROJECT_NAME}_area_selection PROPERTIES CXX_STANDARD 17)
 target_include_directories(${PROJECT_NAME}_area_selection PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include>")
@@ -53,7 +53,7 @@ ament_target_dependencies(${PROJECT_NAME}_area_selection
 # Node that runs area selection service
 add_executable(${PROJECT_NAME}_area_selection_node src/area_selection_node.cpp)
 target_link_libraries(${PROJECT_NAME}_area_selection_node ${PROJECT_NAME}_area_selection)
-set_target_properties(${PROJECT_NAME}_area_selection_node PROPERTIES CXX_STANDARD 14)
+set_target_properties(${PROJECT_NAME}_area_selection_node PROPERTIES CXX_STANDARD 17)
 target_include_directories(${PROJECT_NAME}_area_selection_node PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include>")
diff --git a/crs_area_selection/include/crs_area_selection/area_selector.h b/crs_area_selection/include/crs_area_selection/area_selector.h
index 0c6db797..92a75385 100644
--- a/crs_area_selection/include/crs_area_selection/area_selector.h
+++ b/crs_area_selection/include/crs_area_selection/area_selector.h
@@ -28,11 +28,14 @@
 #include <sensor_msgs/msg/point_cloud2.hpp>
 
 #include "crs_area_selection/area_selector_parameters.h"
+#include <crs_area_selection/types.h>
 
 namespace crs_area_selection
 {
 struct FittedPlane
 {
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
   Eigen::Vector3d normal = Eigen::Vector3d::UnitZ();
   Eigen::Vector3d origin = Eigen::Vector3d::Zero();
 };
@@ -47,6 +50,8 @@ struct FittedPlane
 class AreaSelector
 {
 public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
   /**
    * @brief AreaSelector class constructor
    */
@@ -59,20 +64,20 @@ class AreaSelector
    * otherwise returns true.
    */
   std::vector<int> getRegionOfInterest(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
-                                       const std::vector<Eigen::Vector3d>& points,
+                                       const PointVector& points,
                                        const AreaSelectorParameters& params);
 
 protected:
-  boost::optional<FittedPlane> fitPlaneToPoints(const std::vector<Eigen::Vector3d>& points,
+  boost::optional<FittedPlane> fitPlaneToPoints(const PointVector& points,
                                                 const AreaSelectorParameters& params);
 
-  //  pcl::PointCloud<pcl::PointXYZ>::Ptr projectPointsOntoPlane(const std::vector<Eigen::Vector3d>& points,
-  //                                                             const FittedPlane& plane);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr projectPointsOntoPlane(const PointVector& points,
+                                                               const FittedPlane& plane);
 
-  //  std::vector<int> getPointsInROI(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
-  //                                  const pcl::PointCloud<pcl::PointXYZ>::Ptr proj_sel_points,
-  //                                  const FittedPlane& plane,
-  //                                  const AreaSelectorParameters& params);
+    std::vector<int> getPointsInROI(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
+                                    const pcl::PointCloud<pcl::PointXYZ>::Ptr proj_sel_points,
+                                    const FittedPlane& plane,
+                                    const AreaSelectorParameters& params);
 };
 
 }  // namespace crs_area_selection
diff --git a/crs_area_selection/include/crs_area_selection/selection_artist.h b/crs_area_selection/include/crs_area_selection/selection_artist.h
index 621f6a22..e054b5f8 100644
--- a/crs_area_selection/include/crs_area_selection/selection_artist.h
+++ b/crs_area_selection/include/crs_area_selection/selection_artist.h
@@ -29,6 +29,7 @@
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <std_srvs/srv/trigger.hpp>
 #include <crs_msgs/srv/get_roi_selection.hpp>
+#include <crs_area_selection/types.h>
 
 namespace crs_area_selection
 {
diff --git a/crs_area_selection/include/crs_area_selection/types.h b/crs_area_selection/include/crs_area_selection/types.h
new file mode 100644
index 00000000..f88c9cf4
--- /dev/null
+++ b/crs_area_selection/include/crs_area_selection/types.h
@@ -0,0 +1,12 @@
+#ifndef CRS_AREA_SELECTION_TYPES_H
+#define CRS_AREA_SELECTION_TYPES_H
+
+#include <Eigen/StdVector>
+
+namespace crs_area_selection
+{
+using PointVector = std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> >;
+
+}
+
+#endif
diff --git a/crs_area_selection/src/area_selector.cpp b/crs_area_selection/src/area_selector.cpp
index a351f71d..14ae0eab 100644
--- a/crs_area_selection/src/area_selector.cpp
+++ b/crs_area_selection/src/area_selector.cpp
@@ -73,7 +73,7 @@ bool clusterComparator(const pcl::PointIndices& a,
 
 namespace crs_area_selection
 {
-boost::optional<FittedPlane> AreaSelector::fitPlaneToPoints(const std::vector<Eigen::Vector3d>& points,
+boost::optional<FittedPlane> AreaSelector::fitPlaneToPoints(const PointVector& points,
                                                             const AreaSelectorParameters& params)
 {
   // Create a point cloud from the selection points
@@ -87,8 +87,10 @@ boost::optional<FittedPlane> AreaSelector::fitPlaneToPoints(const std::vector<Ei
     pt.z = static_cast<float>((*it)(2));
 
     input_cloud->points.push_back(pt);
+    input_cloud->width += 1;
     centroid.add(pt);
   }
+  input_cloud->height = 1;
 
   pcl::PointXYZ origin;
   centroid.get(origin);
@@ -115,129 +117,130 @@ boost::optional<FittedPlane> AreaSelector::fitPlaneToPoints(const std::vector<Ei
   return boost::make_optional(plane);
 }
 
-// pcl::PointCloud<pcl::PointXYZ>::Ptr AreaSelector::projectPointsOntoPlane(const std::vector<Eigen::Vector3d>& points,
-//                                                                         const FittedPlane& plane)
-//{
-//  pcl::PointCloud<pcl::PointXYZ> projected_points;
-//  for (const Eigen::Vector3d& current_pt : points)
-//  {
-//    // Create a vector from plane origin to point
-//    Eigen::Vector3d vec(current_pt - plane.origin);
-
-//    // Project the vector connecting the origin and point onto the plane normal to get the distance that the point is
-//    // from the plane
-//    double dp = plane.normal.dot(vec);
-
-//    // Move the point from its current location along the plane's normal by its distance from the plane
-//    Eigen::Vector3d proj_pt = current_pt - dp * plane.normal;
-
-//    // Convert each point to PCL format
-//    pcl::PointXYZ pt;
-//    pt.x = static_cast<float>(proj_pt(0));
-//    pt.y = static_cast<float>(proj_pt(1));
-//    pt.z = static_cast<float>(proj_pt(2));
-
-//    projected_points.points.push_back(pt);
-//  }
-
-//  return projected_points.makeShared();
-//}
-
-// std::vector<int> AreaSelector::getPointsInROI(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
-//                                              const pcl::PointCloud<pcl::PointXYZ>::Ptr proj_sel_points,
-//                                              const FittedPlane& plane,
-//                                              const AreaSelectorParameters& params)
-//{
-//  // Calculate the diagonal of search_points_ cloud
-//  pcl::PointXYZ min_pt, max_pt;
-//  pcl::getMinMax3D(*cloud, min_pt, max_pt);
-//  double half_dist = std::sqrt(static_cast<double>(sqrDistance(min_pt, max_pt))) / 2.0;
-
-//  // Extrude the convex hull by half the max distance
-//  pcl::ExtractPolygonalPrismData<pcl::PointXYZ> prism;
-//  pcl::PointIndicesPtr selection_indices(new pcl::PointIndices{});
-//  prism.setInputCloud(cloud);
-//  prism.setInputPlanarHull(proj_sel_points);
-//  prism.setHeightLimits(-half_dist, half_dist);
-//  prism.segment(*selection_indices);
-
-//  if (selection_indices->indices.empty())
-//  {
+ pcl::PointCloud<pcl::PointXYZ>::Ptr AreaSelector::projectPointsOntoPlane(const PointVector& points,
+                                                                         const FittedPlane& plane)
+{
+  pcl::PointCloud<pcl::PointXYZ> projected_points;
+  for (const Eigen::Vector3d& current_pt : points)
+  {
+    // Create a vector from plane origin to point
+    Eigen::Vector3d vec(current_pt - plane.origin);
+
+    // Project the vector connecting the origin and point onto the plane normal to get the distance that the point is
+    // from the plane
+    double dp = plane.normal.dot(vec);
+
+    // Move the point from its current location along the plane's normal by its distance from the plane
+    Eigen::Vector3d proj_pt = current_pt - dp * plane.normal;
+
+    // Convert each point to PCL format
+    pcl::PointXYZ pt;
+    pt.x = static_cast<float>(proj_pt(0));
+    pt.y = static_cast<float>(proj_pt(1));
+    pt.z = static_cast<float>(proj_pt(2));
+
+    projected_points.points.push_back(pt);
+  }
+
+  return projected_points.makeShared();
+}
+
+ std::vector<int> AreaSelector::getPointsInROI(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
+                                              const pcl::PointCloud<pcl::PointXYZ>::Ptr proj_sel_points,
+                                              const FittedPlane& plane,
+                                              const AreaSelectorParameters& params)
+{
+  // Calculate the diagonal of search_points_ cloud
+  pcl::PointXYZ min_pt, max_pt;
+  pcl::getMinMax3D(*cloud, min_pt, max_pt);
+  double half_dist = std::sqrt(static_cast<double>(sqrDistance(min_pt, max_pt))) / 2.0;
+
+  // Extrude the convex hull by half the max distance
+  pcl::ExtractPolygonalPrismData<pcl::PointXYZ> prism;
+  pcl::PointIndicesPtr selection_indices(new pcl::PointIndices{});
+  prism.setInputCloud(cloud);
+  prism.setInputPlanarHull(proj_sel_points);
+  prism.setHeightLimits(-half_dist, half_dist);
+  prism.segment(*selection_indices);
+
+  if (selection_indices->indices.empty())
+  {
 //    ROS_ERROR("No points found within selection area");
-//    return {};
-//  }
-
-//  // Pull out the points that are inside the user selected prism
-//  pcl::PointCloud<pcl::PointXYZ>::Ptr prism_cloud(new pcl::PointCloud<pcl::PointXYZ>);
-//  pcl::ExtractIndices<pcl::PointXYZ> extract;
-//  extract.setInputCloud(cloud);
-//  extract.setIndices(selection_indices);
-//  extract.setNegative(false);
-//  extract.filter(*prism_cloud);
-
-//  // Estimate the normals for each point in the user selection
-//  pcl::search::Search<pcl::PointXYZ>::Ptr tree =
-//      boost::shared_ptr<pcl::search::Search<pcl::PointXYZ>>(new pcl::search::KdTree<pcl::PointXYZ>);
-//  pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
-//  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
-//  normal_estimator.setSearchMethod(tree);
-//  normal_estimator.setInputCloud(prism_cloud);
-//  normal_estimator.setRadiusSearch(params.normal_est_radius);
-//  normal_estimator.compute(*normals);
-
-//  // Perform region growing using euclidian distance and normals to estimate connectedness
-//  pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
-//  reg.setMinClusterSize(params.min_cluster_size);
-//  reg.setMaxClusterSize(params.max_cluster_size);
-//  reg.setSearchMethod(tree);
-//  reg.setNumberOfNeighbours(static_cast<unsigned int>(params.region_growing_nneighbors));
-//  reg.setInputCloud(prism_cloud);
-//  reg.setInputNormals(normals);
-//  reg.setSmoothnessThreshold(static_cast<float>(params.region_growing_smoothness / 180.0 * M_PI));
-//  reg.setCurvatureThreshold(static_cast<float>(params.region_growing_curvature));
-
-//  std::vector<pcl::PointIndices> int_indices;
-//  reg.extract(int_indices);
-
-//  // We need to translate the indices into the prism cloud into indices into the original cloud (via selection
-//  indices) std::vector<pcl::PointIndices> cluster_indices; cluster_indices.reserve(int_indices.size()); for (const
-//  auto& index_set : int_indices)
-//  {
-//    pcl::PointIndices output_set;
-//    output_set.header = index_set.header;
-//    for (const auto i : index_set.indices)
-//    {
-//      output_set.indices.push_back(selection_indices->indices[static_cast<std::size_t>(i)]);
-//    }
-//    cluster_indices.push_back(output_set);
-//  }
-
-//  if (cluster_indices.size() > 1)
-//  {
+    return {};
+  }
+
+  // Pull out the points that are inside the user selected prism
+  pcl::PointCloud<pcl::PointXYZ>::Ptr prism_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::ExtractIndices<pcl::PointXYZ> extract;
+  extract.setInputCloud(cloud);
+  extract.setIndices(selection_indices);
+  extract.setNegative(false);
+  extract.filter(*prism_cloud);
+
+  // Estimate the normals for each point in the user selection
+  pcl::search::Search<pcl::PointXYZ>::Ptr tree =
+      boost::shared_ptr<pcl::search::Search<pcl::PointXYZ>>(new pcl::search::KdTree<pcl::PointXYZ>);
+  pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
+  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
+  normal_estimator.setSearchMethod(tree);
+  normal_estimator.setInputCloud(prism_cloud);
+  normal_estimator.setRadiusSearch(params.normal_est_radius);
+  normal_estimator.compute(*normals);
+
+  // Perform region growing using euclidian distance and normals to estimate connectedness
+  pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
+  reg.setMinClusterSize(params.min_cluster_size);
+  reg.setMaxClusterSize(params.max_cluster_size);
+  reg.setSearchMethod(tree);
+  reg.setNumberOfNeighbours(static_cast<unsigned int>(params.region_growing_nneighbors));
+  reg.setInputCloud(prism_cloud);
+  reg.setInputNormals(normals);
+  reg.setSmoothnessThreshold(static_cast<float>(params.region_growing_smoothness / 180.0 * M_PI));
+  reg.setCurvatureThreshold(static_cast<float>(params.region_growing_curvature));
+
+  std::vector<pcl::PointIndices> int_indices;
+  reg.extract(int_indices);
+
+  // We need to translate the indices into the prism cloud into indices into the original cloud (via selection
+//  indices)
+  std::vector<pcl::PointIndices> cluster_indices; cluster_indices.reserve(int_indices.size());
+  for (const auto& index_set : int_indices)
+  {
+    pcl::PointIndices output_set;
+    output_set.header = index_set.header;
+    for (const auto i : index_set.indices)
+    {
+      output_set.indices.push_back(selection_indices->indices[static_cast<std::size_t>(i)]);
+    }
+    cluster_indices.push_back(output_set);
+  }
+
+  if (cluster_indices.size() > 1)
+  {
 //    ROS_INFO("%lu clusters found in ROI selection; choosing closest cluster to ROI selection centroid",
 //             cluster_indices.size());
 
-//    // Multiple clusters of points found, so find the cluster closest to the centroid of the selection polygon
-//    pcl::PointXYZ polygon_centroid;
-//    polygon_centroid.x = static_cast<float>(plane.origin(0));
-//    polygon_centroid.y = static_cast<float>(plane.origin(1));
-//    polygon_centroid.z = static_cast<float>(plane.origin(2));
-
-//    // Find the closest cluster
-//    auto closest_cluster = std::min_element(cluster_indices.begin(),
-//                                            cluster_indices.end(),
-//                                            boost::bind(&clusterComparator, _1, _2, cloud, polygon_centroid));
-//    return closest_cluster->indices;
-//  }
-//  else
-//  {
-//    // Either one or zero clusters were found, so just return all of the points in the extruded hull
-//    return selection_indices->indices;
-//  }
-//}
+    // Multiple clusters of points found, so find the cluster closest to the centroid of the selection polygon
+    pcl::PointXYZ polygon_centroid;
+    polygon_centroid.x = static_cast<float>(plane.origin(0));
+    polygon_centroid.y = static_cast<float>(plane.origin(1));
+    polygon_centroid.z = static_cast<float>(plane.origin(2));
+
+    // Find the closest cluster
+    auto closest_cluster = std::min_element(cluster_indices.begin(),
+                                            cluster_indices.end(),
+                                            boost::bind(&clusterComparator, _1, _2, cloud, polygon_centroid));
+    return closest_cluster->indices;
+  }
+  else
+  {
+    // Either one or zero clusters were found, so just return all of the points in the extruded hull
+    return selection_indices->indices;
+  }
+}
 
 std::vector<int> AreaSelector::getRegionOfInterest(const pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud,
-                                                   const std::vector<Eigen::Vector3d>& points,
+                                                   const PointVector& points,
                                                    const AreaSelectorParameters& params)
 {
   // Check size of selection points vector
@@ -262,19 +265,19 @@ std::vector<int> AreaSelector::getRegionOfInterest(const pcl::PointCloud<pcl::Po
     return {};
   }
 
-  //  // Project the selection points onto the fitted plane
-  //  pcl::PointCloud<pcl::PointXYZ>::Ptr proj_sel_points = projectPointsOntoPlane(points, plane.get());
+    // Project the selection points onto the fitted plane
+    pcl::PointCloud<pcl::PointXYZ>::Ptr proj_sel_points = projectPointsOntoPlane(points, plane.get());
 
-  //  /* Find all of the sensor data points inside a volume whose perimeter is defined by the projected selection points
-  //   * and whose depth is defined by the largest length of the bounding box of the input point cloud */
-  //  std::vector<int> roi_cloud_indices = getPointsInROI(input_cloud, proj_sel_points, plane.get(), params);
-  //  if (roi_cloud_indices.empty())
-  //  {
-  //    ROS_ERROR("Unable to identify points in the region of interest");
-  //    return {};
-  //  }
+    /* Find all of the sensor data points inside a volume whose perimeter is defined by the projected selection points
+     * and whose depth is defined by the largest length of the bounding box of the input point cloud */
+    std::vector<int> roi_cloud_indices = getPointsInROI(input_cloud, proj_sel_points, plane.get(), params);
+    if (roi_cloud_indices.empty())
+    {
+//      ROS_ERROR("Unable to identify points in the region of interest");
+      return {};
+    }
 
-  std::vector<int> roi_cloud_indices;
+//  std::vector<int> roi_cloud_indices;
   return roi_cloud_indices;
 }
 
diff --git a/crs_area_selection/src/selection_artist.cpp b/crs_area_selection/src/selection_artist.cpp
index 2102bdac..22efa795 100644
--- a/crs_area_selection/src/selection_artist.cpp
+++ b/crs_area_selection/src/selection_artist.cpp
@@ -266,10 +266,11 @@ bool SelectionArtist::collectROIMesh(const shape_msgs::msg::Mesh& mesh_msg,
   pclFromShapeMsg(mesh_msg, mesh);
   pcl::PointCloud<pcl::PointXYZ> mesh_cloud;
   pcl::fromPCLPointCloud2(mesh.cloud, mesh_cloud);
-  crs_msgs::srv::GetROISelection::Request::SharedPtr req;
+  crs_msgs::srv::GetROISelection::Request::SharedPtr req(new crs_msgs::srv::GetROISelection::Request());
   pcl::toROSMsg(mesh_cloud, req->input_cloud);
 
-  crs_msgs::srv::GetROISelection::Response::SharedPtr res;
+  crs_msgs::srv::GetROISelection::Response::SharedPtr res(new crs_msgs::srv::GetROISelection::Response());
+//  auto res = std::make_shared<crs_msgs::srv::GetROISelection::Response>();
   collectROIPointsCb(req, res);
   if (!res->success)
   {
@@ -293,7 +294,7 @@ void SelectionArtist::collectROIPointsCb(crs_msgs::srv::GetROISelection::Request
                                 [](const visualization_msgs::msg::Marker& marker) { return marker.id == 0; });
 
   // Convert the selection points to Eigen vectors
-  std::vector<Eigen::Vector3d> points;
+  PointVector points;
   for (const geometry_msgs::msg::Point& pt : points_it->points)
   {
     Eigen::Vector3d e;
diff --git a/crs_gui/CMakeLists.txt b/crs_gui/CMakeLists.txt
index 61c0687f..3117baa7 100644
--- a/crs_gui/CMakeLists.txt
+++ b/crs_gui/CMakeLists.txt
@@ -17,6 +17,7 @@ find_package(Qt5 REQUIRED COMPONENTS
   Core
   Widgets
 )
+find_package(Eigen3 REQUIRED)
 
 ###########
 ## Build ##
@@ -66,7 +67,7 @@ add_library(${PROJECT_NAME}_widgets SHARED
   ${crs_gui_widget_MOCS}
   ${crs_gui_widget_SOURCES}
 )
-set_target_properties(${PROJECT_NAME}_widgets PROPERTIES CXX_STANDARD 14)
+set_target_properties(${PROJECT_NAME}_widgets PROPERTIES CXX_STANDARD 17)
 target_include_directories(${PROJECT_NAME}_widgets PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include>")
@@ -113,28 +114,28 @@ ament_target_dependencies(${PROJECT_NAME}_widgets rclcpp shape_msgs std_srvs crs
 # Area selection widget demo
 add_executable(${PROJECT_NAME}_area_selection_demo src/nodes/area_selection_widget_demo.cpp)
 target_link_libraries(${PROJECT_NAME}_area_selection_demo ${PROJECT_NAME}_widgets)
-set_target_properties(${PROJECT_NAME}_area_selection_demo PROPERTIES CXX_STANDARD 14)
+set_target_properties(${PROJECT_NAME}_area_selection_demo PROPERTIES CXX_STANDARD 17)
 target_include_directories(${PROJECT_NAME}_area_selection_demo PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include>")
 
 add_executable(${PROJECT_NAME}_part_selection_demo src/nodes/part_selection_widget_demo.cpp)
 target_link_libraries(${PROJECT_NAME}_part_selection_demo ${PROJECT_NAME}_widgets)
-set_target_properties(${PROJECT_NAME}_part_selection_demo PROPERTIES CXX_STANDARD 14)
+set_target_properties(${PROJECT_NAME}_part_selection_demo PROPERTIES CXX_STANDARD 17)
 target_include_directories(${PROJECT_NAME}_part_selection_demo PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include>")
 
 add_executable(${PROJECT_NAME}_crs_application_demo src/nodes/crs_application_demo.cpp)
 target_link_libraries(${PROJECT_NAME}_crs_application_demo ${PROJECT_NAME}_widgets)
-set_target_properties(${PROJECT_NAME}_crs_application_demo PROPERTIES CXX_STANDARD 14)
+set_target_properties(${PROJECT_NAME}_crs_application_demo PROPERTIES CXX_STANDARD 17)
 target_include_directories(${PROJECT_NAME}_crs_application_demo PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include>")
 
 add_executable(${PROJECT_NAME}_state_machine_interface_demo src/nodes/state_machine_interface_widget_demo.cpp)
 target_link_libraries(${PROJECT_NAME}_state_machine_interface_demo ${PROJECT_NAME}_widgets)
-set_target_properties(${PROJECT_NAME}_state_machine_interface_demo PROPERTIES CXX_STANDARD 14)
+set_target_properties(${PROJECT_NAME}_state_machine_interface_demo PROPERTIES CXX_STANDARD 17)
 target_include_directories(${PROJECT_NAME}_state_machine_interface_demo PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include>")
diff --git a/crs_gui/include/crs_gui/widgets/crs_application_widget.h b/crs_gui/include/crs_gui/widgets/crs_application_widget.h
index af23cce2..79fe610b 100644
--- a/crs_gui/include/crs_gui/widgets/crs_application_widget.h
+++ b/crs_gui/include/crs_gui/widgets/crs_application_widget.h
@@ -26,6 +26,10 @@
 #include <crs_msgs/srv/execute_action.hpp>
 #include <crs_msgs/srv/get_configuration.hpp>
 #include <string>
+#include <eigen3/Eigen/Core>
+
+#define EIGEN_DONT_VECTORIZE
+#define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
 
 namespace Ui
 {
@@ -42,6 +46,8 @@ class CRSApplicationWidget : public QWidget
 {
   Q_OBJECT
 public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
   CRSApplicationWidget(rclcpp::Node::SharedPtr node,
                        QWidget* parent = nullptr,
                        std::string database_directory = std::string(std::getenv("HOME")) + "/.local/share/"
diff --git a/crs_gui/include/crs_gui/widgets/part_selection_widget.h b/crs_gui/include/crs_gui/widgets/part_selection_widget.h
index 29403382..234dacde 100644
--- a/crs_gui/include/crs_gui/widgets/part_selection_widget.h
+++ b/crs_gui/include/crs_gui/widgets/part_selection_widget.h
@@ -21,6 +21,9 @@
 #include <memory>
 #include <string>
 
+#define EIGEN_DONT_VECTORIZE
+#define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
+
 class QListWidgetItem;
 
 namespace Ui
diff --git a/crs_gui/include/crs_gui/widgets/polygon_area_selection_widget.h b/crs_gui/include/crs_gui/widgets/polygon_area_selection_widget.h
index 858f6f9b..6c55fbf0 100644
--- a/crs_gui/include/crs_gui/widgets/polygon_area_selection_widget.h
+++ b/crs_gui/include/crs_gui/widgets/polygon_area_selection_widget.h
@@ -21,10 +21,16 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <shape_msgs/msg/mesh.hpp>
+#include <eigen3/Eigen/Core>
+
 
 #include <crs_area_selection/selection_artist.h>
 #include "crs_gui/register_ros_msgs_for_qt.h"
 
+
+#define EIGEN_DONT_VECTORIZE
+#define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
+
 namespace Ui
 {
 class PolygonAreaSelectionWidget;
@@ -35,8 +41,9 @@ namespace crs_gui
 class PolygonAreaSelectionWidget : public QWidget
 {
   Q_OBJECT
-
 public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
   explicit PolygonAreaSelectionWidget(rclcpp::Node::SharedPtr node,
                                       const std::string& selection_world_frame,
                                       const std::string& selection_sensor_frame,
@@ -45,7 +52,9 @@ class PolygonAreaSelectionWidget : public QWidget
 
 public Q_SLOTS:
 
-  void init(const shape_msgs::msg::Mesh& mesh);
+  void loadShapeMsg(const shape_msgs::msg::Mesh& mesh);
+
+  void loadMeshFile(const std::string& filepath);
 
 Q_SIGNALS:
   void selectedSubmesh(const shape_msgs::msg::Mesh::SharedPtr& selected_submesh);
diff --git a/crs_gui/include/crs_gui/widgets/state_machine_interface_widget.h b/crs_gui/include/crs_gui/widgets/state_machine_interface_widget.h
index d8a8cfd4..0e5fe8e0 100644
--- a/crs_gui/include/crs_gui/widgets/state_machine_interface_widget.h
+++ b/crs_gui/include/crs_gui/widgets/state_machine_interface_widget.h
@@ -25,6 +25,10 @@
 #include <std_msgs/msg/string.hpp>
 #include <string>
 
+
+#define EIGEN_DONT_VECTORIZE
+#define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
+
 namespace Ui
 {
 class StateMachineInterface;
diff --git a/crs_gui/src/widgets/crs_application_widget.cpp b/crs_gui/src/widgets/crs_application_widget.cpp
index 6a300a04..a2710a30 100644
--- a/crs_gui/src/widgets/crs_application_widget.cpp
+++ b/crs_gui/src/widgets/crs_application_widget.cpp
@@ -50,8 +50,8 @@ CRSApplicationWidget::CRSApplicationWidget(rclcpp::Node::SharedPtr node,
   , node_(node)
   , database_directory_(database_directory)
   , part_selector_widget_(new PartSelectionWidget(parent, database_directory))
-  , area_selection_widget_(new PolygonAreaSelectionWidget(node, "world", "world"))
-  , state_machine_interface_widget_(new StateMachineInterfaceWidget(node))
+  , area_selection_widget_(new PolygonAreaSelectionWidget(node, "world", "world", parent))
+  , state_machine_interface_widget_(new StateMachineInterfaceWidget(node, parent))
 {
   ui_->setupUi(this);
 
@@ -111,6 +111,13 @@ void CRSApplicationWidget::onPartSelected(const std::string selected_part)
   visualization_msgs::msg::MarkerArray array;
   array.markers.push_back(marker);
   current_mesh_marker_ = array;
+
+  std::string path2 = database_directory_ + "/" + selected_part + "/" + selected_part + ".ply";
+  area_selection_widget_->loadMeshFile(path2);
+
+  // Clear the old toolpath
+  current_toolpath_marker_ = delete_all_marker_;
+  toolpath_marker_pub_->publish(delete_all_marker_);
 }
 
 void CRSApplicationWidget::onPartPathSelected(const std::string selected_part, const std::string selected_path)
diff --git a/crs_gui/src/widgets/part_selection_widget.cpp b/crs_gui/src/widgets/part_selection_widget.cpp
index e30ddfef..2bc9f922 100644
--- a/crs_gui/src/widgets/part_selection_widget.cpp
+++ b/crs_gui/src/widgets/part_selection_widget.cpp
@@ -121,6 +121,12 @@ void PartSelectionWidget::onPartSelected()
     emit partSelected(current_part);
     emit partPathSelected(current_part, current_path);
   }
+  else if (ui_->list_widget_parts && ui_->list_widget_parts->currentItem())
+  {
+    std::string current_part =
+        ui_->list_widget_parts->currentItem()->data(Qt::ItemDataRole::UserRole).toString().toUtf8().constData();
+    emit partSelected(current_part);
+  }
 }
 
 }  // namespace crs_gui
diff --git a/crs_gui/src/widgets/polygon_area_selection_widget.cpp b/crs_gui/src/widgets/polygon_area_selection_widget.cpp
index f935431c..0f373181 100644
--- a/crs_gui/src/widgets/polygon_area_selection_widget.cpp
+++ b/crs_gui/src/widgets/polygon_area_selection_widget.cpp
@@ -20,9 +20,64 @@
 #include <QPushButton>
 
 #include <std_srvs/srv/trigger.h>
+#include <pcl/io/ply_io.h>
 
 #include <ui_polygon_area_selection_widget.h>
 
+namespace
+{
+/**
+ * @brief ConvertToMeshMsg Converts a polygon mesh to a mesh msgs
+ *
+ * TODO: Port all/most of noether conversions to ROS 2 and pull it into crs
+ * @param mesh Input pcl::PolygonMesh
+ * @param mesh_msg Resulting shape_msgs::Mesh
+ * @return true if successful
+ */
+bool ConvertToMeshMsg(const pcl::PolygonMesh& mesh, shape_msgs::msg::Mesh& mesh_msg)
+{
+  if (mesh.polygons.empty())
+  {
+    std::cerr << "PolygonMesh has no polygons" << std::endl;
+    //    CONSOLE_BRIDGE_logError("PolygonMesh has no polygons");
+    return false;
+  }
+
+  pcl::PointCloud<pcl::PointXYZ> cloud;
+  pcl::fromPCLPointCloud2(mesh.cloud, cloud);
+  if (cloud.empty())
+  {
+    //    CONSOLE_BRIDGE_logError("PolygonMesh has no vertices data");
+    return false;
+  }
+
+  // copying triangles
+  mesh_msg.triangles.resize(mesh.polygons.size());
+  for (std::size_t i = 0; i < mesh.polygons.size(); i++)
+  {
+    const pcl::Vertices& vertices = mesh.polygons[i];
+    if (vertices.vertices.size() != 3)
+    {
+      //      CONSOLE_BRIDGE_logError("Vertex in PolygonMesh needs to have 3 elements only");
+      return false;
+    }
+
+    std::array<uint32_t, 3>& vertex = mesh_msg.triangles[i].vertex_indices;
+    std::tie(vertex[0], vertex[1], vertex[2]) =
+        std::make_tuple(vertices.vertices[0], vertices.vertices[1], vertices.vertices[2]);
+  }
+
+  // copying vertices
+  mesh_msg.vertices.resize(cloud.size());
+  std::transform(cloud.begin(), cloud.end(), mesh_msg.vertices.begin(), [](pcl::PointXYZ& v) {
+    geometry_msgs::msg::Point p;
+    std::tie(p.x, p.y, p.z) = std::make_tuple(v.x, v.y, v.z);
+    return std::move(p);
+  });
+  return true;
+}
+}  // namespace
+
 namespace crs_gui
 {
 PolygonAreaSelectionWidget::PolygonAreaSelectionWidget(rclcpp::Node::SharedPtr node,
@@ -42,13 +97,35 @@ PolygonAreaSelectionWidget::PolygonAreaSelectionWidget(rclcpp::Node::SharedPtr n
 
 PolygonAreaSelectionWidget::~PolygonAreaSelectionWidget() = default;
 
-void PolygonAreaSelectionWidget::init(const shape_msgs::msg::Mesh& mesh)
+void PolygonAreaSelectionWidget::loadShapeMsg(const shape_msgs::msg::Mesh& mesh)
 {
   mesh_.reset(new shape_msgs::msg::Mesh(mesh));
   clearROISelection();
   return;
 }
 
+void PolygonAreaSelectionWidget::loadMeshFile(const std::string& filepath)
+{
+  RCLCPP_INFO_STREAM(node_->get_logger(), "Area selection loading file: " << filepath.c_str());
+  pcl::PolygonMesh polygon_mesh;
+  if (pcl::io::loadPLYFile(filepath, polygon_mesh))
+  {
+    RCLCPP_ERROR_STREAM(node_->get_logger(), "Mesh loading failed");
+    return;
+  }
+
+  shape_msgs::msg::Mesh shape_msg;
+  if (ConvertToMeshMsg(polygon_mesh, shape_msg))
+  {
+    mesh_ = std::make_shared<shape_msgs::msg::Mesh>(shape_msg);
+    RCLCPP_INFO_STREAM(node_->get_logger(), "Mesh loaded");
+  }
+  else
+  {
+    RCLCPP_INFO_STREAM(node_->get_logger(), "Mesh failed to convert");
+  }
+}
+
 void PolygonAreaSelectionWidget::clearROISelection()
 {
   auto req = std::make_shared<std_srvs::srv::Trigger::Request>();
@@ -60,10 +137,9 @@ void PolygonAreaSelectionWidget::clearROISelection()
     RCLCPP_ERROR_STREAM(node_->get_logger(),
                         "Tool Path Parameter Editor Widget: Area Selection error:" << res->message);
   }
-  // This will be uncommented when mesh selection is turned back on
-  //  submesh_.reset(new shape_msgs::msg::Mesh(*mesh_));
+  submesh_.reset(new shape_msgs::msg::Mesh(*mesh_));
 
-  //  emit(selectedSubmesh(submesh_));
+  emit(selectedSubmesh(submesh_));
   return;
 }
 
@@ -71,8 +147,6 @@ void PolygonAreaSelectionWidget::applySelection()
 {
   if (!mesh_)
   {
-    // TODO: Currently this will always happen because we are not setting a mesh. Basically we are just drawing on the
-    // surface an not doing anything
     QMessageBox::warning(this, "Tool Path Planning Error", "No mesh available to crop");
     return;
   }
diff --git a/crs_motion_planning/CMakeLists.txt b/crs_motion_planning/CMakeLists.txt
index 09a5ce64..707b0965 100644
--- a/crs_motion_planning/CMakeLists.txt
+++ b/crs_motion_planning/CMakeLists.txt
@@ -8,7 +8,7 @@ endif()
 
 # Default to C++14
 if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD 17)
 endif()
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
@@ -44,6 +44,8 @@ find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(tf2_eigen REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
+find_package(Eigen3 REQUIRED)
+
 
 find_package(yaml-cpp REQUIRED)
 
diff --git a/crs_motion_planning/include/crs_motion_planning/path_processing_utils.h b/crs_motion_planning/include/crs_motion_planning/path_processing_utils.h
index f8203b74..fe6bcc11 100644
--- a/crs_motion_planning/include/crs_motion_planning/path_processing_utils.h
+++ b/crs_motion_planning/include/crs_motion_planning/path_processing_utils.h
@@ -1,6 +1,9 @@
 #ifndef CRS_MOTION_PLANNING_PATH_PROCESSING_UTILS_H
 #define CRS_MOTION_PLANNING_PATH_PROCESSING_UTILS_H
 
+#define EIGEN_DONT_VECTORIZE
+#define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
+
 #include <yaml-cpp/yaml.h>
 #include <yaml-cpp/node/iterator.h>
 #include <yaml-cpp/node/impl.h>
diff --git a/crs_msgs/CMakeLists.txt b/crs_msgs/CMakeLists.txt
index 7fb47777..c2537f6b 100644
--- a/crs_msgs/CMakeLists.txt
+++ b/crs_msgs/CMakeLists.txt
@@ -8,7 +8,7 @@ endif()
 
 # Default to C++14
 if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD 17)
 endif()
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
diff --git a/crs_perception/CMakeLists.txt b/crs_perception/CMakeLists.txt
index 11a4c3ac..ddeea56e 100644
--- a/crs_perception/CMakeLists.txt
+++ b/crs_perception/CMakeLists.txt
@@ -8,7 +8,7 @@ endif()
 
 # Default to C++14
 if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD 17)
   set(CMAKE_CXX_STANDARD_REQUIRED ON)
   set(CMAKE_CXX_EXTENSIONS OFF)
 endif()
diff --git a/crs_support/CMakeLists.txt b/crs_support/CMakeLists.txt
index 967c0835..ca77f602 100644
--- a/crs_support/CMakeLists.txt
+++ b/crs_support/CMakeLists.txt
@@ -8,7 +8,7 @@ endif()
 
 # Default to C++14
 if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD 17)
 endif()
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
diff --git a/crs_toolpath_gen/CMakeLists.txt b/crs_toolpath_gen/CMakeLists.txt
index 4437d527..c90eb7b9 100644
--- a/crs_toolpath_gen/CMakeLists.txt
+++ b/crs_toolpath_gen/CMakeLists.txt
@@ -8,7 +8,7 @@ endif()
 
 # Default to C++14
 if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD 17)
 endif()
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")