diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 18da2b1e..3bd1e547 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -83,12 +83,6 @@ target_link_libraries(pcl_ros_tf PUBLIC # # -### Declare library for base filter plugin -add_library(pcl_ros_filter - src/pcl_ros/filters/filter.cpp -) -target_link_libraries(pcl_ros_filter pcl_ros_tf ${PCL_LIBRARIES}) - ### Declare the pcl_ros_filters library add_library(pcl_ros_filters SHARED src/pcl_ros/filters/extract_indices.cpp @@ -102,7 +96,6 @@ add_library(pcl_ros_filters SHARED target_link_libraries(pcl_ros_filters PUBLIC ${PCL_LIBRARIES} ${visualization_msgs_TARGETS} - pcl_ros_filter pcl_ros_tf rclcpp_components::component ) @@ -137,16 +130,23 @@ rclcpp_components_register_node(pcl_ros_filters class_loader_hide_library_symbols(pcl_ros_filters) # ### Declare the pcl_ros_segmentation library -#add_library (pcl_ros_segmentation +add_library (pcl_ros_segmentation SHARED # src/pcl_ros/segmentation/extract_clusters.cpp # src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp -# src/pcl_ros/segmentation/sac_segmentation.cpp + src/pcl_ros/segmentation/sac_segmentation.cpp # src/pcl_ros/segmentation/segment_differences.cpp # src/pcl_ros/segmentation/segmentation.cpp -#) -#target_link_libraries(pcl_ros_segmentation pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) -#add_dependencies(pcl_ros_segmentation ${PROJECT_NAME}_gencfg) -#class_loader_hide_library_symbols(pcl_ros_segmentation) +) +target_link_libraries(pcl_ros_segmentation PUBLIC + ${PCL_LIBRARIES} + pcl_ros_tf + rclcpp_components::component +) +rclcpp_components_register_node(pcl_ros_segmentation + PLUGIN "pcl_ros::SACSegmentation" + EXECUTABLE segmentation_sac_node +) +class_loader_hide_library_symbols(pcl_ros_segmentation) # ### Declare the pcl_ros_surface library #add_library (pcl_ros_surface @@ -290,10 +290,9 @@ install( pcd_to_pointcloud_lib # pcl_ros_io # pcl_ros_features - pcl_ros_filter pcl_ros_filters # pcl_ros_surface -# pcl_ros_segmentation + pcl_ros_segmentation pointcloud_to_pcd_lib combined_pointcloud_to_pcd_lib bag_to_pcd_lib diff --git a/pcl_ros/include/pcl_ros/filters/crop_box.hpp b/pcl_ros/include/pcl_ros/filters/crop_box.hpp index 59fb74f3..d2f6b63c 100644 --- a/pcl_ros/include/pcl_ros/filters/crop_box.hpp +++ b/pcl_ros/include/pcl_ros/filters/crop_box.hpp @@ -43,7 +43,7 @@ #include #include #include -#include "pcl_ros/filters/filter.hpp" +#include "pcl_ros/pcl_node.hpp" namespace pcl_ros { @@ -52,49 +52,44 @@ namespace pcl_ros * \author Radu Bogdan Rusu * \author Justin Rosen * \author Marti Morta Garriga + * \author Antonio Brandi */ -class CropBox : public Filter +class CropBox : public PCLNode, Output> { -protected: - /** \brief Call the actual filter. - * \param input the input point cloud dataset - * \param indices the input set of indices to use from \a input - * \param output the resultant filtered dataset +private: + /** \brief The PCL filter implementation used. */ - inline void - filter( - const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, - PointCloud2 & output) override; + pcl::CropBox impl_; - /** \brief Parameter callback - * \param params parameter values to set + /** \brief The crop box marker message. + * The marker's cube gets updated whenever the min/max points are changed. + * The header is adjusted with every point cloud callback. */ - rcl_interfaces::msg::SetParametersResult - config_callback(const std::vector & params); - /** \brief Create publishers for output PointCloud2 data as well as the crop box marker. */ - void createPublishers() override; - /** \brief Update the crop box marker msg. */ - void update_marker_msg(); - - OnSetParametersCallbackHandle::SharedPtr callback_handle_; - /** \brief The crop box marker msg publisher for visualization and debugging purposes. */ - rclcpp::Publisher::SharedPtr crop_box_marker_publisher_; - /** - * \brief The crop box marker message. - * - * The marker's cube gets updated whenever the min/max points are changed. - * The header is adjusted with every point cloud callback. - */ visualization_msgs::msg::Marker crop_box_marker_msg_; -private: - /** \brief The PCL filter implementation used. */ - pcl::CropBox impl_; + /** \brief Update the crop box marker msg. */ + void updateMarkerMsg(); -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /** \brief Parameter callback + * \param params parameter values to set. + */ + virtual rcl_interfaces::msg::SetParametersResult onParamsChanged( + const std::vector & params) override; +public: + /** \brief Constructor. + * \param options A rclcpp::NodeOptions to be passed to the node. + */ explicit CropBox(const rclcpp::NodeOptions & options); + + /** \brief Calls the actual CropBox PCL filter. + * \param input the input point cloud dataset. + * \param output the resultant filtered dataset. + * \param marker the crop box marker for visualization and debugging purposes. + */ + virtual void compute( + const PointCloud2 & input, PointCloud2 & output, + visualization_msgs::msg::Marker & marker) override; }; } // namespace pcl_ros diff --git a/pcl_ros/include/pcl_ros/filters/extract_indices.hpp b/pcl_ros/include/pcl_ros/filters/extract_indices.hpp index 8f4bc55a..e2e89994 100644 --- a/pcl_ros/include/pcl_ros/filters/extract_indices.hpp +++ b/pcl_ros/include/pcl_ros/filters/extract_indices.hpp @@ -41,43 +41,42 @@ // PCL includes #include #include -#include "pcl_ros/filters/filter.hpp" +#include "pcl_ros/pcl_node.hpp" namespace pcl_ros { /** \brief @b ExtractIndices extracts a set of indices from a PointCloud as a separate PointCloud. * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. * \author Radu Bogdan Rusu + * \author Antonio Brandi */ -class ExtractIndices : public Filter +class ExtractIndices : public PCLNode, Output> { -protected: - /** \brief Call the actual filter. - * \param input the input point cloud dataset - * \param indices the input set of indices to use from \a input - * \param output the resultant filtered dataset - */ - inline void - filter( - const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, - PointCloud2 & output) override; +private: + /** \brief The PCL filter implementation used. */ + pcl::ExtractIndices impl_; /** \brief Parameter callback * \param params parameter values to set */ rcl_interfaces::msg::SetParametersResult - config_callback(const std::vector & params); - - OnSetParametersCallbackHandle::SharedPtr callback_handle_; - -private: - /** \brief The PCL filter implementation used. */ - pcl::ExtractIndices impl_; + onParamsChanged(const std::vector & params) override; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - + /** \brief Constructor + * \param options A rclcpp::NodeOptions to be passed to the node. + */ explicit ExtractIndices(const rclcpp::NodeOptions & options); + + /** \brief Calls the actual RadiusOutlierRemoval PCL filter. + * \param input the input point cloud dataset. + * \param indices the input set of indices to use from input. + * \param output the resultant filtered dataset. + */ + virtual void compute( + const PointCloud2 & input, const PointIndices & indices, + PointCloud2 & output) override; + }; } // namespace pcl_ros diff --git a/pcl_ros/include/pcl_ros/filters/filter.hpp b/pcl_ros/include/pcl_ros/filters/filter.hpp deleted file mode 100644 index 08994bb8..00000000 --- a/pcl_ros/include/pcl_ros/filters/filter.hpp +++ /dev/null @@ -1,167 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2009, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * $Id: filter.h 35876 2011-02-09 01:04:36Z rusu $ - * - */ - -#ifndef PCL_ROS__FILTERS__FILTER_HPP_ -#define PCL_ROS__FILTERS__FILTER_HPP_ - -#include -#include -#include -#include "pcl_ros/pcl_node.hpp" - -namespace pcl_ros -{ -namespace sync_policies = message_filters::sync_policies; - -/** \brief @b Filter represents the base filter class. Some generic 3D operations that are - * applicable to all filters are defined here as static methods. - * \author Radu Bogdan Rusu - */ -class Filter : public PCLNode -{ -public: - typedef sensor_msgs::msg::PointCloud2 PointCloud2; - - typedef pcl::IndicesPtr IndicesPtr; - typedef pcl::IndicesConstPtr IndicesConstPtr; - - /** \brief Filter constructor - * \param node_name node name - * \param options node options - */ - Filter(std::string node_name, const rclcpp::NodeOptions & options); - -protected: - /** \brief declare and subscribe to param callback for input_frame and output_frame params */ - void - use_frame_params(); - - /** \brief Add common parameters */ - std::vector add_common_params(); - - /** \brief The input PointCloud subscriber. */ - rclcpp::Subscription::SharedPtr sub_input_; - - message_filters::Subscriber sub_input_filter_; - - /** \brief The desired user filter field name. */ - std::string filter_field_name_; - - /** \brief The minimum allowed filter value a point will be considered from. */ - double filter_limit_min_; - - /** \brief The maximum allowed filter value a point will be considered from. */ - double filter_limit_max_; - - /** \brief Set to true if we want to return the data outside - * (\a filter_limit_min_;\a filter_limit_max_). Default: false. - */ - bool filter_limit_negative_; - - /** \brief The input TF frame the data should be transformed into, - * if input.header.frame_id is different. - */ - std::string tf_input_frame_; - - /** \brief The original data input TF frame. */ - std::string tf_input_orig_frame_; - - /** \brief The output TF frame the data should be transformed into, - * if input.header.frame_id is different. - */ - std::string tf_output_frame_; - - /** \brief Internal mutex. */ - std::mutex mutex_; - - /** \brief Virtual abstract filter method. To be implemented by every child. - * \param input the input point cloud dataset. - * \param indices a pointer to the vector of point indices to use. - * \param output the resultant filtered PointCloud2 - */ - virtual void - filter( - const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, - PointCloud2 & output) = 0; - - /** \brief Lazy transport subscribe routine. */ - virtual void - subscribe(); - - /** \brief Lazy transport unsubscribe routine. */ - virtual void - unsubscribe(); - - /** \brief Create publishers for output PointCloud2 data. */ - virtual void - createPublishers(); - - /** \brief Call the child filter () method, optionally transform the result, and publish it. - * \param input the input point cloud dataset. - * \param indices a pointer to the vector of point indices to use. - */ - void - computePublish(const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices); - -private: - /** \brief Pointer to parameters callback handle. */ - OnSetParametersCallbackHandle::SharedPtr callback_handle_; - - /** \brief Synchronized input, and indices.*/ - std::shared_ptr>> sync_input_indices_e_; - std::shared_ptr>> sync_input_indices_a_; - - /** \brief Parameter callback - * \param params parameter values to set - */ - rcl_interfaces::msg::SetParametersResult - config_callback(const std::vector & params); - - /** \brief PointCloud2 + Indices data callback. */ - void - input_indices_callback( - const PointCloud2::ConstSharedPtr & cloud, - const PointIndices::ConstSharedPtr & indices); - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; -} // namespace pcl_ros - -#endif // PCL_ROS__FILTERS__FILTER_HPP_ diff --git a/pcl_ros/include/pcl_ros/filters/passthrough.hpp b/pcl_ros/include/pcl_ros/filters/passthrough.hpp index dd18bec8..337e099f 100644 --- a/pcl_ros/include/pcl_ros/filters/passthrough.hpp +++ b/pcl_ros/include/pcl_ros/filters/passthrough.hpp @@ -41,43 +41,41 @@ // PCL includes #include #include -#include "pcl_ros/filters/filter.hpp" +#include "pcl_ros/pcl_node.hpp" namespace pcl_ros { /** \brief @b PassThrough uses the base Filter class methods to pass through all data that satisfies the user given * constraints. * \author Radu Bogdan Rusu + * \author Antonio Brandi */ -class PassThrough : public Filter +class PassThrough : public PCLNode, Output> { -protected: - /** \brief Call the actual filter. - * \param input the input point cloud dataset - * \param indices the input set of indices to use from \a input - * \param output the resultant filtered dataset - */ - inline void - filter( - const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, - PointCloud2 & output) override; - - /** \brief Parameter callback - * \param params parameter values to set - */ - rcl_interfaces::msg::SetParametersResult - config_callback(const std::vector & params); - - OnSetParametersCallbackHandle::SharedPtr callback_handle_; - private: + /** \brief Tolerance for comparing floating point parameters. */ + static constexpr double PARAMETER_TOLERANCE = 1e-6; + /** \brief The PCL filter implementation used. */ pcl::PassThrough impl_; -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /** \brief Parameter callback + * \param params parameter values to set. + */ + virtual rcl_interfaces::msg::SetParametersResult onParamsChanged( + const std::vector & params) override; +public: + /** \brief Constructor + * \param options A rclcpp::NodeOptions to be passed to the node. + */ explicit PassThrough(const rclcpp::NodeOptions & options); + + /** \brief Calls the actual RadiusOutlierRemoval PCL filter. + * \param input the input point cloud dataset. + * \param output the resultant filtered dataset. + */ + virtual void compute(const PointCloud2 & input, PointCloud2 & output) override; }; } // namespace pcl_ros diff --git a/pcl_ros/include/pcl_ros/filters/project_inliers.hpp b/pcl_ros/include/pcl_ros/filters/project_inliers.hpp index ca0a772b..179f4b73 100644 --- a/pcl_ros/include/pcl_ros/filters/project_inliers.hpp +++ b/pcl_ros/include/pcl_ros/filters/project_inliers.hpp @@ -41,9 +41,7 @@ // PCL includes #include #include -#include "pcl_ros/filters/filter.hpp" -#include - +#include "pcl_ros/pcl_node.hpp" namespace pcl_ros { @@ -53,50 +51,33 @@ namespace sync_policies = message_filters::sync_policies; * separate PointCloud. * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. * \author Radu Bogdan Rusu + * \author Antonio Brandi */ -class ProjectInliers : public Filter +class ProjectInliers : public PCLNode, + Output> { -public: - explicit ProjectInliers(const rclcpp::NodeOptions & options); - -protected: - /** \brief Call the actual filter. - * \param input the input point cloud dataset - * \param indices the input set of indices to use from \a input - * \param output the resultant filtered dataset - */ - inline void - filter( - const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, - PointCloud2 & output) override; - private: - /** \brief A pointer to the vector of model coefficients. */ - ModelCoefficientsConstPtr model_; - - /** \brief The message filter subscriber for model coefficients. */ - message_filters::Subscriber sub_model_; - - /** \brief Synchronized input, indices, and model coefficients.*/ - std::shared_ptr>> sync_input_indices_model_e_; - std::shared_ptr>> sync_input_indices_model_a_; /** \brief The PCL filter implementation used. */ pcl::ProjectInliers impl_; - void subscribe() override; - void unsubscribe() override; - - /** \brief PointCloud2 + Indices + Model data callback. */ - void - input_indices_model_callback( - const PointCloud2::ConstSharedPtr & cloud, - const PointIndicesConstPtr & indices, - const ModelCoefficientsConstPtr & model); + /** \brief Parameter callback + * \param params parameter values to set. + */ + virtual rcl_interfaces::msg::SetParametersResult onParamsChanged( + const std::vector & params) override; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + explicit ProjectInliers(const rclcpp::NodeOptions & options); + + /** \brief Calls the actual StatisticalOutlierRemoval PCL filter. + * \param input the input point cloud dataset. + * \param indices the input set of indices to use from the input dataset. + * \param model the model coefficients to use for filtering. + * \param output the resultant filtered dataset. + */ + void compute( + const PointCloud2 & input, const PointIndices & indices, + const ModelCoefficients & model, PointCloud2 & output) override; }; } // namespace pcl_ros diff --git a/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp b/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp index 5f7333df..e1d28fc1 100644 --- a/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp +++ b/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp @@ -41,7 +41,7 @@ // PCL includes #include #include -#include "pcl_ros/filters/filter.hpp" +#include "pcl_ros/pcl_node.hpp" namespace pcl_ros { @@ -49,36 +49,35 @@ namespace pcl_ros * search radius is smaller than a given K. * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. * \author Radu Bogdan Rusu + * \author Antonio Brandi */ -class RadiusOutlierRemoval : public Filter +class RadiusOutlierRemoval : public PCLNode, Output> { -protected: - /** \brief Call the actual filter. - * \param input the input point cloud dataset - * \param indices the input set of indices to use from \a input - * \param output the resultant filtered dataset - */ - inline void - filter( - const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, - PointCloud2 & output) override; - - /** \brief Parameter callback - * \param params parameter values to set - */ - rcl_interfaces::msg::SetParametersResult - config_callback(const std::vector & params); - - OnSetParametersCallbackHandle::SharedPtr callback_handle_; - private: + /** \brief Tolerance for comparing floating point parameters */ + static constexpr double PARAMETER_TOLERANCE = 1e-6; + /** \brief The PCL filter implementation used. */ pcl::RadiusOutlierRemoval impl_; -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /** \brief Parameter callback + * \param params parameter values to set. + */ + virtual rcl_interfaces::msg::SetParametersResult onParamsChanged( + const std::vector & params) override; +public: + /** \brief Constructor + * \param options A rclcpp::NodeOptions to be passed to the node. + */ explicit RadiusOutlierRemoval(const rclcpp::NodeOptions & options); + + /** \brief Calls the actual RadiusOutlierRemoval PCL filter. + * \param input the input point cloud dataset. + * \param output the resultant filtered dataset. + */ + virtual void compute(const PointCloud2 & input, PointCloud2 & output) override; + }; } // namespace pcl_ros diff --git a/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp b/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp index f8e14a2b..eb76f4ad 100644 --- a/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp +++ b/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp @@ -41,7 +41,7 @@ // PCL includes #include #include -#include "pcl_ros/filters/filter.hpp" +#include "pcl_ros/pcl_node.hpp" namespace pcl_ros { @@ -55,36 +55,34 @@ namespace pcl_ros * * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. * \author Radu Bogdan Rusu + * \author Antonio Brandi */ -class StatisticalOutlierRemoval : public Filter +class StatisticalOutlierRemoval : public PCLNode, Output> { -protected: - /** \brief Call the actual filter. - * \param input the input point cloud dataset - * \param indices the input set of indices to use from \a input - * \param output the resultant filtered dataset - */ - inline void - filter( - const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, - PointCloud2 & output) override; - - /** \brief Parameter callback - * \param params parameter values to set - */ - rcl_interfaces::msg::SetParametersResult - config_callback(const std::vector & params); - - OnSetParametersCallbackHandle::SharedPtr callback_handle_; - private: + /** \brief Tolerance for comparing floating point parameters */ + static constexpr double PARAMETER_TOLERANCE = 1e-6; + /** \brief The PCL filter implementation used. */ pcl::StatisticalOutlierRemoval impl_; -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /** \brief Parameter callback + * \param params parameter values to set. + */ + virtual rcl_interfaces::msg::SetParametersResult onParamsChanged( + const std::vector & params) override; +public: + /** \brief Constructor + * \param options A rclcpp::NodeOptions to be passed to the node. + */ explicit StatisticalOutlierRemoval(const rclcpp::NodeOptions & options); + + /** \brief Calls the actual StatisticalOutlierRemoval PCL filter. + * \param input the input point cloud dataset. + * \param output the resultant filtered dataset. + */ + void compute(const PointCloud2 & input, PointCloud2 & output) override; }; } // namespace pcl_ros diff --git a/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp b/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp index 4b51113c..332b7f86 100644 --- a/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp +++ b/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp @@ -41,42 +41,40 @@ // PCL includes #include #include -#include "pcl_ros/filters/filter.hpp" +#include "pcl_ros/pcl_node.hpp" namespace pcl_ros { /** \brief @b VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data. * \author Radu Bogdan Rusu + * \author Antonio Brandi */ -class VoxelGrid : public Filter +class VoxelGrid : public PCLNode, Output> { -protected: - /** \brief Call the actual filter. - * \param input the input point cloud dataset - * \param indices the input set of indices to use from \a input - * \param output the resultant filtered dataset - */ - inline void - filter( - const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, - PointCloud2 & output) override; - - /** \brief Parameter callback - * \param params parameter values to set - */ - rcl_interfaces::msg::SetParametersResult - config_callback(const std::vector & params); - - OnSetParametersCallbackHandle::SharedPtr callback_handle_; - private: + /** \brief Tolerance for comparing floating point parameters */ + static constexpr double PARAMETER_TOLERANCE = 1e-6; + /** \brief The PCL filter implementation used. */ pcl::VoxelGrid impl_; -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /** \brief Parameter callback + * \param params parameter values to set. + */ + virtual rcl_interfaces::msg::SetParametersResult onParamsChanged( + const std::vector & params) override; +public: + /** \brief Constructor + * \param options A rclcpp::NodeOptions to be passed to the node. + */ explicit VoxelGrid(const rclcpp::NodeOptions & options); + + /** \brief Calls the actual VoxelGrid PCL filter. + * \param input the input point cloud dataset. + * \param output the resultant filtered dataset. + */ + virtual void compute(const PointCloud2 & input, PointCloud2 & output) override; }; } // namespace pcl_ros diff --git a/pcl_ros/include/pcl_ros/pcl_node.hpp b/pcl_ros/include/pcl_ros/pcl_node.hpp index 4567bc5c..9dd4897b 100644 --- a/pcl_ros/include/pcl_ros/pcl_node.hpp +++ b/pcl_ros/include/pcl_ros/pcl_node.hpp @@ -38,6 +38,7 @@ /** \author Radu Bogdan Rusu +\author Antonio Brandi **/ @@ -63,6 +64,8 @@ #include #include +#include "pcl_ros/transforms.hpp" + // #include "pcl_ros/point_cloud.hpp" using pcl_conversions::fromPCL; @@ -72,37 +75,92 @@ namespace pcl_ros //////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////// -/** \brief @b PCLNode represents the base PCL Node class. All PCL node should inherit from - * this class. */ -template> -class PCLNode : public rclcpp::Node +using PointCloud = pcl::PointCloud; +using PointCloudPtr = PointCloud::Ptr; +using PointCloudConstPtr = PointCloud::ConstPtr; + +using Indices = pcl::PointIndices; +using IndicesPtr = Indices::Ptr; +using IndicesConstPtr = Indices::ConstPtr; + +using Coefficients = pcl::ModelCoefficients; +using CoefficientsPtr = Coefficients::Ptr; +using CoefficientsConstPtr = Coefficients::ConstPtr; + +using PointCloud2 = sensor_msgs::msg::PointCloud2; + +using PointIndices = pcl_msgs::msg::PointIndices; +using PointIndicesPtr = PointIndices::SharedPtr; +using PointIndicesConstPtr = PointIndices::ConstSharedPtr; + +using ModelCoefficients = pcl_msgs::msg::ModelCoefficients; +using ModelCoefficientsPtr = ModelCoefficients::SharedPtr; +using ModelCoefficientsConstPtr = ModelCoefficients::ConstSharedPtr; + +/** + * @brief Check whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-zero). + * @param cloud the point cloud to test. + */ +template +inline bool isValid(const std::shared_ptr & cloud) { -public: - typedef sensor_msgs::msg::PointCloud2 PointCloud2; + return cloud->width * cloud->height * cloud->point_step == cloud->data.size(); +} + +/** + * @brief Template specialization for PointIndices messages. + */ +template<> +inline bool isValid(const PointIndices::ConstSharedPtr &) +{ + return true; +} - typedef pcl::PointCloud PointCloud; - typedef PointCloud::Ptr PointCloudPtr; - typedef PointCloud::ConstPtr PointCloudConstPtr; +/** + * @brief Template specialization for ModelCoefficients messages. + */ +template<> +inline bool isValid(const ModelCoefficients::ConstSharedPtr &) +{ + return true; +} - typedef pcl_msgs::msg::PointIndices PointIndices; - typedef PointIndices::SharedPtr PointIndicesPtr; - typedef PointIndices::ConstSharedPtr PointIndicesConstPtr; +template +struct Input {}; - typedef pcl_msgs::msg::ModelCoefficients ModelCoefficients; - typedef ModelCoefficients::SharedPtr ModelCoefficientsPtr; - typedef ModelCoefficients::ConstSharedPtr ModelCoefficientsConstPtr; +template +struct Output {}; - typedef pcl::IndicesPtr IndicesPtr; - typedef pcl::IndicesConstPtr IndicesConstPtr; +template +class PCLNode; +/** \brief @b PCLNode represents the base PCL Node class. All PCL node should inherit from + * this class. */ +template +class PCLNode, Output>: public rclcpp::Node +{ + static constexpr std::size_t NInputs = sizeof...(In); + static constexpr std::size_t NOutputs = sizeof...(Out); + + using InputsTuple = std::tuple; + using OutputsTuple = std::tuple; + +public: /** \brief Empty constructor. */ - PCLNode(std::string node_name, const rclcpp::NodeOptions & options) + PCLNode( + std::string node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions(), + std::vector input_topics = {}, + std::vector output_topics = {}) : rclcpp::Node(node_name, options), - use_indices_(false), transient_local_indices_(false), - max_queue_size_(3), approximate_sync_(false), + approximate_sync_(false), + max_queue_size_(3), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { + input_topics_ = make_topics_array(input_topics, "input"); + output_topics_ = make_topics_array(output_topics, "output"); + + // Common parameters to all PCL Nodes { rcl_interfaces::msg::ParameterDescriptor desc; desc.name = "max_queue_size"; @@ -114,181 +172,460 @@ class PCLNode : public rclcpp::Node { rcl_interfaces::msg::ParameterDescriptor desc; - desc.name = "use_indices"; + desc.name = "approximate_sync"; desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - desc.description = "Only process a subset of the point cloud from an indices topic"; + desc.description = + "Match indices and point cloud messages if time stamps are approximately the same."; desc.read_only = true; - use_indices_ = declare_parameter(desc.name, use_indices_, desc); + approximate_sync_ = declare_parameter(desc.name, approximate_sync_, desc); } { rcl_interfaces::msg::ParameterDescriptor desc; - desc.name = "transient_local_indices"; - desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - desc.description = "Does indices topic use transient local documentation"; - desc.read_only = true; - transient_local_indices_ = declare_parameter(desc.name, transient_local_indices_, desc); + desc.name = "input_frame"; + desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + desc.description = + "The input TF frame the data should be transformed into before processing, " + "if input.header.frame_id is different."; + tf_input_frame_ = declare_parameter( + desc.name, rclcpp::ParameterValue(""), + desc).get(); } { rcl_interfaces::msg::ParameterDescriptor desc; - desc.name = "approximate_sync"; - desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + desc.name = "output_frame"; + desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; desc.description = - "Match indices and point cloud messages if time stamps are approximately the same."; - desc.read_only = true; - approximate_sync_ = declare_parameter(desc.name, approximate_sync_, desc); + "The output TF frame the data should be transformed into before processing, " + "if output.header.frame_id is different."; + tf_output_frame_ = declare_parameter( + desc.name, rclcpp::ParameterValue(""), + desc).get(); } + // Register param callback handler + callback_handle_ = + add_on_set_parameters_callback( + std::bind( + &PCLNode::paramsCallback, this, + std::placeholders::_1)); + + initSubscribers(); + initPublishers(std::make_index_sequence{}); + RCLCPP_DEBUG( - this->get_logger(), "PCL Node successfully created with the following parameters:\n" + get_logger(), "PCL Node successfully created with the following parameters:\n" " - approximate_sync : %s\n" - " - use_indices : %s\n" - " - transient_local_indices_ : %s\n" - " - max_queue_size : %d", + " - max_queue_size : %d\n" + " - input_frame : %s\n" + " - output_frame : %s\n", (approximate_sync_) ? "true" : "false", - (use_indices_) ? "true" : "false", - (transient_local_indices_) ? "true" : "false", - max_queue_size_); + max_queue_size_, + tf_input_frame_.c_str(), + tf_output_frame_.c_str()); } -protected: - /** \brief Set to true if point indices are used. - * - * When receiving a point cloud, if use_indices_ is false, the entire - * point cloud is processed for the given operation. If use_indices_ is - * true, then the ~indices topic is read to get the vector of point - * indices specifying the subset of the point cloud that will be used for - * the operation. In the case where use_indices_ is true, the ~input and - * ~indices topics must be synchronised in time, either exact or within a - * specified jitter. See also @ref transient_local_indices_ and approximate_sync. - **/ - bool use_indices_; - /** \brief Set to true if the indices topic has transient_local durability. - * - * If use_indices_ is true, the ~input and ~indices topics generally must - * be synchronised in time. By setting this flag to true, the most recent - * value from ~indices can be used instead of requiring a synchronised - * message. - **/ - bool transient_local_indices_; - - /** \brief The message filter subscriber for PointCloud2. */ - message_filters::Subscriber sub_input_filter_; - - /** \brief The message filter subscriber for PointIndices. */ - message_filters::Subscriber sub_indices_filter_; - - /** \brief The output PointCloud publisher. Allow each individual class that inherits from this - * to declare the Publisher with their type. - */ - std::shared_ptr pub_output_; - - /** \brief The maximum queue size (default: 3). */ - int max_queue_size_; + /** + * @brief Virtual destructor + */ + virtual ~PCLNode() = default; - /** \brief True if we use an approximate time synchronizer - * versus an exact one (false by default). - **/ +protected: + /** + * @brief Callback for parameter changes. + * Implement this function in your PCL Node. + * @param params the vector of parameters that have changed. + */ + virtual rcl_interfaces::msg::SetParametersResult onParamsChanged( + const std::vector & params) = 0; + + /** + * @brief Virtual abstract compute method. To be implemented by every child. + * @param inputs the requested inputs dataset. + * @param output the the resultant filtered outputs. + */ + virtual void compute(const In &... inputs, Out &... output) = 0; + +private: + /** + * @brief Internal mutex. + */ + std::mutex mutex_; + + /** + * @brief Pointer to the callback handle for the parameters event. + */ + OnSetParametersCallbackHandle::SharedPtr callback_handle_; + + /** + * @brief True if we use an approximate time synchronizer + * versus an exact one (false by default). + */ bool approximate_sync_; - /** \brief TF listener object. */ + /** + * @brief The maximum queue size (default: 3). + */ + int max_queue_size_; + + /** + * @brief The input TF frame the data should be transformed into, + * if input.header.frame_id is different. + */ + std::string tf_input_frame_; + + /** + * @brief The original data input TF frame. + */ + std::string tf_input_orig_frame_; + + /** + * @brief The output TF frame the data should be transformed into, + * if input.header.frame_id is different. + */ + std::string tf_output_frame_; + + /** + * @brief TF listener and Buffer objects. + */ tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - /** \brief Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-zero). - * \param cloud the point cloud to test - * \param topic_name an optional topic name (only used for printing, defaults to "input") - */ - inline bool - isValid(const PointCloud2::ConstSharedPtr & cloud, const std::string & topic_name = "input") + std::array input_topics_{}; + std::array output_topics_{}; + + typename rclcpp::Subscription::type>::SharedPtr sub_single_; + + std::tuple...> mf_subs_; + std::shared_ptr sync_handle_; + + std::tuple::SharedPtr...> publishers_; + + /** + * @brief Lazy transport subscribe/unsubscribe routine. + * It is optional for backward compatibility. + */ + void initSubscribers() { - if (cloud->width * cloud->height * cloud->point_step != cloud->data.size()) { - RCLCPP_WARN( - this->get_logger(), - "Invalid PointCloud (data = %zu, width = %d, height = %d, step = %d) " - "with stamp %d.%09d, and frame %s on topic %s received!", - cloud->data.size(), cloud->width, cloud->height, cloud->point_step, - cloud->header.stamp.sec, cloud->header.stamp.nanosec, - cloud->header.frame_id.c_str(), topic_name.c_str()); - - return false; + if constexpr (NInputs == 1) { + // Subscribe in an old fashion to input only (no filters) + using First = typename std::tuple_element<0, InputsTuple>::type; + auto qos = rclcpp::SensorDataQoS().keep_last(max_queue_size_); + rmw_qos_profile_t rmw_qos = qos.get_rmw_qos_profile(); + + sub_single_ = create_subscription( + input_topics_[0], qos, + [this](const typename First::ConstSharedPtr msg) { + // Forward to unified callback + inputCallback(msg); + }); + } else { + // If multiple inputs are given, we synchronize them with message filters + auto sensor_qos = rclcpp::SensorDataQoS().keep_last(max_queue_size_); + rmw_qos_profile_t rmw_qos = sensor_qos.get_rmw_qos_profile(); + + // Initialize MF subscribers + initMFSubscribers(rmw_qos, std::make_index_sequence{}); + + // Build synchronizer over all MF subscribers + if (approximate_sync_) { + std::apply( + [this](auto &... subs) { + using Policy = message_filters::sync_policies::ApproximateTime; + using Sync = message_filters::Synchronizer; + auto sync = std::make_shared(Policy(max_queue_size_), subs ...); + sync->registerCallback(&PCLNode::inputCallback, this); + sync_handle_ = std::move(sync); + }, mf_subs_); + } else { + std::apply( + [this](auto &... subs) { + using Policy = message_filters::sync_policies::ExactTime; + using Sync = message_filters::Synchronizer; + auto sync = std::make_shared(Policy(max_queue_size_), subs ...); + sync->registerCallback(&PCLNode::inputCallback, this); + sync_handle_ = std::move(sync); + }, mf_subs_); + } } - return true; } - /** \brief Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-zero). - * \param cloud the point cloud to test - * \param topic_name an optional topic name (only used for printing, defaults to "input") - */ - inline bool - isValid(const PointCloudConstPtr & cloud, const std::string & topic_name = "input") + /** + * @brief Unsubscribe from the input and indices topics. + */ + void unsubscribe() { - if (cloud->width * cloud->height != cloud->points.size()) { - RCLCPP_WARN( - this->get_logger(), - "Invalid PointCloud (points = %zu, width = %d, height = %d) " - "with stamp %d.%09d, and frame %s on topic %s received!", - cloud->points.size(), cloud->width, cloud->height, - fromPCL(cloud->header).stamp.sec, fromPCL(cloud->header).stamp.nanosec, - cloud->header.frame_id.c_str(), topic_name.c_str()); - - return false; + if constexpr (NInputs == 1) { + sub_single_.reset(); + } else { + // Disconnect synchronizers + sync_handle_.reset(); + // Unsubscribe each message_filters subscriber + unsubscribeMF(std::make_index_sequence{}); } - return true; } - /** \brief Test whether a given PointIndices message is "valid" (i.e., has values). - * \param indices the point indices message to test - * \param topic_name an optional topic name (only used for printing, defaults to "indices") - */ - inline bool - isValid( - const PointIndicesConstPtr & /*indices*/, - const std::string & /*topic_name*/ = "indices") + /** + * @brief The callback for the input data. This is where the action happens. + * @param inputs the input messages. + */ + void inputCallback( + const typename In::ConstSharedPtr &... inputs) { - /*if (indices->indices.empty ()) + std::lock_guard lock(mutex_); + if (!(isValid(inputs) && ...)) { + RCLCPP_ERROR(get_logger(), "One or more invalid inputs received, skipping this callback..."); + return; + } + + // Check whether the user has given a different input TF frame + auto in_ptr_tuple = std::forward_as_tuple(inputs ...); + + InputsTuple transformed_inputs{}; + if (!transformMessages( + std::make_index_sequence{}, transformed_inputs, in_ptr_tuple, tf_input_frame_)) { - RCLCPP_WARN( - this->get_logger(), "Empty indices (values = %zu) with stamp %d.%09d, " - "and frame %s on topic %s received!", - indices->indices.size(), indices->header.stamp.sec, indices->header.stamp.nanosec, - indices->header.frame_id.c_str(), topic_name.c_str()); - //return (true); // looks like it should be false - return false; - }*/ - return true; + // transformMessages already logged the reason + return; + } + + // Prepare output messages + OutputsTuple outputs{}; + + // Call derived compute with const refs to messages and refs to outputs + std::apply( + [&](const In &... t_in) { + std::apply( + [&](Out &... outs) { + compute(t_in ..., outs ...); + }, + outputs); + }, + transformed_inputs); + + // Check whether the user has given a different input TF frame + OutputsTuple transformed_outputs{}; + if (!transformMessages(std::make_index_sequence{}, transformed_outputs, outputs, tf_output_frame_)) + { + // transformMessages already logged the reason + return; + } + + // Publish all outputs + publishOutputs(transformed_outputs, std::make_index_sequence{}); } - /** \brief Test whether a given ModelCoefficients message is "valid" (i.e., has values). - * \param model the model coefficients to test - * \param topic_name an optional topic name (only used for printing, defaults to "model") - */ - inline bool - isValid( - const ModelCoefficientsConstPtr & /*model*/, - const std::string & /*topic_name*/ = "model") + /** + * @brief Callback function for parameter updates. + * @param params parameter values to set. + */ + rcl_interfaces::msg::SetParametersResult paramsCallback( + const std::vector & params) { - /*if (model->values.empty ()) - { - RCLCPP_WARN( - this->get_logger(), "Empty model (values = %zu) with stamp %d.%09d, " - "and frame %s on topic %s received!", - model->values.size(), model->header.stamp.sec, model->header.stamp.nanosec, - model->header.frame_id.c_str(), topic_name.c_str()); - return (false); - }*/ - return true; + std::lock_guard lock(mutex_); + for (const rclcpp::Parameter & param : params) { + if (param.get_name() == "input_frame") { + if (tf_input_frame_ != param.as_string()) { + tf_input_frame_ = param.as_string(); + RCLCPP_DEBUG(get_logger(), "Setting the input frame to: %s.", tf_input_frame_.c_str()); + } + } + if (param.get_name() == "output_frame") { + if (tf_output_frame_ != param.as_string()) { + tf_output_frame_ = param.as_string(); + RCLCPP_DEBUG(get_logger(), "Setting the output frame to: %s.", tf_output_frame_.c_str()); + } + } + } + return onParamsChanged(params); } - /** \brief Lazy transport subscribe/unsubscribe routine. - * It is optional for backward compatibility. - **/ - virtual void subscribe() {} - virtual void unsubscribe() {} + /** + * @brief Create an array of topic names, given a vector of names and a prefix. + * If the vector is empty, the names will be auto-generated using the prefix. + * If the vector has less names than required, the remaining names will be + * auto-generated using the prefix, and a warning will be printed. + * If the vector has more names than required, the extra names will be ignored. + * @param v the vector of topic names. + * @param prefix the prefix to use for auto-generated names. + * @return an array of topic names of size N. + */ + template + static std::array make_topics_array( + const std::vector & v, const std::string & prefix) + { + std::array arr{}; + if (v.empty()) { + for (std::size_t i = 0; i < N; ++i) {arr[i] = prefix + std::to_string(i);} + } else { + if (v.size() != N) { + // best effort: fill provided names, auto-fill remaining, warn + RCLCPP_WARN( + rclcpp::get_logger("PCLNode"), + "Expected %zu %s topics, got %zu. Filling defaults for the rest.", + N, prefix.c_str(), v.size()); + } + std::size_t i = 0; + for (; i < std::min(N, v.size()); ++i) {arr[i] = v[i];} + for (; i < N; ++i) {arr[i] = prefix + std::to_string(i);} + } + return arr; + } -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /** + * @brief Initialize message_filters subscribers for each input type In[I]. + * @param rmw_qos the RMW QoS profile to use for subscribing. + * @param index_sequence a compile-time index sequence for the input types. + */ + template + void initMFSubscribers(const rmw_qos_profile_t & rmw_qos, std::index_sequence) + { + // For each input type In[I], subscribe with message_filters + ( ( std::get(mf_subs_).subscribe(this, input_topics_[I], rmw_qos) ), ... ); + } + + /** + * @brief Initialize publishers for each output type Out[J]. + * @param index_sequence a compile-time index sequence for the output types. + */ + template + void initPublishers(std::index_sequence) + { + // For each output type Out[J], create a publisher + ( ( std::get(publishers_) = + create_publisher::type>( + output_topics_[J], rclcpp::QoS(max_queue_size_)) ), ... ); + } + + /** + * @brief Publish each output message to its corresponding publisher. + * @param outputs the tuple of output messages to publish. + * @param index_sequence a compile-time index sequence for the output types. + */ + template + void publishOutputs(OutputsTuple & outputs, std::index_sequence) + { + // Publish each output message to its corresponding publisher + ( ( std::get(publishers_)->publish(std::get(outputs)) ), ... ); + } + + /** + * @brief Unsubscribe each message_filters subscriber. + * @param index_sequence a compile-time index sequence for the input types. + */ + template + void unsubscribeMF(std::index_sequence) + { + ( ( std::get(mf_subs_).unsubscribe() ), ... ); + } + + template + struct is_const_shared_ptr : std::false_type {}; + + template + struct is_const_shared_ptr>: std::true_type {}; + + template + static constexpr bool is_const_shared_ptr_v = is_const_shared_ptr::value; + + template + bool transformMessages( + std::index_sequence, + DestTuple & dest_tuple, + const SourceTuple & src_tuple, + const std::string & target_frame) + { + bool ok = true; + + // No target frame specified + if (target_frame.empty()) { + (void)std::initializer_list{ + ( [&] { + // Get the source element (either a smart ptr or a value) + const auto & src_element = std::get(src_tuple); + using SrcElementT = std::remove_cv_t>; + + if constexpr (is_const_shared_ptr_v) { + if (!src_element) { + RCLCPP_ERROR( + get_logger(), "Null at index %zu, skipping copy.", + static_cast(Is)); + ok = false; + return; + } + std::get(dest_tuple) = *src_element; + } else { + std::get(dest_tuple) = src_element; + } + }(), + 0 )... + }; + return ok; + } + + // Transform to target frame + (void)std::initializer_list{ + ( [&] { + // Get the *destination* message type (e.g., PointCloud2) + using DestMsgT = std::tuple_element_t; + // Get the *source* element (smart ptr or value) + const auto & src_element = std::get(src_tuple); + using SrcElementT = std::remove_cv_t>; + + auto get_msg_ptr = [&]() -> const DestMsgT * { + if constexpr (is_const_shared_ptr_v) { + if (!src_element) { + RCLCPP_ERROR( + get_logger(), "Null at index %zu.", + static_cast(Is)); + ok = false; + return nullptr; + } + return &(*src_element); + } else { + return &(static_cast(src_element)); + } + }; + + const DestMsgT * msg_in_ptr = get_msg_ptr(); + if (!msg_in_ptr) { + return; + } + + const DestMsgT & msg_in = *msg_in_ptr; + auto & msg_out = std::get(dest_tuple); + + if constexpr (std::is_same_v) { + // This is a PointCloud2, transform it. + if (msg_in.header.frame_id != target_frame) { + if (!pcl_ros::transformPointCloud(target_frame, msg_in, msg_out, tf_buffer_)) { + RCLCPP_ERROR( + get_logger(), + "TF transform failed for %zu: '%s' -> '%s'.", + static_cast(Is), + msg_in.header.frame_id.c_str(), target_frame.c_str()); + ok = false; + return; + } + // Preserve stamp, set new frame + msg_out.header.stamp = msg_in.header.stamp; + msg_out.header.frame_id = target_frame; + } else { + // Already in target frame, just copy. + msg_out = msg_in; + } + } else { + // Not a PointCloud2, just copy it through. + msg_out = msg_in; + } + }(), + 0 )... + }; + + return ok; + } }; } // namespace pcl_ros diff --git a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp index ca4ca34f..d8abb350 100644 --- a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp +++ b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp @@ -38,264 +38,170 @@ #ifndef PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_ #define PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_ -#include -#include #include -#include -#include "pcl_ros/pcl_nodelet.hpp" -#include "pcl_ros/SACSegmentationConfig.hpp" -#include "pcl_ros/SACSegmentationFromNormalsConfig.hpp" +#include + +#include "pcl_ros/pcl_node.hpp" +#include + +// PCL includes +#include +#include "pcl_ros/pcl_node.hpp" namespace pcl_ros { -namespace sync_policies = message_filters::sync_policies; - //////////////////////////////////////////////////////////////////////////////////////////// -/** \brief @b SACSegmentation represents the Nodelet segmentation class for Sample Consensus - * methods and models, in the sense that it just creates a Nodelet wrapper for generic-purpose +/** \brief @b SACSegmentation represents a segmentation class for Sample Consensus + * methods and models, in the sense that it just creates a wrapper for generic-purpose * SAC-based segmentation. * \author Radu Bogdan Rusu + * \author Antonio Brandi */ -class SACSegmentation : public PCLNodelet +class SACSegmentation : public PCLNode, Output> { - typedef pcl::PointCloud PointCloud; - typedef boost::shared_ptr PointCloudPtr; - typedef boost::shared_ptr PointCloudConstPtr; - -public: - /** \brief Constructor. */ - SACSegmentation() - : min_inliers_(0) {} - - /** \brief Set the input TF frame the data should be transformed into before processing, - * if input.header.frame_id is different. - * \param tf_frame the TF frame the input PointCloud should be transformed into before processing - */ - inline void setInputTFframe(std::string tf_frame) {tf_input_frame_ = tf_frame;} - - /** \brief Get the TF frame the input PointCloud should be transformed into before processing. */ - inline std::string getInputTFframe() {return tf_input_frame_;} - - /** \brief Set the output TF frame the data should be transformed into after processing. - * \param tf_frame the TF frame the PointCloud should be transformed into after processing - */ - inline void setOutputTFframe(std::string tf_frame) {tf_output_frame_ = tf_frame;} - - /** \brief Get the TF frame the PointCloud should be transformed into after processing. */ - inline std::string getOutputTFframe() {return tf_output_frame_;} - -protected: - // The minimum number of inliers a model must have in order to be considered valid. - int min_inliers_; - - // ROS nodelet attributes - /** \brief The output PointIndices publisher. */ - ros::Publisher pub_indices_; - - /** \brief The output ModelCoefficients publisher. */ - ros::Publisher pub_model_; - - /** \brief The input PointCloud subscriber. */ - ros::Subscriber sub_input_; - - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr> srv_; - - /** \brief The input TF frame the data should be transformed into, - * if input.header.frame_id is different. - */ - std::string tf_input_frame_; - - /** \brief The original data input TF frame. */ - std::string tf_input_orig_frame_; - - /** \brief The output TF frame the data should be transformed into, - * if input.header.frame_id is different. - */ - std::string tf_output_frame_; - - /** \brief Null passthrough filter, used for pushing empty elements in the - * synchronizer */ - message_filters::PassThrough nf_pi_; - - /** \brief Nodelet initialization routine. */ - virtual void onInit(); - - /** \brief LazyNodelet connection routine. */ - virtual void subscribe(); - virtual void unsubscribe(); - - /** \brief Dynamic reconfigure callback - * \param config the config object - * \param level the dynamic reconfigure level - */ - void config_callback(SACSegmentationConfig & config, uint32_t level); - - /** \brief Input point cloud callback. Used when \a use_indices is set. - * \param cloud the pointer to the input point cloud - * \param indices the pointer to the input point cloud indices - */ - void input_indices_callback( - const PointCloudConstPtr & cloud, - const PointIndicesConstPtr & indices); - - /** \brief Pointer to a set of indices stored internally. - * (used when \a latched_indices_ is set). - */ - PointIndices indices_; - - /** \brief Indices callback. Used when \a latched_indices_ is set. - * \param indices the pointer to the input point cloud indices - */ - inline void - indices_callback(const PointIndicesConstPtr & indices) - { - indices_ = *indices; - } - - /** \brief Input callback. Used when \a latched_indices_ is set. - * \param input the pointer to the input point cloud - */ - inline void - input_callback(const PointCloudConstPtr & input) - { - indices_.header = fromPCL(input->header); - PointIndicesConstPtr indices; - indices.reset(new PointIndices(indices_)); - nf_pi_.add(indices); - } - private: - /** \brief Internal mutex. */ - boost::mutex mutex_; + /** \brief Tolerance for comparing floating point parameters */ + static constexpr double PARAMETER_TOLERANCE = 1e-6; /** \brief The PCL implementation used. */ pcl::SACSegmentation impl_; - /** \brief Synchronized input, and indices.*/ - boost::shared_ptr>> sync_input_indices_e_; - boost::shared_ptr>> sync_input_indices_a_; + /** \brief Parameter callback + * \param params parameter values to set. + */ + virtual rcl_interfaces::msg::SetParametersResult onParamsChanged( + const std::vector & params) override; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /** \brief Constructor + * \param options A rclcpp::NodeOptions to be passed to the node. + */ + explicit SACSegmentation(const rclcpp::NodeOptions & options); + + /** \brief Call the actual filter. + * \param input the input point cloud dataset + * \param indices the output indices that contain the inliers found + * \param model the resultant model coefficients + */ + virtual void compute( + const PointCloud2 & input, PointIndices & indices, + ModelCoefficients & model) override; }; //////////////////////////////////////////////////////////////////////////////////////////// /** \brief @b SACSegmentationFromNormals represents the PCL nodelet segmentation class for * Sample Consensus methods and models that require the use of surface normals for estimation. */ -class SACSegmentationFromNormals : public SACSegmentation -{ - typedef pcl::PointCloud PointCloud; - typedef boost::shared_ptr PointCloudPtr; - typedef boost::shared_ptr PointCloudConstPtr; - - typedef pcl::PointCloud PointCloudN; - typedef boost::shared_ptr PointCloudNPtr; - typedef boost::shared_ptr PointCloudNConstPtr; - -public: - /** \brief Set the input TF frame the data should be transformed into before processing, - * if input.header.frame_id is different. - * \param tf_frame the TF frame the input PointCloud should be transformed into before processing - */ - inline void setInputTFframe(std::string tf_frame) {tf_input_frame_ = tf_frame;} - - /** \brief Get the TF frame the input PointCloud should be transformed into before processing. */ - inline std::string getInputTFframe() {return tf_input_frame_;} - - /** \brief Set the output TF frame the data should be transformed into after processing. - * \param tf_frame the TF frame the PointCloud should be transformed into after processing - */ - inline void setOutputTFframe(std::string tf_frame) {tf_output_frame_ = tf_frame;} - - /** \brief Get the TF frame the PointCloud should be transformed into after processing. */ - inline std::string getOutputTFframe() {return tf_output_frame_;} - -protected: - // ROS nodelet attributes - /** \brief The normals PointCloud subscriber filter. */ - message_filters::Subscriber sub_normals_filter_; - - /** \brief The input PointCloud subscriber. */ - ros::Subscriber sub_axis_; - - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr> srv_; - - /** \brief Input point cloud callback. - * Because we want to use the same synchronizer object, we push back - * empty elements with the same timestamp. - */ - inline void - input_callback(const PointCloudConstPtr & cloud) - { - PointIndices indices; - indices.header.stamp = fromPCL(cloud->header).stamp; - nf_.add(boost::make_shared(indices)); - } - - /** \brief Null passthrough filter, used for pushing empty elements in the - * synchronizer */ - message_filters::PassThrough nf_; - - /** \brief The input TF frame the data should be transformed into, - * if input.header.frame_id is different. - */ - std::string tf_input_frame_; - /** \brief The original data input TF frame. */ - std::string tf_input_orig_frame_; - /** \brief The output TF frame the data should be transformed into, - * if input.header.frame_id is different. - */ - std::string tf_output_frame_; - - /** \brief Nodelet initialization routine. */ - virtual void onInit(); - - /** \brief LazyNodelet connection routine. */ - virtual void subscribe(); - virtual void unsubscribe(); - - /** \brief Model callback - * \param model the sample consensus model found - */ - void axis_callback(const pcl_msgs::ModelCoefficientsConstPtr & model); - - /** \brief Dynamic reconfigure callback - * \param config the config object - * \param level the dynamic reconfigure level - */ - void config_callback(SACSegmentationFromNormalsConfig & config, uint32_t level); - - /** \brief Input point cloud callback. - * \param cloud the pointer to the input point cloud - * \param cloud_normals the pointer to the input point cloud normals - * \param indices the pointer to the input point cloud indices - */ - void input_normals_indices_callback( - const PointCloudConstPtr & cloud, - const PointCloudNConstPtr & cloud_normals, - const PointIndicesConstPtr & indices); - -private: - /** \brief Internal mutex. */ - boost::mutex mutex_; - - /** \brief The PCL implementation used. */ - pcl::SACSegmentationFromNormals impl_; - - /** \brief Synchronized input, normals, and indices.*/ - boost::shared_ptr>> sync_input_normals_indices_a_; - boost::shared_ptr>> sync_input_normals_indices_e_; - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; +// class SACSegmentationFromNormals : public SACSegmentation +// { +// typedef pcl::PointCloud PointCloud; +// typedef boost::shared_ptr PointCloudPtr; +// typedef boost::shared_ptr PointCloudConstPtr; + +// typedef pcl::PointCloud PointCloudN; +// typedef boost::shared_ptr PointCloudNPtr; +// typedef boost::shared_ptr PointCloudNConstPtr; + +// public: +// /** \brief Set the input TF frame the data should be transformed into before processing, +// * if input.header.frame_id is different. +// * \param tf_frame the TF frame the input PointCloud should be transformed into before processing +// */ +// inline void setInputTFframe(std::string tf_frame) {tf_input_frame_ = tf_frame;} + +// /** \brief Get the TF frame the input PointCloud should be transformed into before processing. */ +// inline std::string getInputTFframe() {return tf_input_frame_;} + +// /** \brief Set the output TF frame the data should be transformed into after processing. +// * \param tf_frame the TF frame the PointCloud should be transformed into after processing +// */ +// inline void setOutputTFframe(std::string tf_frame) {tf_output_frame_ = tf_frame;} + +// /** \brief Get the TF frame the PointCloud should be transformed into after processing. */ +// inline std::string getOutputTFframe() {return tf_output_frame_;} + +// protected: +// // ROS nodelet attributes +// /** \brief The normals PointCloud subscriber filter. */ +// message_filters::Subscriber sub_normals_filter_; + +// /** \brief The input PointCloud subscriber. */ +// ros::Subscriber sub_axis_; + +// /** \brief Pointer to a dynamic reconfigure service. */ +// boost::shared_ptr> srv_; + +// /** \brief Input point cloud callback. +// * Because we want to use the same synchronizer object, we push back +// * empty elements with the same timestamp. +// */ +// inline void +// input_callback(const PointCloudConstPtr & cloud) +// { +// PointIndices indices; +// indices.header.stamp = fromPCL(cloud->header).stamp; +// nf_.add(boost::make_shared(indices)); +// } + +// /** \brief Null passthrough filter, used for pushing empty elements in the +// * synchronizer */ +// message_filters::PassThrough nf_; + +// /** \brief The input TF frame the data should be transformed into, +// * if input.header.frame_id is different. +// */ +// std::string tf_input_frame_; +// /** \brief The original data input TF frame. */ +// std::string tf_input_orig_frame_; +// /** \brief The output TF frame the data should be transformed into, +// * if input.header.frame_id is different. +// */ +// std::string tf_output_frame_; + +// /** \brief Nodelet initialization routine. */ +// virtual void onInit(); + +// /** \brief LazyNodelet connection routine. */ +// virtual void subscribe(); +// virtual void unsubscribe(); + +// /** \brief Model callback +// * \param model the sample consensus model found +// */ +// void axis_callback(const pcl_msgs::ModelCoefficientsConstPtr & model); + +// /** \brief Dynamic reconfigure callback +// * \param config the config object +// * \param level the dynamic reconfigure level +// */ +// void config_callback(SACSegmentationFromNormalsConfig & config, uint32_t level); + +// /** \brief Input point cloud callback. +// * \param cloud the pointer to the input point cloud +// * \param cloud_normals the pointer to the input point cloud normals +// * \param indices the pointer to the input point cloud indices +// */ +// void input_normals_indices_callback( +// const PointCloudConstPtr & cloud, +// const PointCloudNConstPtr & cloud_normals, +// const PointIndicesConstPtr & indices); + +// private: +// /** \brief Internal mutex. */ +// boost::mutex mutex_; + +// /** \brief The PCL implementation used. */ +// pcl::SACSegmentationFromNormals impl_; + +// /** \brief Synchronized input, normals, and indices.*/ +// boost::shared_ptr>> sync_input_normals_indices_a_; +// boost::shared_ptr>> sync_input_normals_indices_e_; + +// public: +// EIGEN_MAKE_ALIGNED_OPERATOR_NEW +// }; } // namespace pcl_ros #endif // PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_ diff --git a/pcl_ros/src/pcl_ros/filters/crop_box.cpp b/pcl_ros/src/pcl_ros/filters/crop_box.cpp index 28ba5280..d12e2ccd 100644 --- a/pcl_ros/src/pcl_ros/filters/crop_box.cpp +++ b/pcl_ros/src/pcl_ros/filters/crop_box.cpp @@ -38,12 +38,12 @@ #include "pcl_ros/filters/crop_box.hpp" -pcl_ros::CropBox::CropBox(const rclcpp::NodeOptions & options) -: Filter("CropBoxNode", options) +namespace pcl_ros +{ +CropBox::CropBox(const rclcpp::NodeOptions & options) +: PCLNode("CropBoxNode", options, std::vector{"input"}, + std::vector{"output", "~/crop_box_marker"}) { - // This both declares and initializes the input and output frames - use_frame_params(); - rcl_interfaces::msg::ParameterDescriptor min_x_desc; min_x_desc.name = "min_x"; min_x_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; @@ -136,58 +136,33 @@ pcl_ros::CropBox::CropBox(const rclcpp::NodeOptions & options) negative_desc.description = "Set whether the inliers should be returned (true) or the outliers (false)."; declare_parameter(negative_desc.name, rclcpp::ParameterValue(false), negative_desc); - - const std::vector param_names { - min_x_desc.name, - max_x_desc.name, - min_y_desc.name, - max_y_desc.name, - min_z_desc.name, - max_z_desc.name, - keep_organized_desc.name, - negative_desc.name, - }; - - callback_handle_ = - add_on_set_parameters_callback( - std::bind( - &CropBox::config_callback, this, - std::placeholders::_1)); - - config_callback(get_parameters(param_names)); - - createPublishers(); } -void -pcl_ros::CropBox::filter( - const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, - PointCloud2 & output) +void CropBox::compute( + const PointCloud2 & input, PointCloud2 & output, + visualization_msgs::msg::Marker & marker) { - std::lock_guard lock(mutex_); + if(input.data.empty()) { + output = input; + return; + } pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); - pcl_conversions::toPCL(*(input), *(pcl_input)); + pcl_conversions::toPCL(input, *(pcl_input)); impl_.setInputCloud(pcl_input); - impl_.setIndices(indices); pcl::PCLPointCloud2 pcl_output; impl_.filter(pcl_output); pcl_conversions::moveFromPCL(pcl_output, output); // Publish the crop box as a cube marker for visualization purposes - if (crop_box_marker_publisher_->get_subscription_count() > 0) { - crop_box_marker_msg_.header.frame_id = - tf_input_frame_.empty() ? input->header.frame_id : tf_input_frame_; - crop_box_marker_msg_.header.stamp = input->header.stamp; - crop_box_marker_publisher_->publish(crop_box_marker_msg_); - } + crop_box_marker_msg_.header.frame_id = input.header.frame_id; + crop_box_marker_msg_.header.stamp = input.header.stamp; + marker = crop_box_marker_msg_; } ////////////////////////////////////////////////////////////////////////////////////////////// -rcl_interfaces::msg::SetParametersResult -pcl_ros::CropBox::config_callback(const std::vector & params) +rcl_interfaces::msg::SetParametersResult CropBox::onParamsChanged( + const std::vector & params) { - std::lock_guard lock(mutex_); - Eigen::Vector4f min_point, max_point; min_point = impl_.getMin(); max_point = impl_.getMax(); @@ -252,7 +227,7 @@ pcl_ros::CropBox::config_callback(const std::vector & params) crop_box_marker_updated = true; } if (crop_box_marker_updated) { - update_marker_msg(); + updateMarkerMsg(); } // Range constraints are enforced by rclcpp::Parameter. @@ -261,14 +236,7 @@ pcl_ros::CropBox::config_callback(const std::vector & params) return result; } -void pcl_ros::CropBox::createPublishers() -{ - pcl_ros::Filter::createPublishers(); - crop_box_marker_publisher_ = create_publisher( - "~/crop_box_marker", max_queue_size_); -} - -void pcl_ros::CropBox::update_marker_msg() +void CropBox::updateMarkerMsg() { auto min_point = impl_.getMin(); auto max_point = impl_.getMax(); @@ -288,6 +256,7 @@ void pcl_ros::CropBox::update_marker_msg() crop_box_marker_msg_.scale.y = size.y(); crop_box_marker_msg_.scale.z = size.z(); } +} // namespace pcl_ros #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::CropBox) diff --git a/pcl_ros/src/pcl_ros/filters/extract_indices.cpp b/pcl_ros/src/pcl_ros/filters/extract_indices.cpp index 9ba366ad..20027090 100644 --- a/pcl_ros/src/pcl_ros/filters/extract_indices.cpp +++ b/pcl_ros/src/pcl_ros/filters/extract_indices.cpp @@ -37,50 +37,39 @@ #include "pcl_ros/filters/extract_indices.hpp" -pcl_ros::ExtractIndices::ExtractIndices(const rclcpp::NodeOptions & options) -: Filter("ExtractIndicesNode", options) +namespace pcl_ros +{ +ExtractIndices::ExtractIndices(const rclcpp::NodeOptions & options) +: PCLNode("ExtractIndicesNode", options, std::vector{"input", "indices"}, + std::vector{"output"}) { rcl_interfaces::msg::ParameterDescriptor neg_desc; neg_desc.name = "negative"; neg_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; neg_desc.description = "Extract indices or the negative (all-indices)"; declare_parameter(neg_desc.name, rclcpp::ParameterValue(false), neg_desc); - - // Validate initial values using same callback - callback_handle_ = - add_on_set_parameters_callback( - std::bind(&ExtractIndices::config_callback, this, std::placeholders::_1)); - - std::vector param_names{neg_desc.name}; - auto result = config_callback(get_parameters(param_names)); - if (!result.successful) { - throw std::runtime_error(result.reason); - } - - createPublishers(); } -void -pcl_ros::ExtractIndices::filter( - const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, +void ExtractIndices::compute( + const PointCloud2 & input, const PointIndices & indices, PointCloud2 & output) { - std::lock_guard lock(mutex_); pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); - pcl_conversions::toPCL(*(input), *(pcl_input)); + pcl_conversions::toPCL(input, *(pcl_input)); impl_.setInputCloud(pcl_input); - impl_.setIndices(indices); + + IndicesPtr pcl_indices (new pcl::PointIndices); + pcl_indices->indices = indices.indices; + impl_.setIndices(pcl_indices); pcl::PCLPointCloud2 pcl_output; impl_.filter(pcl_output); pcl_conversions::moveFromPCL(pcl_output, output); } ////////////////////////////////////////////////////////////////////////////////////////////// -rcl_interfaces::msg::SetParametersResult -pcl_ros::ExtractIndices::config_callback(const std::vector & params) +rcl_interfaces::msg::SetParametersResult ExtractIndices::onParamsChanged( + const std::vector & params) { - std::lock_guard lock(mutex_); - for (const rclcpp::Parameter & param : params) { if (param.get_name() == "negative") { // Check the current value for the negative flag @@ -97,6 +86,7 @@ pcl_ros::ExtractIndices::config_callback(const std::vector & result.successful = true; return result; } +} // namespace pcl_ros #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::ExtractIndices) diff --git a/pcl_ros/src/pcl_ros/filters/filter.cpp b/pcl_ros/src/pcl_ros/filters/filter.cpp deleted file mode 100644 index 8d5f262c..00000000 --- a/pcl_ros/src/pcl_ros/filters/filter.cpp +++ /dev/null @@ -1,370 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2009, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * $Id: filter.cpp 35876 2011-02-09 01:04:36Z rusu $ - * - */ - -#include "pcl_ros/filters/filter.hpp" -#include -#include "pcl_ros/transforms.hpp" - -/*//#include -//#include -*/ - -/*//typedef pcl::PixelGrid PixelGrid; -//typedef pcl::FilterDimension FilterDimension; -*/ - -// Include the implementations instead of compiling them separately to speed up compile time -// #include "extract_indices.cpp" -// #include "passthrough.cpp" -// #include "project_inliers.cpp" -// #include "radius_outlier_removal.cpp" -// #include "statistical_outlier_removal.cpp" -// #include "voxel_grid.cpp" - -/*//PLUGINLIB_EXPORT_CLASS(PixelGrid,nodelet::Nodelet); -//PLUGINLIB_EXPORT_CLASS(FilterDimension,nodelet::Nodelet); -*/ - -/////////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::Filter::computePublish( - const PointCloud2::ConstSharedPtr & input, - const IndicesPtr & indices) -{ - PointCloud2 output; - // Call the virtual method in the child - if (!input->data.empty()) { - filter(input, indices, output); - } else { - RCLCPP_DEBUG(this->get_logger(), "Received empty input point cloud"); - output = *input; - } - - PointCloud2::UniquePtr cloud_tf(new PointCloud2(output)); // set the output by default - // Check whether the user has given a different output TF frame - if (!tf_output_frame_.empty() && output.header.frame_id != tf_output_frame_) { - RCLCPP_DEBUG( - this->get_logger(), "Transforming output dataset from %s to %s.", - output.header.frame_id.c_str(), tf_output_frame_.c_str()); - // Convert the cloud into the different frame - PointCloud2 cloud_transformed; - if (!pcl_ros::transformPointCloud(tf_output_frame_, output, cloud_transformed, tf_buffer_)) { - RCLCPP_ERROR( - this->get_logger(), "Error converting output dataset from %s to %s.", - output.header.frame_id.c_str(), tf_output_frame_.c_str()); - return; - } - cloud_tf.reset(new PointCloud2(cloud_transformed)); - } - if (tf_output_frame_.empty() && output.header.frame_id != tf_input_orig_frame_) { - // no tf_output_frame given, transform the dataset to its original frame - RCLCPP_DEBUG( - this->get_logger(), "Transforming output dataset from %s back to %s.", - output.header.frame_id.c_str(), tf_input_orig_frame_.c_str()); - // Convert the cloud into the different frame - PointCloud2 cloud_transformed; - if (!pcl_ros::transformPointCloud( - tf_input_orig_frame_, output, cloud_transformed, - tf_buffer_)) - { - RCLCPP_ERROR( - this->get_logger(), "Error converting output dataset from %s back to %s.", - output.header.frame_id.c_str(), tf_input_orig_frame_.c_str()); - return; - } - cloud_tf.reset(new PointCloud2(cloud_transformed)); - } - - // Copy timestamp to keep it - cloud_tf->header.stamp = input->header.stamp; - - // Publish the unique ptr - pub_output_->publish(move(cloud_tf)); -} - -////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::Filter::subscribe() -{ - // If we're supposed to look for PointIndices (indices) - if (use_indices_) { - // Subscribe to the input using a filter - auto sensor_qos_profile = rclcpp::SensorDataQoS().keep_last(max_queue_size_); - sub_input_filter_.subscribe(this, "input", sensor_qos_profile); - sub_indices_filter_.subscribe(this, "indices", sensor_qos_profile); - - if (approximate_sync_) { - sync_input_indices_a_ = - std::make_shared>>(max_queue_size_); - sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); - sync_input_indices_a_->registerCallback( - std::bind( - &Filter::input_indices_callback, this, - std::placeholders::_1, std::placeholders::_2)); - } else { - sync_input_indices_e_ = - std::make_shared>>(max_queue_size_); - sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); - sync_input_indices_e_->registerCallback( - std::bind( - &Filter::input_indices_callback, this, - std::placeholders::_1, std::placeholders::_2)); - } - } else { - // Workaround for a callback with custom arguments ros2/rclcpp#766 - std::function callback = - std::bind(&Filter::input_indices_callback, this, std::placeholders::_1, nullptr); - - // Subscribe in an old fashion to input only (no filters) - sub_input_ = - this->create_subscription( - "input", rclcpp::SensorDataQoS().keep_last(max_queue_size_), - callback); - } -} - -////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::Filter::unsubscribe() -{ - if (use_indices_) { - sub_input_filter_.unsubscribe(); - sub_indices_filter_.unsubscribe(); - } else { - sub_input_.reset(); - } -} - -////////////////////////////////////////////////////////////////////////////////////////////// -pcl_ros::Filter::Filter(std::string node_name, const rclcpp::NodeOptions & options) -: PCLNode(node_name, options) -{ - RCLCPP_DEBUG(this->get_logger(), "Node successfully created."); -} - -////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::Filter::createPublishers() -{ - auto pub_options = rclcpp::PublisherOptions(); - pub_options.event_callbacks.matched_callback = [this](rclcpp::MatchedInfo & /*info*/) { - if (pub_output_->get_subscription_count() == 0) { - unsubscribe(); - } else { - if (use_indices_) { - if (!sub_input_filter_.getSubscriber() || !sub_indices_filter_.getSubscriber()) { - subscribe(); - } - } else { - if (!sub_input_) { - subscribe(); - } - } - } - }; - pub_output_ = create_publisher("output", max_queue_size_, pub_options); -} - -////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::Filter::use_frame_params() -{ - rcl_interfaces::msg::ParameterDescriptor input_frame_desc; - input_frame_desc.name = "input_frame"; - input_frame_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - input_frame_desc.description = - "The input TF frame the data should be transformed into before processing, " - "if input.header.frame_id is different."; - declare_parameter(input_frame_desc.name, rclcpp::ParameterValue(""), input_frame_desc); - - rcl_interfaces::msg::ParameterDescriptor output_frame_desc; - output_frame_desc.name = "output_frame"; - output_frame_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - output_frame_desc.description = - "The output TF frame the data should be transformed into after processing, " - "if input.header.frame_id is different."; - declare_parameter(output_frame_desc.name, rclcpp::ParameterValue(""), output_frame_desc); - - // Validate initial values using same callback - callback_handle_ = - add_on_set_parameters_callback( - std::bind( - &Filter::config_callback, this, - std::placeholders::_1)); - - std::vector param_names{input_frame_desc.name, output_frame_desc.name}; - auto result = config_callback(get_parameters(param_names)); - if (!result.successful) { - throw std::runtime_error(result.reason); - } -} - -////////////////////////////////////////////////////////////////////////////////////////////// -std::vector -pcl_ros::Filter::add_common_params() -{ - rcl_interfaces::msg::ParameterDescriptor ffn_desc; - ffn_desc.name = "filter_field_name"; - ffn_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - ffn_desc.description = "The field name used for filtering"; - declare_parameter(ffn_desc.name, rclcpp::ParameterValue("z"), ffn_desc); - - rcl_interfaces::msg::ParameterDescriptor flmin_desc; - flmin_desc.name = "filter_limit_min"; - flmin_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - flmin_desc.description = "The minimum allowed field value a point will be considered from"; - rcl_interfaces::msg::FloatingPointRange flmin_range; - flmin_range.from_value = -100000.0; - flmin_range.to_value = 100000.0; - flmin_desc.floating_point_range.push_back(flmin_range); - declare_parameter(flmin_desc.name, rclcpp::ParameterValue(0.0), flmin_desc); - - rcl_interfaces::msg::ParameterDescriptor flmax_desc; - flmax_desc.name = "filter_limit_max"; - flmax_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - flmax_desc.description = "The maximum allowed field value a point will be considered from"; - rcl_interfaces::msg::FloatingPointRange flmax_range; - flmax_range.from_value = -100000.0; - flmax_range.to_value = 100000.0; - flmax_desc.floating_point_range.push_back(flmax_range); - declare_parameter(flmax_desc.name, rclcpp::ParameterValue(1.0), flmax_desc); - - rcl_interfaces::msg::ParameterDescriptor flneg_desc; - flneg_desc.name = "filter_limit_negative"; - flneg_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - flneg_desc.description = - "Set to true if we want to return the data outside [filter_limit_min; filter_limit_max]."; - declare_parameter(flneg_desc.name, rclcpp::ParameterValue(false), flneg_desc); - - return std::vector { - ffn_desc.name, - flmin_desc.name, - flmax_desc.name, - flneg_desc.name - }; -} - -////////////////////////////////////////////////////////////////////////////////////////////// -rcl_interfaces::msg::SetParametersResult -pcl_ros::Filter::config_callback(const std::vector & params) -{ - std::lock_guard lock(mutex_); - - for (const rclcpp::Parameter & param : params) { - if (param.get_name() == "input_frame") { - if (tf_input_frame_ != param.as_string()) { - tf_input_frame_ = param.as_string(); - RCLCPP_DEBUG(get_logger(), "Setting the input frame to: %s.", tf_input_frame_.c_str()); - } - } - if (param.get_name() == "output_frame") { - if (tf_output_frame_ != param.as_string()) { - tf_output_frame_ = param.as_string(); - RCLCPP_DEBUG(get_logger(), "Setting the output frame to: %s.", tf_output_frame_.c_str()); - } - } - } - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - return result; -} - -////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::Filter::input_indices_callback( - const PointCloud2::ConstSharedPtr & cloud, - const PointIndices::ConstSharedPtr & indices) -{ - // If cloud is given, check if it's valid - if (!isValid(cloud)) { - RCLCPP_ERROR(this->get_logger(), "Invalid input!"); - return; - } - // If indices are given, check if they are valid - if (indices && !isValid(indices)) { - RCLCPP_ERROR(this->get_logger(), "Invalid indices!"); - return; - } - - /// DEBUG - if (indices) { - RCLCPP_DEBUG( - this->get_logger(), "[input_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %d.%09d, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %d.%09d, and frame %s on topic %s received.", - cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), - cloud->header.stamp.sec, cloud->header.stamp.nanosec, cloud->header.frame_id.c_str(), "input", - indices->indices.size(), indices->header.stamp.sec, indices->header.stamp.nanosec, - indices->header.frame_id.c_str(), "indices"); - } else { - RCLCPP_DEBUG( - this->get_logger(), "PointCloud with %d data points and frame %s on topic %s received.", - cloud->width * cloud->height, cloud->header.frame_id.c_str(), "input"); - } - /// - - // Check whether the user has given a different input TF frame - tf_input_orig_frame_ = cloud->header.frame_id; - PointCloud2::ConstSharedPtr cloud_tf; - if (!tf_input_frame_.empty() && cloud->header.frame_id != tf_input_frame_) { - RCLCPP_DEBUG( - this->get_logger(), "Transforming input dataset from %s to %s.", - cloud->header.frame_id.c_str(), tf_input_frame_.c_str()); - // Save the original frame ID - // Convert the cloud into the different frame - PointCloud2 cloud_transformed; - if (!pcl_ros::transformPointCloud(tf_input_frame_, *cloud, cloud_transformed, tf_buffer_)) { - RCLCPP_ERROR( - this->get_logger(), "Error converting input dataset from %s to %s.", - cloud->header.frame_id.c_str(), tf_input_frame_.c_str()); - return; - } - cloud_tf = std::make_shared(cloud_transformed); - } else { - cloud_tf = cloud; - } - - // Need setInputCloud () here because we have to extract x/y/z - IndicesPtr vindices; - if (indices) { - vindices.reset(new std::vector(indices->indices)); - } - - computePublish(cloud_tf, vindices); -} diff --git a/pcl_ros/src/pcl_ros/filters/passthrough.cpp b/pcl_ros/src/pcl_ros/filters/passthrough.cpp index 8d3cec52..059dd174 100644 --- a/pcl_ros/src/pcl_ros/filters/passthrough.cpp +++ b/pcl_ros/src/pcl_ros/filters/passthrough.cpp @@ -37,11 +37,48 @@ #include "pcl_ros/filters/passthrough.hpp" -pcl_ros::PassThrough::PassThrough(const rclcpp::NodeOptions & options) -: Filter("PassThroughNode", options) +namespace pcl_ros { - use_frame_params(); - std::vector common_param_names = add_common_params(); +PassThrough::PassThrough(const rclcpp::NodeOptions & options) +: PCLNode("PassThroughNode", options, std::vector{"input"}, + std::vector{"output"}) +{ + rcl_interfaces::msg::ParameterDescriptor ffn_desc; + ffn_desc.name = "filter_field_name"; + ffn_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + ffn_desc.description = "The field name used for filtering"; + declare_parameter(ffn_desc.name, rclcpp::ParameterValue("z"), ffn_desc); + + rcl_interfaces::msg::ParameterDescriptor flmin_desc; + flmin_desc.name = "filter_limit_min"; + flmin_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + flmin_desc.description = "The minimum allowed field value a point will be considered from"; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = -100000.0; + float_range.to_value = 100000.0; + flmin_desc.floating_point_range.push_back(float_range); + } + declare_parameter(flmin_desc.name, rclcpp::ParameterValue(0.0), flmin_desc); + + rcl_interfaces::msg::ParameterDescriptor flmax_desc; + flmax_desc.name = "filter_limit_max"; + flmax_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + flmax_desc.description = "The maximum allowed field value a point will be considered from"; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = -100000.0; + float_range.to_value = 100000.0; + flmax_desc.floating_point_range.push_back(float_range); + } + declare_parameter(flmax_desc.name, rclcpp::ParameterValue(1.0), flmax_desc); + + rcl_interfaces::msg::ParameterDescriptor flneg_desc; + flneg_desc.name = "filter_limit_negative"; + flneg_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + flneg_desc.description = + "Set to true if we want to return the data outside [filter_limit_min; filter_limit_max]."; + declare_parameter(flneg_desc.name, rclcpp::ParameterValue(false), flneg_desc); rcl_interfaces::msg::ParameterDescriptor keep_organized_desc; keep_organized_desc.name = "keep_organized"; @@ -50,43 +87,27 @@ pcl_ros::PassThrough::PassThrough(const rclcpp::NodeOptions & options) "Set whether the filtered points should be kept and set to NaN, " "or removed from the PointCloud, thus potentially breaking its organized structure."; declare_parameter(keep_organized_desc.name, rclcpp::ParameterValue(false), keep_organized_desc); - - std::vector param_names { - keep_organized_desc.name, - }; - param_names.insert(param_names.end(), common_param_names.begin(), common_param_names.end()); - - callback_handle_ = - add_on_set_parameters_callback( - std::bind( - &PassThrough::config_callback, this, - std::placeholders::_1)); - - config_callback(get_parameters(param_names)); - - createPublishers(); } -void pcl_ros::PassThrough::filter( - const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, - PointCloud2 & output) +void PassThrough::compute(const PointCloud2 & input, PointCloud2 & output) { - std::lock_guard lock(mutex_); + if(input.data.empty()) { + output = input; + return; + } pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); - pcl_conversions::toPCL(*(input), *(pcl_input)); + pcl_conversions::toPCL(input, *(pcl_input)); impl_.setInputCloud(pcl_input); - impl_.setIndices(indices); pcl::PCLPointCloud2 pcl_output; impl_.filter(pcl_output); pcl_conversions::moveFromPCL(pcl_output, output); + output.header = input.header; } ////////////////////////////////////////////////////////////////////////////////////////////// -rcl_interfaces::msg::SetParametersResult -pcl_ros::PassThrough::config_callback(const std::vector & params) +rcl_interfaces::msg::SetParametersResult PassThrough::onParamsChanged( + const std::vector & params) { - std::lock_guard lock(mutex_); - double filter_min, filter_max; impl_.getFilterLimits(filter_min, filter_max); @@ -151,6 +172,7 @@ pcl_ros::PassThrough::config_callback(const std::vector & par result.successful = true; return result; } +} // namespace pcl_ros #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::PassThrough) diff --git a/pcl_ros/src/pcl_ros/filters/project_inliers.cpp b/pcl_ros/src/pcl_ros/filters/project_inliers.cpp index 87c69dee..c4aaed03 100644 --- a/pcl_ros/src/pcl_ros/filters/project_inliers.cpp +++ b/pcl_ros/src/pcl_ros/filters/project_inliers.cpp @@ -38,8 +38,11 @@ #include "pcl_ros/filters/project_inliers.hpp" ////////////////////////////////////////////////////////////////////////////////////////////// -pcl_ros::ProjectInliers::ProjectInliers(const rclcpp::NodeOptions & options) -: Filter("ProjectInliersNode", options), model_() +namespace pcl_ros +{ +ProjectInliers::ProjectInliers(const rclcpp::NodeOptions & options) +: PCLNode("ProjectInliersNode", options, std::vector{"input", "indices", "model"}, + std::vector{"output"}) { // ---[ Mandatory parameters // The type of model to use (user given parameter). @@ -53,140 +56,89 @@ pcl_ros::ProjectInliers::ProjectInliers(const rclcpp::NodeOptions & options) } // ---[ Optional parameters // True if all data will be returned, false if only the projected inliers. Default: false. - declare_parameter("copy_all_data", rclcpp::ParameterValue(false)); - bool copy_all_data = get_parameter("copy_all_data").as_bool(); + rcl_interfaces::msg::ParameterDescriptor copy_all_data_desc; + copy_all_data_desc.name = "copy_all_data"; + copy_all_data_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + copy_all_data_desc.description = + "Whether all data will be returned, or only the projected inliers. true if all data should be returned, false if only the projected inliers"; + // Optional Parameter - Default Value: false + declare_parameter( + copy_all_data_desc.name, rclcpp::ParameterValue(false), copy_all_data_desc); // True if all fields will be returned, false if only XYZ. Default: true. - declare_parameter("copy_all_fields", rclcpp::ParameterValue(true)); - bool copy_all_fields = get_parameter("copy_all_fields").as_bool(); - - pub_output_ = create_publisher("output", max_queue_size_); - - RCLCPP_DEBUG( - this->get_logger(), - "[onConstruct] Node successfully created with the following parameters:\n" - " - model_type : %d\n" - " - copy_all_data : %s\n" - " - copy_all_fields : %s", - model_type, (copy_all_data) ? "true" : "false", (copy_all_fields) ? "true" : "false"); - - // Set given parameters here - impl_.setModelType(model_type); - impl_.setCopyAllFields(copy_all_fields); - impl_.setCopyAllData(copy_all_data); - - createPublishers(); + rcl_interfaces::msg::ParameterDescriptor copy_all_fields_desc; + copy_all_fields_desc.name = "copy_all_fields"; + copy_all_fields_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + copy_all_fields_desc.description = + "Whether all fields should be copied, or only the XYZ. true if all fields will be returned, false if only XYZ"; + // Optional Parameter - Default Value: true + declare_parameter( + copy_all_fields_desc.name, rclcpp::ParameterValue(true), copy_all_fields_desc); } -void -pcl_ros::ProjectInliers::filter( - const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, - PointCloud2 & output) +void ProjectInliers::compute( + const PointCloud2 & input, const PointIndices & indices, + const ModelCoefficients & model, PointCloud2 & output) { + if(input.data.empty()) { + output = input; + return; + } + pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); - pcl_conversions::toPCL(*(input), *(pcl_input)); + pcl_conversions::toPCL(input, *(pcl_input)); impl_.setInputCloud(pcl_input); - impl_.setIndices(indices); + + IndicesPtr pcl_indices(new pcl::PointIndices); + pcl_indices->indices = indices.indices; + impl_.setIndices(pcl_indices); pcl::ModelCoefficients::Ptr pcl_model(new pcl::ModelCoefficients); - pcl_conversions::toPCL(*(model_), *(pcl_model)); + pcl_conversions::toPCL(model, *(pcl_model)); impl_.setModelCoefficients(pcl_model); pcl::PCLPointCloud2 pcl_output; impl_.filter(pcl_output); pcl_conversions::moveFromPCL(pcl_output, output); } -////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::ProjectInliers::subscribe() -{ - RCLCPP_DEBUG(get_logger(), "subscribe"); -/* - TODO : implement use_indices_ - if (use_indices_) - {*/ - - auto qos_profile = rclcpp::QoS(rclcpp::KeepLast(max_queue_size_), rmw_qos_profile_default); - auto sensor_qos_profile = - rclcpp::QoS(rclcpp::KeepLast(max_queue_size_), rmw_qos_profile_sensor_data); - sub_input_filter_.subscribe(this, "input", sensor_qos_profile); - sub_indices_filter_.subscribe(this, "indices", qos_profile); - sub_model_.subscribe(this, "model", qos_profile); - - if (approximate_sync_) { - sync_input_indices_model_a_ = std::make_shared< - message_filters::Synchronizer< - message_filters::sync_policies::ApproximateTime< - PointCloud2, PointIndices, ModelCoefficients>>>(max_queue_size_); - sync_input_indices_model_a_->connectInput(sub_input_filter_, sub_indices_filter_, sub_model_); - sync_input_indices_model_a_->registerCallback( - std::bind( - &ProjectInliers::input_indices_model_callback, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - } else { - sync_input_indices_model_e_ = std::make_shared< - message_filters::Synchronizer< - message_filters::sync_policies::ExactTime< - PointCloud2, PointIndices, ModelCoefficients>>>(max_queue_size_); - sync_input_indices_model_e_->connectInput(sub_input_filter_, sub_indices_filter_, sub_model_); - sync_input_indices_model_e_->registerCallback( - std::bind( - &ProjectInliers::input_indices_model_callback, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - } -} - -////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::ProjectInliers::unsubscribe() -{ -/* - TODO : implement use_indices_ - if (use_indices_) - {*/ - sub_input_filter_.unsubscribe(); - sub_indices_filter_.unsubscribe(); - sub_model_.unsubscribe(); -} - -////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::ProjectInliers::input_indices_model_callback( - const PointCloud2::ConstSharedPtr & cloud, - const PointIndicesConstPtr & indices, - const ModelCoefficientsConstPtr & model) +rcl_interfaces::msg::SetParametersResult ProjectInliers::onParamsChanged( + const std::vector & params) { - if (pub_output_->get_subscription_count() == 0) { - return; - } - - if (!isValid(model) || !isValid(indices) || !isValid(cloud)) { - RCLCPP_ERROR( - this->get_logger(), "[%s::input_indices_model_callback] Invalid input!", this->get_name()); - return; - } - - RCLCPP_DEBUG( - this->get_logger(), - "[%s::input_indices_model_callback]\n" - " - PointCloud with %d data points (%s), stamp %d.%09d, and frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %d.%09d, and frame %s on topic %s received.\n" - " - ModelCoefficients with %zu values, stamp %d.%09d, and frame %s on topic %s received.", - this->get_name(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), - cloud->header.stamp.sec, cloud->header.stamp.nanosec, cloud->header.frame_id.c_str(), "input", - indices->indices.size(), indices->header.stamp.sec, indices->header.stamp.nanosec, - indices->header.frame_id.c_str(), "inliers", model->values.size(), - model->header.stamp.sec, model->header.stamp.nanosec, model->header.frame_id.c_str(), "model"); - - tf_input_orig_frame_ = cloud->header.frame_id; - - IndicesPtr vindices; - if (indices) { - vindices.reset(new std::vector(indices->indices)); + for (const rclcpp::Parameter & param : params) { + if (param.get_name() == "model_type") { + if (impl_.getModelType() != param.as_int()) { + RCLCPP_DEBUG( + get_logger(), + "Setting the model type to: %ld.", + param.as_int()); + impl_.setModelType(param.as_int()); + } + } + if (param.get_name() == "copy_all_data") { + if (impl_.getCopyAllData() != param.as_bool()) { + RCLCPP_DEBUG( + get_logger(), + "Setting copy all data to: %s.", + (param.as_bool() ? "true" : "false")); + impl_.setCopyAllData(param.as_bool()); + } + } + if (param.get_name() == "copy_all_fields") { + if (impl_.getCopyAllFields() != param.as_bool()) { + RCLCPP_DEBUG( + get_logger(), + "Setting copy all fields to: %s.", + (param.as_bool() ? "true" : "false")); + impl_.setCopyAllFields(param.as_bool()); + } + } } - model_ = model; - computePublish(cloud, vindices); + // Range constraints are enforced by rclcpp::Parameter. + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; } +} // namespace pcl_ros #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::ProjectInliers) diff --git a/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp b/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp index 1e487bf6..63a12d25 100644 --- a/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp +++ b/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp @@ -37,8 +37,11 @@ #include "pcl_ros/filters/radius_outlier_removal.hpp" -pcl_ros::RadiusOutlierRemoval::RadiusOutlierRemoval(const rclcpp::NodeOptions & options) -: Filter("RadiusOutlierRemovalNode", options) +namespace pcl_ros +{ +RadiusOutlierRemoval::RadiusOutlierRemoval(const rclcpp::NodeOptions & options) +: PCLNode("RadiusOutlierRemovalNode", options, std::vector{"input"}, + std::vector{"output"}) { rcl_interfaces::msg::ParameterDescriptor min_neighbors_desc; min_neighbors_desc.name = "min_neighbors"; @@ -65,44 +68,27 @@ pcl_ros::RadiusOutlierRemoval::RadiusOutlierRemoval(const rclcpp::NodeOptions & radius_search_desc.floating_point_range.push_back(float_range); } declare_parameter(radius_search_desc.name, rclcpp::ParameterValue(0.1), radius_search_desc); - - const std::vector param_names { - min_neighbors_desc.name, - radius_search_desc.name, - }; - - callback_handle_ = - add_on_set_parameters_callback( - std::bind( - &RadiusOutlierRemoval::config_callback, this, - std::placeholders::_1)); - - config_callback(get_parameters(param_names)); - - createPublishers(); } -void -pcl_ros::RadiusOutlierRemoval::filter( - const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, - PointCloud2 & output) +void RadiusOutlierRemoval::compute(const PointCloud2 & input, PointCloud2 & output) { - std::lock_guard lock(mutex_); + if(input.data.empty()) { + output = input; + return; + } + pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); - pcl_conversions::toPCL(*(input), *(pcl_input)); + pcl_conversions::toPCL(input, *(pcl_input)); impl_.setInputCloud(pcl_input); - impl_.setIndices(indices); pcl::PCLPointCloud2 pcl_output; impl_.filter(pcl_output); pcl_conversions::moveFromPCL(pcl_output, output); } ////////////////////////////////////////////////////////////////////////////////////////////// -rcl_interfaces::msg::SetParametersResult -pcl_ros::RadiusOutlierRemoval::config_callback(const std::vector & params) +rcl_interfaces::msg::SetParametersResult RadiusOutlierRemoval::onParamsChanged( + const std::vector & params) { - std::lock_guard lock(mutex_); - for (const rclcpp::Parameter & param : params) { if (param.get_name() == "min_neighbors") { if (impl_.getMinNeighborsInRadius() != param.as_int()) { @@ -127,6 +113,7 @@ pcl_ros::RadiusOutlierRemoval::config_callback(const std::vector{"input"}, + std::vector{"output"}) { rcl_interfaces::msg::ParameterDescriptor mean_k_desc; mean_k_desc.name = "mean_k"; @@ -73,45 +76,27 @@ pcl_ros::StatisticalOutlierRemoval::StatisticalOutlierRemoval(const rclcpp::Node negative_desc.description = "Set whether the inliers should be returned (false) or the outliers (true)."; declare_parameter(negative_desc.name, rclcpp::ParameterValue(false), negative_desc); - - const std::vector param_names { - mean_k_desc.name, - stddev_desc.name, - negative_desc.name, - }; - - callback_handle_ = - add_on_set_parameters_callback( - std::bind( - &StatisticalOutlierRemoval::config_callback, this, - std::placeholders::_1)); - - config_callback(get_parameters(param_names)); - - createPublishers(); } -void -pcl_ros::StatisticalOutlierRemoval::filter( - const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, - PointCloud2 & output) +void StatisticalOutlierRemoval::compute(const PointCloud2 & input, PointCloud2 & output) { - std::lock_guard lock(mutex_); + if(input.data.empty()) { + output = input; + return; + } pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); - pcl_conversions::toPCL(*(input), *(pcl_input)); + pcl_conversions::toPCL(input, *(pcl_input)); impl_.setInputCloud(pcl_input); - impl_.setIndices(indices); pcl::PCLPointCloud2 pcl_output; impl_.filter(pcl_output); pcl_conversions::moveFromPCL(pcl_output, output); + output.header = input.header; } ////////////////////////////////////////////////////////////////////////////////////////////// -rcl_interfaces::msg::SetParametersResult -pcl_ros::StatisticalOutlierRemoval::config_callback(const std::vector & params) +rcl_interfaces::msg::SetParametersResult StatisticalOutlierRemoval::onParamsChanged( + const std::vector & params) { - std::lock_guard lock(mutex_); - for (const rclcpp::Parameter & param : params) { if (param.get_name() == "mean_k") { if (impl_.getMeanK() != param.as_int()) { @@ -147,6 +132,7 @@ pcl_ros::StatisticalOutlierRemoval::config_callback(const std::vector{"input"}, + std::vector{"output"}) { - std::vector common_param_names = add_common_params(); + rcl_interfaces::msg::ParameterDescriptor ffn_desc; + ffn_desc.name = "filter_field_name"; + ffn_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + ffn_desc.description = "The field name used for filtering"; + declare_parameter(ffn_desc.name, rclcpp::ParameterValue("z"), ffn_desc); + + rcl_interfaces::msg::ParameterDescriptor flmin_desc; + flmin_desc.name = "filter_limit_min"; + flmin_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + flmin_desc.description = "The minimum allowed field value a point will be considered from"; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = -100000.0; + float_range.to_value = 100000.0; + flmin_desc.floating_point_range.push_back(float_range); + } + declare_parameter(flmin_desc.name, rclcpp::ParameterValue(0.0), flmin_desc); + + rcl_interfaces::msg::ParameterDescriptor flmax_desc; + flmax_desc.name = "filter_limit_max"; + flmax_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + flmax_desc.description = "The maximum allowed field value a point will be considered from"; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = -100000.0; + float_range.to_value = 100000.0; + flmax_desc.floating_point_range.push_back(float_range); + } + declare_parameter(flmax_desc.name, rclcpp::ParameterValue(1.0), flmax_desc); + + rcl_interfaces::msg::ParameterDescriptor flneg_desc; + flneg_desc.name = "filter_limit_negative"; + flneg_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + flneg_desc.description = + "Set to true if we want to return the data outside [filter_limit_min; filter_limit_max]."; + declare_parameter(flneg_desc.name, rclcpp::ParameterValue(false), flneg_desc); rcl_interfaces::msg::ParameterDescriptor leaf_size_desc; leaf_size_desc.name = "leaf_size"; @@ -70,34 +107,13 @@ pcl_ros::VoxelGrid::VoxelGrid(const rclcpp::NodeOptions & options) } declare_parameter( min_points_per_voxel_desc.name, rclcpp::ParameterValue(2), min_points_per_voxel_desc); - - std::vector param_names { - leaf_size_desc.name, - min_points_per_voxel_desc.name, - }; - param_names.insert(param_names.end(), common_param_names.begin(), common_param_names.end()); - - callback_handle_ = - add_on_set_parameters_callback( - std::bind( - &VoxelGrid::config_callback, this, - std::placeholders::_1)); - - config_callback(get_parameters(param_names)); - - createPublishers(); } -void -pcl_ros::VoxelGrid::filter( - const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, - PointCloud2 & output) +void VoxelGrid::compute(const PointCloud2 & input, PointCloud2 & output) { - std::lock_guard lock(mutex_); pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); - pcl_conversions::toPCL(*(input), *(pcl_input)); + pcl_conversions::toPCL(input, *(pcl_input)); impl_.setInputCloud(pcl_input); - impl_.setIndices(indices); pcl::PCLPointCloud2 pcl_output; impl_.filter(pcl_output); pcl_conversions::moveFromPCL(pcl_output, output); @@ -105,10 +121,8 @@ pcl_ros::VoxelGrid::filter( ////////////////////////////////////////////////////////////////////////////////////////////// rcl_interfaces::msg::SetParametersResult -pcl_ros::VoxelGrid::config_callback(const std::vector & params) +VoxelGrid::onParamsChanged(const std::vector & params) { - std::lock_guard lock(mutex_); - double filter_min, filter_max; impl_.getFilterLimits(filter_min, filter_max); @@ -186,6 +200,7 @@ pcl_ros::VoxelGrid::config_callback(const std::vector & param result.successful = true; return result; } +} // namespace pcl_ros #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::VoxelGrid) diff --git a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp index bcc128c4..a6e4ba6f 100644 --- a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp @@ -35,750 +35,665 @@ * */ -#include -#include -#include -#include -#include "pcl_ros/segmentation/sac_segmentation.hpp" - -using pcl_conversions::fromPCL; - -////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::SACSegmentation::onInit() -{ - // Call the super onInit () - PCLNodelet::onInit(); - - - // Advertise the output topics - pub_indices_ = advertise(*pnh_, "inliers", max_queue_size_); - pub_model_ = advertise(*pnh_, "model", max_queue_size_); - - // ---[ Mandatory parameters - int model_type; - if (!pnh_->getParam("model_type", model_type)) { - NODELET_ERROR("[onInit] Need a 'model_type' parameter to be set before continuing!"); - return; - } - double threshold; // unused - set via dynamic reconfigure in the callback - if (!pnh_->getParam("distance_threshold", threshold)) { - NODELET_ERROR("[onInit] Need a 'distance_threshold' parameter to be set before continuing!"); - return; - } - - // ---[ Optional parameters - int method_type = 0; - pnh_->getParam("method_type", method_type); - - XmlRpc::XmlRpcValue axis_param; - pnh_->getParam("axis", axis_param); - Eigen::Vector3f axis = Eigen::Vector3f::Zero(); - - switch (axis_param.getType()) { - case XmlRpc::XmlRpcValue::TypeArray: - { - if (axis_param.size() != 3) { - NODELET_ERROR( - "[%s::onInit] Parameter 'axis' given but with a different number of values (%d) " - "than required (3)!", - getName().c_str(), axis_param.size()); - return; - } - for (int i = 0; i < 3; ++i) { - if (axis_param[i].getType() != XmlRpc::XmlRpcValue::TypeDouble) { - NODELET_ERROR( - "[%s::onInit] Need floating point values for 'axis' parameter.", - getName().c_str()); - return; - } - double value = axis_param[i]; axis[i] = value; - } - break; - } - default: - { - break; - } - } +#include - // Initialize the random number generator - srand(time(0)); - - // Enable the dynamic reconfigure service - srv_ = boost::make_shared>(*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind( - &SACSegmentation::config_callback, this, _1, _2); - srv_->setCallback(f); - - NODELET_DEBUG( - "[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - model_type : %d\n" - " - method_type : %d\n" - " - model_threshold : %f\n" - " - axis : [%f, %f, %f]\n", - getName().c_str(), model_type, method_type, threshold, - axis[0], axis[1], axis[2]); - - // Set given parameters here - impl_.setModelType(model_type); - impl_.setMethodType(method_type); - impl_.setAxis(axis); - - onInitPostProcess(); -} +#include "pcl_ros/segmentation/sac_segmentation.hpp" +#include +#include "pcl_ros/transforms.hpp" -////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::SACSegmentation::subscribe() -{ - // If we're supposed to look for PointIndices (indices) - if (use_indices_) { - // Subscribe to the input using a filter - sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_); - sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); - - // when "use_indices" is set to true, and "latched_indices" is set to true, - // we'll subscribe and get a separate callback for PointIndices that will - // save the indices internally, and a PointCloud + PointIndices callback - // will take care of meshing the new PointClouds with the old saved indices. - if (latched_indices_) { - // Subscribe to a callback that saves the indices - sub_indices_filter_.registerCallback(bind(&SACSegmentation::indices_callback, this, _1)); - // Subscribe to a callback that sets the header of the saved indices to the cloud header - sub_input_filter_.registerCallback(bind(&SACSegmentation::input_callback, this, _1)); - - // Synchronize the two topics. No need for an approximate synchronizer here, as we'll - // match the timestamps exactly - sync_input_indices_e_ = - boost::make_shared>>(max_queue_size_); - sync_input_indices_e_->connectInput(sub_input_filter_, nf_pi_); - sync_input_indices_e_->registerCallback( - bind( - &SACSegmentation::input_indices_callback, this, - _1, _2)); - } else { // "latched_indices" not set, proceed with regular pairs - if (approximate_sync_) { - sync_input_indices_a_ = - boost::make_shared>>(max_queue_size_); - sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); - sync_input_indices_a_->registerCallback( - bind( - &SACSegmentation::input_indices_callback, this, - _1, _2)); - } else { - sync_input_indices_e_ = - boost::make_shared>>(max_queue_size_); - sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); - sync_input_indices_e_->registerCallback( - bind( - &SACSegmentation::input_indices_callback, this, - _1, _2)); - } - } - } else { - // Subscribe in an old fashion to input only (no filters) - sub_input_ = - pnh_->subscribe( - "input", max_queue_size_, - bind(&SACSegmentation::input_indices_callback, this, _1, PointIndicesConstPtr())); - } -} ////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::SACSegmentation::unsubscribe() +namespace pcl_ros { - if (use_indices_) { - sub_input_filter_.unsubscribe(); - sub_indices_filter_.unsubscribe(); - } else { - sub_input_.shutdown(); - } -} - -////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::SACSegmentation::config_callback(SACSegmentationConfig & config, uint32_t level) +pcl_ros::SACSegmentation::SACSegmentation(const rclcpp::NodeOptions & options) +: PCLNode("SACSegmentationNode", options, std::vector{"input"}, + std::vector{"indices", "model"}) { - boost::mutex::scoped_lock lock(mutex_); - - if (impl_.getDistanceThreshold() != config.distance_threshold) { - // sac_->setDistanceThreshold (threshold_); - done in initSAC - impl_.setDistanceThreshold(config.distance_threshold); - NODELET_DEBUG( - "[%s::config_callback] Setting new distance to model threshold to: %f.", - getName().c_str(), config.distance_threshold); - } - // The maximum allowed difference between the model normal and the given axis _in radians_ - if (impl_.getEpsAngle() != config.eps_angle) { - impl_.setEpsAngle(config.eps_angle); - NODELET_DEBUG( - "[%s::config_callback] Setting new epsilon angle to model threshold to: %f (%f degrees).", - getName().c_str(), config.eps_angle, config.eps_angle * 180.0 / M_PI); - } - - // Number of inliers - if (min_inliers_ != config.min_inliers) { - min_inliers_ = config.min_inliers; - NODELET_DEBUG( - "[%s::config_callback] Setting new minimum number of inliers to: %d.", - getName().c_str(), min_inliers_); - } - - if (impl_.getMaxIterations() != config.max_iterations) { - // sac_->setMaxIterations (max_iterations_); - done in initSAC - impl_.setMaxIterations(config.max_iterations); - NODELET_DEBUG( - "[%s::config_callback] Setting new maximum number of iterations to: %d.", - getName().c_str(), config.max_iterations); - } - if (impl_.getProbability() != config.probability) { - // sac_->setProbability (probability_); - done in initSAC - impl_.setProbability(config.probability); - NODELET_DEBUG( - "[%s::config_callback] Setting new probability to: %f.", - getName().c_str(), config.probability); - } - if (impl_.getOptimizeCoefficients() != config.optimize_coefficients) { - impl_.setOptimizeCoefficients(config.optimize_coefficients); - NODELET_DEBUG( - "[%s::config_callback] Setting coefficient optimization to: %s.", - getName().c_str(), (config.optimize_coefficients) ? "true" : "false"); - } - - double radius_min, radius_max; - impl_.getRadiusLimits(radius_min, radius_max); - if (radius_min != config.radius_min) { - radius_min = config.radius_min; - NODELET_DEBUG("[config_callback] Setting minimum allowable model radius to: %f.", radius_min); - impl_.setRadiusLimits(radius_min, radius_max); - } - if (radius_max != config.radius_max) { - radius_max = config.radius_max; - NODELET_DEBUG("[config_callback] Setting maximum allowable model radius to: %f.", radius_max); - impl_.setRadiusLimits(radius_min, radius_max); - } - - if (tf_input_frame_ != config.input_frame) { - tf_input_frame_ = config.input_frame; - NODELET_DEBUG("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str()); - NODELET_WARN("input_frame TF not implemented yet!"); - } - if (tf_output_frame_ != config.output_frame) { - tf_output_frame_ = config.output_frame; - NODELET_DEBUG( - "[config_callback] Setting the output TF frame to: %s.", - tf_output_frame_.c_str()); - NODELET_WARN("output_frame TF not implemented yet!"); - } + rcl_interfaces::msg::ParameterDescriptor model_type_desc; + model_type_desc.name = "model_type"; + model_type_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + model_type_desc.description = + "The type of model to use for segmentation."; + { + rcl_interfaces::msg::IntegerRange int_range; + int_range.from_value = 0; + int_range.to_value = 17; + model_type_desc.integer_range.push_back(int_range); + } + declare_parameter( + model_type_desc.name, rclcpp::ParameterValue( + pcl::SACMODEL_PLANE), model_type_desc); + + rcl_interfaces::msg::ParameterDescriptor distance_threshold_desc; + distance_threshold_desc.name = "distance_threshold"; + distance_threshold_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + distance_threshold_desc.description = + "The minimum distance (in meters) for a point to be considered an inlier."; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = 0.0; + float_range.to_value = 100000.0; + distance_threshold_desc.floating_point_range.push_back(float_range); + } + declare_parameter( + distance_threshold_desc.name, rclcpp::ParameterValue(0.01), distance_threshold_desc); + + rcl_interfaces::msg::ParameterDescriptor eps_angle_desc; + eps_angle_desc.name = "eps_angle"; + eps_angle_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + eps_angle_desc.description = + "The maximum allowed difference (radians) between the model normal and the given axis."; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = 0.0; + float_range.to_value = M_PI_2; + eps_angle_desc.floating_point_range.push_back(float_range); + } + declare_parameter( + eps_angle_desc.name, rclcpp::ParameterValue(0.0), eps_angle_desc); + + rcl_interfaces::msg::ParameterDescriptor method_type_desc; + method_type_desc.name = "method_type"; + method_type_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + method_type_desc.description = + "The type of method to use for segmentation."; + { + rcl_interfaces::msg::IntegerRange int_range; + int_range.from_value = 0; + int_range.to_value = 6; + method_type_desc.integer_range.push_back(int_range); + } + declare_parameter( + method_type_desc.name, rclcpp::ParameterValue(pcl::SAC_RANSAC), + method_type_desc); + + rcl_interfaces::msg::ParameterDescriptor axis_desc; + axis_desc.name = "axis"; + axis_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY; + axis_desc.description = + "The axis along which the method need to search for a model perpendicular to."; + declare_parameter( + axis_desc.name, rclcpp::ParameterValue(std::vector({0.0, 0.0, 0.0})), + axis_desc); + + rcl_interfaces::msg::ParameterDescriptor max_iterations_desc; + max_iterations_desc.name = "max_iterations"; + max_iterations_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + max_iterations_desc.description = + "The maximum number of iterations the sample consensus method will run."; + declare_parameter( + max_iterations_desc.name, rclcpp::ParameterValue(pcl::SAC_RANSAC), + max_iterations_desc); + + rcl_interfaces::msg::ParameterDescriptor probability_desc; + probability_desc.name = "probability"; + probability_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + probability_desc.description = + "The desired probability of choosing at least one sample free from outliers."; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = 0.0; + float_range.to_value = 1.0; + probability_desc.floating_point_range.push_back(float_range); + } + declare_parameter( + probability_desc.name, rclcpp::ParameterValue(0.99), probability_desc); + + rcl_interfaces::msg::ParameterDescriptor optimize_coefficients_desc; + optimize_coefficients_desc.name = "optimize_coefficients"; + optimize_coefficients_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + optimize_coefficients_desc.description = + "Model coefficient refinement. true for enabling model coefficient refinement, false otherwise."; + declare_parameter( + optimize_coefficients_desc.name, rclcpp::ParameterValue(true), optimize_coefficients_desc); + + rcl_interfaces::msg::ParameterDescriptor radius_min_desc; + radius_min_desc.name = "radius_min"; + radius_min_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + radius_min_desc.description = + "The minimum allowable radius for the model (applicable to models that estimate a radius)"; + declare_parameter( + radius_min_desc.name, rclcpp::ParameterValue( + -std::numeric_limits::max()), radius_min_desc); + + rcl_interfaces::msg::ParameterDescriptor radius_max_desc; + radius_max_desc.name = "radius_max"; + radius_max_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + radius_max_desc.description = + "The maximum allowable radius for the model (applicable to models that estimate a radius)"; + declare_parameter( + radius_max_desc.name, rclcpp::ParameterValue( + std::numeric_limits::max()), radius_max_desc); } ////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::SACSegmentation::input_indices_callback( - const PointCloudConstPtr & cloud, - const PointIndicesConstPtr & indices) +void SACSegmentation::compute( + const PointCloud2 & input, PointIndices & indices, ModelCoefficients & model) { - boost::mutex::scoped_lock lock(mutex_); - - pcl_msgs::PointIndices inliers; - pcl_msgs::ModelCoefficients model; - // Enforce that the TF frame and the timestamp are copied - inliers.header = model.header = fromPCL(cloud->header); - - // If cloud is given, check if it's valid - if (!isValid(cloud)) { - NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str()); - pub_indices_.publish(inliers); - pub_model_.publish(model); - return; - } - // If indices are given, check if they are valid - if (indices && !isValid(indices)) { - NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str()); - pub_indices_.publish(inliers); - pub_model_.publish(model); + if(input.data.empty()) { + indices.header = model.header = input.header; return; } - /// DEBUG - if (indices && !indices->header.frame_id.empty()) { - NODELET_DEBUG( - "[%s::input_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and " - "frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and " - "frame %s on topic %s received.", - getName().c_str(), - cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( - cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( - "input").c_str(), - indices->indices.size(), indices->header.stamp.toSec(), - indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); - } else { - NODELET_DEBUG( - "[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and " - "frame %s on topic %s received.", - getName().c_str(), cloud->width * cloud->height, fromPCL( - cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( - "input").c_str()); - } - /// - - // Check whether the user has given a different input TF frame - tf_input_orig_frame_ = cloud->header.frame_id; - PointCloudConstPtr cloud_tf; -/* if (!tf_input_frame_.empty () && cloud->header.frame_id != tf_input_frame_) - { - NODELET_DEBUG ("[input_callback] Transforming input dataset from %s to %s.", - // cloud->header.frame_id.c_str (), tf_input_frame_.c_str ()); - // Save the original frame ID - // Convert the cloud into the different frame - PointCloud cloud_transformed; - if (!pcl::transformPointCloud (tf_input_frame_, cloud->header.stamp, *cloud, - cloud_transformed, tf_listener_)) - return; - cloud_tf.reset (new PointCloud (cloud_transformed)); - } - else*/ - cloud_tf = cloud; - - IndicesPtr indices_ptr; - if (indices && !indices->header.frame_id.empty()) { - indices_ptr.reset(new std::vector(indices->indices)); - } - - impl_.setInputCloud(pcl_ptr(cloud_tf)); - impl_.setIndices(indices_ptr); - - // Final check if the data is empty - // (remember that indices are set to the size of the data -- if indices* = NULL) - if (!cloud->points.empty()) { - pcl::PointIndices pcl_inliers; - pcl::ModelCoefficients pcl_model; - pcl_conversions::moveToPCL(inliers, pcl_inliers); - pcl_conversions::moveToPCL(model, pcl_model); - impl_.segment(pcl_inliers, pcl_model); - pcl_conversions::moveFromPCL(pcl_inliers, inliers); - pcl_conversions::moveFromPCL(pcl_model, model); - } - - // Probably need to transform the model of the plane here - - // Check if we have enough inliers, clear inliers + model if not - if (static_cast(inliers.indices.size()) <= min_inliers_) { - inliers.indices.clear(); - model.values.clear(); - } - - // Publish - pub_indices_.publish(boost::make_shared(inliers)); - pub_model_.publish(boost::make_shared(model)); - NODELET_DEBUG( - "[%s::input_indices_callback] Published PointIndices with %zu values on topic %s, " - "and ModelCoefficients with %zu values on topic %s", - getName().c_str(), inliers.indices.size(), pnh_->resolveName("inliers").c_str(), - model.values.size(), pnh_->resolveName("model").c_str()); - - if (inliers.indices.empty()) { - NODELET_WARN("[%s::input_indices_callback] No inliers found!", getName().c_str()); - } + pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); + pcl::fromROSMsg(input, *pcl_input); + impl_.setInputCloud(pcl_input); + pcl::PointIndices::Ptr pcl_inliers(new pcl::PointIndices()); + pcl::ModelCoefficients::Ptr pcl_model(new pcl::ModelCoefficients); + impl_.segment(*pcl_inliers, *pcl_model); + pcl_conversions::moveFromPCL(*pcl_inliers, indices); + pcl_conversions::moveFromPCL(*pcl_model, model); } ////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::SACSegmentationFromNormals::onInit() +rcl_interfaces::msg::SetParametersResult SACSegmentation::onParamsChanged( + const std::vector & params) { - // Call the super onInit () - PCLNodelet::onInit(); - - // Enable the dynamic reconfigure service - srv_ = boost::make_shared>(*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind( - &SACSegmentationFromNormals::config_callback, this, _1, _2); - srv_->setCallback(f); - - // Advertise the output topics - pub_indices_ = advertise(*pnh_, "inliers", max_queue_size_); - pub_model_ = advertise(*pnh_, "model", max_queue_size_); - - // ---[ Mandatory parameters - int model_type; - if (!pnh_->getParam("model_type", model_type)) { - NODELET_ERROR( - "[%s::onInit] Need a 'model_type' parameter to be set before continuing!", - getName().c_str()); - return; - } - double threshold; // unused - set via dynamic reconfigure in the callback - if (!pnh_->getParam("distance_threshold", threshold)) { - NODELET_ERROR( - "[%s::onInit] Need a 'distance_threshold' parameter to be set before continuing!", - getName().c_str()); - return; - } - - // ---[ Optional parameters - int method_type = 0; - pnh_->getParam("method_type", method_type); - - XmlRpc::XmlRpcValue axis_param; - pnh_->getParam("axis", axis_param); - Eigen::Vector3f axis = Eigen::Vector3f::Zero(); - - switch (axis_param.getType()) { - case XmlRpc::XmlRpcValue::TypeArray: - { - if (axis_param.size() != 3) { - NODELET_ERROR( - "[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than " - "required (3)!", - getName().c_str(), axis_param.size()); - return; - } - for (int i = 0; i < 3; ++i) { - if (axis_param[i].getType() != XmlRpc::XmlRpcValue::TypeDouble) { - NODELET_ERROR( - "[%s::onInit] Need floating point values for 'axis' parameter.", - getName().c_str()); - return; - } - double value = axis_param[i]; axis[i] = value; - } - break; + for (const rclcpp::Parameter & param : params) { + if (param.get_name() == "model_type") { + int model_type = impl_.getModelType(); + if (model_type != param.as_int()) { + RCLCPP_DEBUG( + get_logger(), + "Setting the model type to: %u.", + model_type); + impl_.setModelType(param.as_int()); } - default: - { - break; + } + if (param.get_name() == "distance_threshold") { + double distance_threshold = impl_.getDistanceThreshold(); + if (distance_threshold != param.as_double()) { + distance_threshold = param.as_double(); + RCLCPP_DEBUG( + get_logger(), + "Setting the distance threshold to: %f.", + distance_threshold); + impl_.setDistanceThreshold(distance_threshold); } - } - - // Initialize the random number generator - srand(time(0)); - - NODELET_DEBUG( - "[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - model_type : %d\n" - " - method_type : %d\n" - " - model_threshold : %f\n" - " - axis : [%f, %f, %f]\n", - getName().c_str(), model_type, method_type, threshold, - axis[0], axis[1], axis[2]); - - // Set given parameters here - impl_.setModelType(model_type); - impl_.setMethodType(method_type); - impl_.setAxis(axis); - - onInitPostProcess(); -} - -////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::SACSegmentationFromNormals::subscribe() -{ - // Subscribe to the input and normals using filters - sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_); - sub_normals_filter_.subscribe(*pnh_, "normals", max_queue_size_); - - // Subscribe to an axis direction along which the model search is to be constrained (the first - // 3 model coefficients will be checked) - sub_axis_ = pnh_->subscribe("axis", 1, &SACSegmentationFromNormals::axis_callback, this); - - if (approximate_sync_) { - sync_input_normals_indices_a_ = - boost::make_shared>>(max_queue_size_); - } else { - sync_input_normals_indices_e_ = - boost::make_shared>>(max_queue_size_); - } - - // If we're supposed to look for PointIndices (indices) - if (use_indices_) { - // Subscribe to the input using a filter - sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); - - if (approximate_sync_) { - sync_input_normals_indices_a_->connectInput( - sub_input_filter_, sub_normals_filter_, - sub_indices_filter_); - } else { - sync_input_normals_indices_e_->connectInput( - sub_input_filter_, sub_normals_filter_, - sub_indices_filter_); } - } else { - // Create a different callback for copying over the timestamp to fake indices - sub_input_filter_.registerCallback(bind(&SACSegmentationFromNormals::input_callback, this, _1)); - - if (approximate_sync_) { - sync_input_normals_indices_a_->connectInput(sub_input_filter_, sub_normals_filter_, nf_); - } else { - sync_input_normals_indices_e_->connectInput(sub_input_filter_, sub_normals_filter_, nf_); + if (param.get_name() == "eps_angle") { + double eps_angle = impl_.getEpsAngle(); + if (eps_angle != param.as_double()) { + eps_angle = param.as_double(); + RCLCPP_DEBUG( + get_logger(), + "Setting the eps angle to: %f.", + eps_angle); + impl_.setEpsAngle(eps_angle); + } + } + if (param.get_name() == "method_type") { + int method_type = impl_.getMethodType(); + if (method_type != param.as_int()) { + RCLCPP_DEBUG( + get_logger(), + "Setting the method type to: %u.", + method_type); + impl_.setMethodType(param.as_int()); + } + } + if (param.get_name() == "axis") { + std::vector axis_param = param.as_double_array(); + Eigen::Vector3f axis(axis_param[0], axis_param[1], axis_param[2]); + if (impl_.getAxis() != axis) { + RCLCPP_DEBUG( + get_logger(), "Setting the axis to: %f %f %f.", + axis[0], axis[1], axis[2]); + impl_.setAxis(axis); + } + } + if (param.get_name() == "max_iterations") { + int max_iterations = impl_.getMaxIterations(); + if (max_iterations != param.as_int()) { + RCLCPP_DEBUG( + get_logger(), + "Setting the max iterations to: %u.", + max_iterations); + impl_.setMaxIterations(param.as_int()); + } + } + if (param.get_name() == "probability") { + double probability = impl_.getProbability(); + if (probability != param.as_double()) { + probability = param.as_double(); + RCLCPP_DEBUG( + get_logger(), + "Setting the probability to: %f.", + probability); + impl_.setProbability(probability); + } + } + if (param.get_name() == "optimize_coefficients") { + bool optimize_coefficients = param.as_bool(); + if (impl_.getOptimizeCoefficients() != optimize_coefficients) { + RCLCPP_DEBUG( + get_logger(), + "Setting optimize coefficients to: %s.", + (optimize_coefficients ? "true" : "false")); + impl_.setOptimizeCoefficients(optimize_coefficients); + } + } + if (param.get_name() == "radius_min") { + double radius_min, radius_max; + impl_.getRadiusLimits(radius_min, radius_max); + if (radius_min != param.as_double()) { + radius_min = param.as_double(); + RCLCPP_DEBUG( + get_logger(), + "Setting the minimum radius to: %f.", + radius_min); + impl_.setRadiusLimits(radius_min, radius_max); + } + } + if (param.get_name() == "radius_max") { + double radius_min, radius_max; + impl_.getRadiusLimits(radius_min, radius_max); + if (radius_max != param.as_double()) { + radius_max = param.as_double(); + RCLCPP_DEBUG( + get_logger(), + "Setting the maximum radius to: %f.", + radius_max); + impl_.setRadiusLimits(radius_min, radius_max); + } } } - if (approximate_sync_) { - sync_input_normals_indices_a_->registerCallback( - bind( - &SACSegmentationFromNormals:: - input_normals_indices_callback, this, _1, _2, _3)); - } else { - sync_input_normals_indices_e_->registerCallback( - bind( - &SACSegmentationFromNormals:: - input_normals_indices_callback, this, _1, _2, _3)); - } -} - -////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::SACSegmentationFromNormals::unsubscribe() -{ - sub_input_filter_.unsubscribe(); - sub_normals_filter_.unsubscribe(); - - sub_axis_.shutdown(); - - if (use_indices_) { - sub_indices_filter_.unsubscribe(); - } -} - -////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::SACSegmentationFromNormals::axis_callback( - const pcl_msgs::ModelCoefficientsConstPtr & model) -{ - boost::mutex::scoped_lock lock(mutex_); - - if (model->values.size() < 3) { - NODELET_ERROR( - "[%s::axis_callback] Invalid axis direction / model coefficients with %zu values sent on %s!", - getName().c_str(), model->values.size(), pnh_->resolveName("axis").c_str()); - return; - } - NODELET_DEBUG( - "[%s::axis_callback] Received axis direction: %f %f %f", - getName().c_str(), model->values[0], model->values[1], model->values[2]); - - Eigen::Vector3f axis(model->values[0], model->values[1], model->values[2]); - impl_.setAxis(axis); -} - -////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::SACSegmentationFromNormals::config_callback( - SACSegmentationFromNormalsConfig & config, - uint32_t level) -{ - boost::mutex::scoped_lock lock(mutex_); - - if (impl_.getDistanceThreshold() != config.distance_threshold) { - impl_.setDistanceThreshold(config.distance_threshold); - NODELET_DEBUG( - "[%s::config_callback] Setting distance to model threshold to: %f.", - getName().c_str(), config.distance_threshold); - } - // The maximum allowed difference between the model normal and the given axis _in radians_ - if (impl_.getEpsAngle() != config.eps_angle) { - impl_.setEpsAngle(config.eps_angle); - NODELET_DEBUG( - "[%s::config_callback] Setting new epsilon angle to model threshold to: %f (%f degrees).", - getName().c_str(), config.eps_angle, config.eps_angle * 180.0 / M_PI); - } - - if (impl_.getMaxIterations() != config.max_iterations) { - impl_.setMaxIterations(config.max_iterations); - NODELET_DEBUG( - "[%s::config_callback] Setting new maximum number of iterations to: %d.", - getName().c_str(), config.max_iterations); - } - - // Number of inliers - if (min_inliers_ != config.min_inliers) { - min_inliers_ = config.min_inliers; - NODELET_DEBUG( - "[%s::config_callback] Setting new minimum number of inliers to: %d.", - getName().c_str(), min_inliers_); - } - - - if (impl_.getProbability() != config.probability) { - impl_.setProbability(config.probability); - NODELET_DEBUG( - "[%s::config_callback] Setting new probability to: %f.", - getName().c_str(), config.probability); - } - - if (impl_.getOptimizeCoefficients() != config.optimize_coefficients) { - impl_.setOptimizeCoefficients(config.optimize_coefficients); - NODELET_DEBUG( - "[%s::config_callback] Setting coefficient optimization to: %s.", - getName().c_str(), (config.optimize_coefficients) ? "true" : "false"); - } - - if (impl_.getNormalDistanceWeight() != config.normal_distance_weight) { - impl_.setNormalDistanceWeight(config.normal_distance_weight); - NODELET_DEBUG( - "[%s::config_callback] Setting new distance weight to: %f.", - getName().c_str(), config.normal_distance_weight); - } - - double radius_min, radius_max; - impl_.getRadiusLimits(radius_min, radius_max); - if (radius_min != config.radius_min) { - radius_min = config.radius_min; - NODELET_DEBUG( - "[%s::config_callback] Setting minimum allowable model radius to: %f.", - getName().c_str(), radius_min); - impl_.setRadiusLimits(radius_min, radius_max); - } - if (radius_max != config.radius_max) { - radius_max = config.radius_max; - NODELET_DEBUG( - "[%s::config_callback] Setting maximum allowable model radius to: %f.", - getName().c_str(), radius_max); - impl_.setRadiusLimits(radius_min, radius_max); - } + // Range constraints are enforced by rclcpp::Parameter. + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; } ////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback( - const PointCloudConstPtr & cloud, - const PointCloudNConstPtr & cloud_normals, - const PointIndicesConstPtr & indices -) -{ - boost::mutex::scoped_lock lock(mutex_); - - PointIndices inliers; - ModelCoefficients model; - // Enforce that the TF frame and the timestamp are copied - inliers.header = model.header = fromPCL(cloud->header); - - if (impl_.getModelType() < 0) { - NODELET_ERROR("[%s::input_normals_indices_callback] Model type not set!", getName().c_str()); - pub_indices_.publish(boost::make_shared(inliers)); - pub_model_.publish(boost::make_shared(model)); - return; - } - - if (!isValid(cloud)) { // || !isValid (cloud_normals, "normals")) - NODELET_ERROR("[%s::input_normals_indices_callback] Invalid input!", getName().c_str()); - pub_indices_.publish(boost::make_shared(inliers)); - pub_model_.publish(boost::make_shared(model)); - return; - } - // If indices are given, check if they are valid - if (indices && !isValid(indices)) { - NODELET_ERROR("[%s::input_normals_indices_callback] Invalid indices!", getName().c_str()); - pub_indices_.publish(boost::make_shared(inliers)); - pub_model_.publish(boost::make_shared(model)); - return; - } - - /// DEBUG - if (indices && !indices->header.frame_id.empty()) { - NODELET_DEBUG( - "[%s::input_normals_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and " - "frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and " - "frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and " - "frame %s on topic %s received.", - getName().c_str(), - cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( - cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( - "input").c_str(), - cloud_normals->width * cloud_normals->height, pcl::getFieldsList( - *cloud_normals).c_str(), fromPCL( - cloud_normals->header).stamp.toSec(), - cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str(), - indices->indices.size(), indices->header.stamp.toSec(), - indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); - } else { - NODELET_DEBUG( - "[%s::input_normals_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and " - "frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and " - "frame %s on topic %s received.", - getName().c_str(), - cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( - cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( - "input").c_str(), - cloud_normals->width * cloud_normals->height, pcl::getFieldsList( - *cloud_normals).c_str(), fromPCL( - cloud_normals->header).stamp.toSec(), - cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str()); - } - /// - - - // Extra checks for safety - int cloud_nr_points = cloud->width * cloud->height; - int cloud_normals_nr_points = cloud_normals->width * cloud_normals->height; - if (cloud_nr_points != cloud_normals_nr_points) { - NODELET_ERROR( - "[%s::input_normals_indices_callback] Number of points in the input dataset (%d) differs " - "from the number of points in the normals (%d)!", - getName().c_str(), cloud_nr_points, cloud_normals_nr_points); - pub_indices_.publish(boost::make_shared(inliers)); - pub_model_.publish(boost::make_shared(model)); - return; - } - - impl_.setInputCloud(pcl_ptr(cloud)); - impl_.setInputNormals(pcl_ptr(cloud_normals)); - - IndicesPtr indices_ptr; - if (indices && !indices->header.frame_id.empty()) { - indices_ptr.reset(new std::vector(indices->indices)); - } - - impl_.setIndices(indices_ptr); - - // Final check if the data is empty - // (remember that indices are set to the size of the data -- if indices* = NULL) - if (!cloud->points.empty()) { - pcl::PointIndices pcl_inliers; - pcl::ModelCoefficients pcl_model; - pcl_conversions::moveToPCL(inliers, pcl_inliers); - pcl_conversions::moveToPCL(model, pcl_model); - impl_.segment(pcl_inliers, pcl_model); - pcl_conversions::moveFromPCL(pcl_inliers, inliers); - pcl_conversions::moveFromPCL(pcl_model, model); - } - - // Check if we have enough inliers, clear inliers + model if not - if (static_cast(inliers.indices.size()) <= min_inliers_) { - inliers.indices.clear(); - model.values.clear(); - } - - // Publish - pub_indices_.publish(boost::make_shared(inliers)); - pub_model_.publish(boost::make_shared(model)); - NODELET_DEBUG( - "[%s::input_normals_callback] Published PointIndices with %zu values on topic %s, and " - "ModelCoefficients with %zu values on topic %s", - getName().c_str(), inliers.indices.size(), pnh_->resolveName("inliers").c_str(), - model.values.size(), pnh_->resolveName("model").c_str()); - if (inliers.indices.empty()) { - NODELET_WARN("[%s::input_indices_callback] No inliers found!", getName().c_str()); - } -} - -typedef pcl_ros::SACSegmentation SACSegmentation; -typedef pcl_ros::SACSegmentationFromNormals SACSegmentationFromNormals; -PLUGINLIB_EXPORT_CLASS(SACSegmentation, nodelet::Nodelet) -PLUGINLIB_EXPORT_CLASS(SACSegmentationFromNormals, nodelet::Nodelet) +// void +// pcl_ros::SACSegmentationFromNormals::onInit() +// { +// // Call the super onInit () +// PCLNodelet::onInit(); + +// // Enable the dynamic reconfigure service +// srv_ = boost::make_shared>(*pnh_); +// dynamic_reconfigure::Server::CallbackType f = boost::bind( +// &SACSegmentationFromNormals::config_callback, this, _1, _2); +// srv_->setCallback(f); + +// // Advertise the output topics +// pub_indices_ = advertise(*pnh_, "inliers", max_queue_size_); +// pub_model_ = advertise(*pnh_, "model", max_queue_size_); + +// // ---[ Mandatory parameters +// int model_type; +// if (!pnh_->getParam("model_type", model_type)) { +// NODELET_ERROR( +// "[%s::onInit] Need a 'model_type' parameter to be set before continuing!", +// getName().c_str()); +// return; +// } +// double threshold; // unused - set via dynamic reconfigure in the callback +// if (!pnh_->getParam("distance_threshold", threshold)) { +// NODELET_ERROR( +// "[%s::onInit] Need a 'distance_threshold' parameter to be set before continuing!", +// getName().c_str()); +// return; +// } + +// // ---[ Optional parameters +// int method_type = 0; +// pnh_->getParam("method_type", method_type); + +// XmlRpc::XmlRpcValue axis_param; +// pnh_->getParam("axis", axis_param); +// Eigen::Vector3f axis = Eigen::Vector3f::Zero(); + +// switch (axis_param.getType()) { +// case XmlRpc::XmlRpcValue::TypeArray: +// { +// if (axis_param.size() != 3) { +// NODELET_ERROR( +// "[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than " +// "required (3)!", +// getName().c_str(), axis_param.size()); +// return; +// } +// for (int i = 0; i < 3; ++i) { +// if (axis_param[i].getType() != XmlRpc::XmlRpcValue::TypeDouble) { +// NODELET_ERROR( +// "[%s::onInit] Need floating point values for 'axis' parameter.", +// getName().c_str()); +// return; +// } +// double value = axis_param[i]; axis[i] = value; +// } +// break; +// } +// default: +// { +// break; +// } +// } + +// // Initialize the random number generator +// srand(time(0)); + +// NODELET_DEBUG( +// "[%s::onInit] Nodelet successfully created with the following parameters:\n" +// " - model_type : %d\n" +// " - method_type : %d\n" +// " - model_threshold : %f\n" +// " - axis : [%f, %f, %f]\n", +// getName().c_str(), model_type, method_type, threshold, +// axis[0], axis[1], axis[2]); + +// // Set given parameters here +// impl_.setModelType(model_type); +// impl_.setMethodType(method_type); +// impl_.setAxis(axis); + +// onInitPostProcess(); +// } + +// ////////////////////////////////////////////////////////////////////////////////////////////// +// void +// pcl_ros::SACSegmentationFromNormals::subscribe() +// { +// // Subscribe to the input and normals using filters +// sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_); +// sub_normals_filter_.subscribe(*pnh_, "normals", max_queue_size_); + +// // Subscribe to an axis direction along which the model search is to be constrained (the first +// // 3 model coefficients will be checked) +// sub_axis_ = pnh_->subscribe("axis", 1, &SACSegmentationFromNormals::axis_callback, this); + +// if (approximate_sync_) { +// sync_input_normals_indices_a_ = +// boost::make_shared>>(max_queue_size_); +// } else { +// sync_input_normals_indices_e_ = +// boost::make_shared>>(max_queue_size_); +// } + +// // If we're supposed to look for PointIndices (indices) +// if (use_indices_) { +// // Subscribe to the input using a filter +// sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); + +// if (approximate_sync_) { +// sync_input_normals_indices_a_->connectInput( +// sub_input_filter_, sub_normals_filter_, +// sub_indices_filter_); +// } else { +// sync_input_normals_indices_e_->connectInput( +// sub_input_filter_, sub_normals_filter_, +// sub_indices_filter_); +// } +// } else { +// // Create a different callback for copying over the timestamp to fake indices +// sub_input_filter_.registerCallback(bind(&SACSegmentationFromNormals::input_callback, this, _1)); + +// if (approximate_sync_) { +// sync_input_normals_indices_a_->connectInput(sub_input_filter_, sub_normals_filter_, nf_); +// } else { +// sync_input_normals_indices_e_->connectInput(sub_input_filter_, sub_normals_filter_, nf_); +// } +// } + +// if (approximate_sync_) { +// sync_input_normals_indices_a_->registerCallback( +// bind( +// &SACSegmentationFromNormals:: +// input_normals_indices_callback, this, _1, _2, _3)); +// } else { +// sync_input_normals_indices_e_->registerCallback( +// bind( +// &SACSegmentationFromNormals:: +// input_normals_indices_callback, this, _1, _2, _3)); +// } +// } + +// ////////////////////////////////////////////////////////////////////////////////////////////// +// void +// pcl_ros::SACSegmentationFromNormals::unsubscribe() +// { +// sub_input_filter_.unsubscribe(); +// sub_normals_filter_.unsubscribe(); + +// sub_axis_.shutdown(); + +// if (use_indices_) { +// sub_indices_filter_.unsubscribe(); +// } +// } + +// ////////////////////////////////////////////////////////////////////////////////////////////// +// void +// pcl_ros::SACSegmentationFromNormals::axis_callback( +// const pcl_msgs::ModelCoefficientsConstPtr & model) +// { +// boost::mutex::scoped_lock lock(mutex_); + +// if (model->values.size() < 3) { +// NODELET_ERROR( +// "[%s::axis_callback] Invalid axis direction / model coefficients with %zu values sent on %s!", +// getName().c_str(), model->values.size(), pnh_->resolveName("axis").c_str()); +// return; +// } +// NODELET_DEBUG( +// "[%s::axis_callback] Received axis direction: %f %f %f", +// getName().c_str(), model->values[0], model->values[1], model->values[2]); + +// Eigen::Vector3f axis(model->values[0], model->values[1], model->values[2]); +// impl_.setAxis(axis); +// } + +// ////////////////////////////////////////////////////////////////////////////////////////////// +// void +// pcl_ros::SACSegmentationFromNormals::config_callback( +// SACSegmentationFromNormalsConfig & config, +// uint32_t level) +// { +// boost::mutex::scoped_lock lock(mutex_); + +// if (impl_.getDistanceThreshold() != config.distance_threshold) { +// impl_.setDistanceThreshold(config.distance_threshold); +// NODELET_DEBUG( +// "[%s::config_callback] Setting distance to model threshold to: %f.", +// getName().c_str(), config.distance_threshold); +// } +// // The maximum allowed difference between the model normal and the given axis _in radians_ +// if (impl_.getEpsAngle() != config.eps_angle) { +// impl_.setEpsAngle(config.eps_angle); +// NODELET_DEBUG( +// "[%s::config_callback] Setting new epsilon angle to model threshold to: %f (%f degrees).", +// getName().c_str(), config.eps_angle, config.eps_angle * 180.0 / M_PI); +// } + +// if (impl_.getMaxIterations() != config.max_iterations) { +// impl_.setMaxIterations(config.max_iterations); +// NODELET_DEBUG( +// "[%s::config_callback] Setting new maximum number of iterations to: %d.", +// getName().c_str(), config.max_iterations); +// } + +// // Number of inliers +// if (min_inliers_ != config.min_inliers) { +// min_inliers_ = config.min_inliers; +// NODELET_DEBUG( +// "[%s::config_callback] Setting new minimum number of inliers to: %d.", +// getName().c_str(), min_inliers_); +// } + + +// if (impl_.getProbability() != config.probability) { +// impl_.setProbability(config.probability); +// NODELET_DEBUG( +// "[%s::config_callback] Setting new probability to: %f.", +// getName().c_str(), config.probability); +// } + +// if (impl_.getOptimizeCoefficients() != config.optimize_coefficients) { +// impl_.setOptimizeCoefficients(config.optimize_coefficients); +// NODELET_DEBUG( +// "[%s::config_callback] Setting coefficient optimization to: %s.", +// getName().c_str(), (config.optimize_coefficients) ? "true" : "false"); +// } + +// if (impl_.getNormalDistanceWeight() != config.normal_distance_weight) { +// impl_.setNormalDistanceWeight(config.normal_distance_weight); +// NODELET_DEBUG( +// "[%s::config_callback] Setting new distance weight to: %f.", +// getName().c_str(), config.normal_distance_weight); +// } + +// double radius_min, radius_max; +// impl_.getRadiusLimits(radius_min, radius_max); +// if (radius_min != config.radius_min) { +// radius_min = config.radius_min; +// NODELET_DEBUG( +// "[%s::config_callback] Setting minimum allowable model radius to: %f.", +// getName().c_str(), radius_min); +// impl_.setRadiusLimits(radius_min, radius_max); +// } +// if (radius_max != config.radius_max) { +// radius_max = config.radius_max; +// NODELET_DEBUG( +// "[%s::config_callback] Setting maximum allowable model radius to: %f.", +// getName().c_str(), radius_max); +// impl_.setRadiusLimits(radius_min, radius_max); +// } +// } + +// ////////////////////////////////////////////////////////////////////////////////////////////// +// void +// pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback( +// const PointCloudConstPtr & cloud, +// const PointCloudNConstPtr & cloud_normals, +// const PointIndicesConstPtr & indices +// ) +// { +// boost::mutex::scoped_lock lock(mutex_); + +// PointIndices inliers; +// ModelCoefficients model; +// // Enforce that the TF frame and the timestamp are copied +// inliers.header = model.header = fromPCL(cloud->header); + +// if (impl_.getModelType() < 0) { +// NODELET_ERROR("[%s::input_normals_indices_callback] Model type not set!", getName().c_str()); +// pub_indices_.publish(boost::make_shared(inliers)); +// pub_model_.publish(boost::make_shared(model)); +// return; +// } + +// if (!isValid(cloud)) { // || !isValid (cloud_normals, "normals")) +// NODELET_ERROR("[%s::input_normals_indices_callback] Invalid input!", getName().c_str()); +// pub_indices_.publish(boost::make_shared(inliers)); +// pub_model_.publish(boost::make_shared(model)); +// return; +// } +// // If indices are given, check if they are valid +// if (indices && !isValid(indices)) { +// NODELET_ERROR("[%s::input_normals_indices_callback] Invalid indices!", getName().c_str()); +// pub_indices_.publish(boost::make_shared(inliers)); +// pub_model_.publish(boost::make_shared(model)); +// return; +// } + +// /// DEBUG +// if (indices && !indices->header.frame_id.empty()) { +// NODELET_DEBUG( +// "[%s::input_normals_indices_callback]\n" +// " - PointCloud with %d data points (%s), stamp %f, and " +// "frame %s on topic %s received.\n" +// " - PointCloud with %d data points (%s), stamp %f, and " +// "frame %s on topic %s received.\n" +// " - PointIndices with %zu values, stamp %f, and " +// "frame %s on topic %s received.", +// getName().c_str(), +// cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( +// cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( +// "input").c_str(), +// cloud_normals->width * cloud_normals->height, pcl::getFieldsList( +// *cloud_normals).c_str(), fromPCL( +// cloud_normals->header).stamp.toSec(), +// cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str(), +// indices->indices.size(), indices->header.stamp.toSec(), +// indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); +// } else { +// NODELET_DEBUG( +// "[%s::input_normals_indices_callback]\n" +// " - PointCloud with %d data points (%s), stamp %f, and " +// "frame %s on topic %s received.\n" +// " - PointCloud with %d data points (%s), stamp %f, and " +// "frame %s on topic %s received.", +// getName().c_str(), +// cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( +// cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( +// "input").c_str(), +// cloud_normals->width * cloud_normals->height, pcl::getFieldsList( +// *cloud_normals).c_str(), fromPCL( +// cloud_normals->header).stamp.toSec(), +// cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str()); +// } +// /// + + +// // Extra checks for safety +// int cloud_nr_points = cloud->width * cloud->height; +// int cloud_normals_nr_points = cloud_normals->width * cloud_normals->height; +// if (cloud_nr_points != cloud_normals_nr_points) { +// NODELET_ERROR( +// "[%s::input_normals_indices_callback] Number of points in the input dataset (%d) differs " +// "from the number of points in the normals (%d)!", +// getName().c_str(), cloud_nr_points, cloud_normals_nr_points); +// pub_indices_.publish(boost::make_shared(inliers)); +// pub_model_.publish(boost::make_shared(model)); +// return; +// } + +// impl_.setInputCloud(pcl_ptr(cloud)); +// impl_.setInputNormals(pcl_ptr(cloud_normals)); + +// IndicesPtr indices_ptr; +// if (indices && !indices->header.frame_id.empty()) { +// indices_ptr.reset(new std::vector(indices->indices)); +// } + +// impl_.setIndices(indices_ptr); + +// // Final check if the data is empty +// // (remember that indices are set to the size of the data -- if indices* = NULL) +// if (!cloud->points.empty()) { +// pcl::PointIndices pcl_inliers; +// pcl::ModelCoefficients pcl_model; +// pcl_conversions::moveToPCL(inliers, pcl_inliers); +// pcl_conversions::moveToPCL(model, pcl_model); +// impl_.segment(pcl_inliers, pcl_model); +// pcl_conversions::moveFromPCL(pcl_inliers, inliers); +// pcl_conversions::moveFromPCL(pcl_model, model); +// } + +// // Check if we have enough inliers, clear inliers + model if not +// if (static_cast(inliers.indices.size()) <= min_inliers_) { +// inliers.indices.clear(); +// model.values.clear(); +// } + +// // Publish +// pub_indices_.publish(boost::make_shared(inliers)); +// pub_model_.publish(boost::make_shared(model)); +// NODELET_DEBUG( +// "[%s::input_normals_callback] Published PointIndices with %zu values on topic %s, and " +// "ModelCoefficients with %zu values on topic %s", +// getName().c_str(), inliers.indices.size(), pnh_->resolveName("inliers").c_str(), +// model.values.size(), pnh_->resolveName("model").c_str()); +// if (inliers.indices.empty()) { +// NODELET_WARN("[%s::input_indices_callback] No inliers found!", getName().c_str()); +// } +// } +} // namespace pcl_ros + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::SACSegmentation)