WIP: Add ability to crop waypoints based on area selection

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")

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>")

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

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
 {

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