diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 258f23666..553dbb069 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -26,6 +26,7 @@ find_package(tf2_ros REQUIRED) set(dependencies pcl_conversions rclcpp + rclcpp_components sensor_msgs geometry_msgs tf2 @@ -80,19 +81,48 @@ ament_target_dependencies(pcl_ros_tf # # ### Declare the pcl_ros_filters library -#add_library(pcl_ros_filters -# src/pcl_ros/filters/extract_indices.cpp -# src/pcl_ros/filters/filter.cpp -# src/pcl_ros/filters/passthrough.cpp -# src/pcl_ros/filters/project_inliers.cpp -# src/pcl_ros/filters/radius_outlier_removal.cpp -# src/pcl_ros/filters/statistical_outlier_removal.cpp -# src/pcl_ros/filters/voxel_grid.cpp -# src/pcl_ros/filters/crop_box.cpp -#) -#target_link_libraries(pcl_ros_filters pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) -#add_dependencies(pcl_ros_filters ${PROJECT_NAME}_gencfg) -#class_loader_hide_library_symbols(pcl_ros_filters) +add_library(pcl_ros_filters SHARED + src/pcl_ros/filters/extract_indices.cpp + src/pcl_ros/filters/filter.cpp + src/pcl_ros/filters/passthrough.cpp + src/pcl_ros/filters/project_inliers.cpp + src/pcl_ros/filters/radius_outlier_removal.cpp + src/pcl_ros/filters/statistical_outlier_removal.cpp + src/pcl_ros/filters/voxel_grid.cpp + src/pcl_ros/filters/crop_box.cpp +) +target_link_libraries(pcl_ros_filters pcl_ros_tf ${PCL_LIBRARIES}) +ament_target_dependencies(pcl_ros_filters ${dependencies}) +rclcpp_components_register_node(pcl_ros_filters + PLUGIN "pcl_ros::ExtractIndices" + EXECUTABLE filter_extract_indices_node +) +rclcpp_components_register_node(pcl_ros_filters + PLUGIN "pcl_ros::PassThrough" + EXECUTABLE filter_passthrough_node +) +rclcpp_components_register_node(pcl_ros_filters + PLUGIN "pcl_ros::ProjectInliers" + EXECUTABLE filter_project_inliers_node +) +rclcpp_components_register_node(pcl_ros_filters + PLUGIN "pcl_ros::RadiusOutlierRemoval" + EXECUTABLE filter_radius_outlier_removal_node +) +rclcpp_components_register_node(pcl_ros_filters + PLUGIN "pcl_ros::StatisticalOutlierRemoval" + EXECUTABLE filter_statistical_outlier_removal_node +) +rclcpp_components_register_node(pcl_ros_filters + PLUGIN "pcl_ros::CropBox" + EXECUTABLE filter_crop_box_node +) +rclcpp_components_register_node(pcl_ros_filters + PLUGIN "pcl_ros::VoxelGrid" + EXECUTABLE filter_voxel_grid_node +) +# add_dependencies(pcl_ros_filters ${PROJECT_NAME}_gencfg) +class_loader_hide_library_symbols(pcl_ros_filters) # ### Declare the pcl_ros_segmentation library #add_library (pcl_ros_segmentation @@ -163,6 +193,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(tests/filters) #add_rostest_gtest(test_tf_message_filter_pcl tests/test_tf_message_filter_pcl.launch src/test/test_tf_message_filter_pcl.cpp) #target_link_libraries(test_tf_message_filter_pcl ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) #add_rostest(samples/pcl_ros/features/sample_normal_3d.launch ARGS gui:=false) @@ -184,7 +215,7 @@ install( pcd_to_pointcloud_lib # pcl_ros_io # pcl_ros_features -# pcl_ros_filters + pcl_ros_filters # pcl_ros_surface # pcl_ros_segmentation # pointcloud_to_pcd diff --git a/pcl_ros/include/pcl_ros/filters/crop_box.hpp b/pcl_ros/include/pcl_ros/filters/crop_box.hpp index 3665521a6..36733776b 100644 --- a/pcl_ros/include/pcl_ros/filters/crop_box.hpp +++ b/pcl_ros/include/pcl_ros/filters/crop_box.hpp @@ -41,11 +41,9 @@ // PCL includes #include +#include #include "pcl_ros/filters/filter.hpp" -// Dynamic reconfigure -#include "pcl_ros/CropBoxConfig.hpp" - namespace pcl_ros { /** \brief @b CropBox is a filter that allows the user to filter all the data inside of a given box. @@ -57,9 +55,6 @@ namespace pcl_ros class CropBox : public Filter { protected: - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr> srv_; // TODO(xxx) - /** \brief Call the actual filter. * \param input the input point cloud dataset * \param indices the input set of indices to use from \a input @@ -67,32 +62,16 @@ class CropBox : public Filter */ inline void filter( - const PointCloud2::ConstPtr & input, const IndicesPtr & indices, - PointCloud2 & output) - { - boost::mutex::scoped_lock lock(mutex_); - pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); - 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); - } + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) override; - /** \brief Child initialization routine. - * \param nh ROS node handle - * \param has_service set to true if the child has a Dynamic Reconfigure service + /** \brief Parameter callback + * \param params parameter values to set */ - bool - child_init(ros::NodeHandle & nh, bool & has_service); + rcl_interfaces::msg::SetParametersResult + config_callback(const std::vector & params); - /** \brief Dynamic reconfigure service callback. - * \param config the dynamic reconfigure CropBoxConfig object - * \param level the dynamic reconfigure level - */ - void - config_callback(pcl_ros::CropBoxConfig & config, uint32_t level); + OnSetParametersCallbackHandle::SharedPtr callback_handle_; private: /** \brief The PCL filter implementation used. */ @@ -100,6 +79,8 @@ class CropBox : public Filter public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit CropBox(const rclcpp::NodeOptions & options); }; } // 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 95740f600..8f4bc55aa 100644 --- a/pcl_ros/include/pcl_ros/filters/extract_indices.hpp +++ b/pcl_ros/include/pcl_ros/filters/extract_indices.hpp @@ -40,9 +40,8 @@ // PCL includes #include - +#include #include "pcl_ros/filters/filter.hpp" -#include "pcl_ros/ExtractIndicesConfig.hpp" namespace pcl_ros { @@ -53,9 +52,6 @@ namespace pcl_ros class ExtractIndices : public Filter { protected: - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr> srv_; - /** \brief Call the actual filter. * \param input the input point cloud dataset * \param indices the input set of indices to use from \a input @@ -63,29 +59,16 @@ class ExtractIndices : public Filter */ inline void filter( - const PointCloud2::ConstPtr & input, const IndicesPtr & indices, - PointCloud2 & output) - { - boost::mutex::scoped_lock lock(mutex_); - pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); - 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); - } + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) override; - /** \brief Child initialization routine. - * \param nh ROS node handle - * \param has_service set to true if the child has a Dynamic Reconfigure service + /** \brief Parameter callback + * \param params parameter values to set */ - virtual bool - child_init(ros::NodeHandle & nh, bool & has_service); + rcl_interfaces::msg::SetParametersResult + config_callback(const std::vector & params); - /** \brief Dynamic reconfigure service callback. */ - void - config_callback(pcl_ros::ExtractIndicesConfig & config, uint32_t level); + OnSetParametersCallbackHandle::SharedPtr callback_handle_; private: /** \brief The PCL filter implementation used. */ @@ -93,6 +76,8 @@ class ExtractIndices : public Filter public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit ExtractIndices(const rclcpp::NodeOptions & options); }; } // namespace pcl_ros diff --git a/pcl_ros/include/pcl_ros/filters/filter.hpp b/pcl_ros/include/pcl_ros/filters/filter.hpp index e589c0cb7..08994bb85 100644 --- a/pcl_ros/include/pcl_ros/filters/filter.hpp +++ b/pcl_ros/include/pcl_ros/filters/filter.hpp @@ -38,11 +38,10 @@ #ifndef PCL_ROS__FILTERS__FILTER_HPP_ #define PCL_ROS__FILTERS__FILTER_HPP_ -#include -#include +#include #include -#include "pcl_ros/pcl_nodelet.hpp" -#include "pcl_ros/FilterConfig.hpp" +#include +#include "pcl_ros/pcl_node.hpp" namespace pcl_ros { @@ -52,19 +51,30 @@ namespace sync_policies = message_filters::sync_policies; * applicable to all filters are defined here as static methods. * \author Radu Bogdan Rusu */ -class Filter : public PCLNodelet +class Filter : public PCLNode { public: - typedef sensor_msgs::PointCloud2 PointCloud2; + typedef sensor_msgs::msg::PointCloud2 PointCloud2; typedef pcl::IndicesPtr IndicesPtr; typedef pcl::IndicesConstPtr IndicesConstPtr; - Filter() {} + /** \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. */ - ros::Subscriber sub_input_; + rclcpp::Subscription::SharedPtr sub_input_; message_filters::Subscriber sub_input_filter_; @@ -96,18 +106,7 @@ class Filter : public PCLNodelet std::string tf_output_frame_; /** \brief Internal mutex. */ - boost::mutex mutex_; - - /** \brief Child initialization routine. - * \param nh ROS node handle - * \param has_service set to true if the child has a Dynamic Reconfigure service - */ - virtual bool - child_init(ros::NodeHandle & nh, bool & has_service) - { - has_service = false; - return true; - } + std::mutex mutex_; /** \brief Virtual abstract filter method. To be implemented by every child. * \param input the input point cloud dataset. @@ -116,7 +115,7 @@ class Filter : public PCLNodelet */ virtual void filter( - const PointCloud2::ConstPtr & input, const IndicesPtr & indices, + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, PointCloud2 & output) = 0; /** \brief Lazy transport subscribe routine. */ @@ -127,36 +126,38 @@ class Filter : public PCLNodelet virtual void unsubscribe(); - /** \brief Nodelet initialization routine. */ + /** \brief Create publishers for output PointCloud2 data. */ virtual void - onInit(); + 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::ConstPtr & input, const IndicesPtr & indices); + computePublish(const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices); private: - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr> srv_; + /** \brief Pointer to parameters callback handle. */ + OnSetParametersCallbackHandle::SharedPtr callback_handle_; /** \brief Synchronized input, and indices.*/ - boost::shared_ptr>> sync_input_indices_e_; - boost::shared_ptr>> sync_input_indices_a_; - /** \brief Dynamic reconfigure service callback. */ - virtual void - config_callback(pcl_ros::FilterConfig & config, uint32_t level); + /** \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::ConstPtr & cloud, - const PointIndicesConstPtr & indices); + const PointCloud2::ConstSharedPtr & cloud, + const PointIndices::ConstSharedPtr & indices); public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/pcl_ros/include/pcl_ros/filters/passthrough.hpp b/pcl_ros/include/pcl_ros/filters/passthrough.hpp index f85d38102..dd18bec84 100644 --- a/pcl_ros/include/pcl_ros/filters/passthrough.hpp +++ b/pcl_ros/include/pcl_ros/filters/passthrough.hpp @@ -40,6 +40,7 @@ // PCL includes #include +#include #include "pcl_ros/filters/filter.hpp" namespace pcl_ros @@ -51,9 +52,6 @@ namespace pcl_ros class PassThrough : public Filter { protected: - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr> srv_; - /** \brief Call the actual filter. * \param input the input point cloud dataset * \param indices the input set of indices to use from \a input @@ -61,32 +59,16 @@ class PassThrough : public Filter */ inline void filter( - const PointCloud2::ConstPtr & input, const IndicesPtr & indices, - PointCloud2 & output) - { - boost::mutex::scoped_lock lock(mutex_); - pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); - 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); - } + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) override; - /** \brief Child initialization routine. - * \param nh ROS node handle - * \param has_service set to true if the child has a Dynamic Reconfigure service + /** \brief Parameter callback + * \param params parameter values to set */ - bool - child_init(ros::NodeHandle & nh, bool & has_service); + rcl_interfaces::msg::SetParametersResult + config_callback(const std::vector & params); - /** \brief Dynamic reconfigure service callback. - * \param config the dynamic reconfigure FilterConfig object - * \param level the dynamic reconfigure level - */ - void - config_callback(pcl_ros::FilterConfig & config, uint32_t level); + OnSetParametersCallbackHandle::SharedPtr callback_handle_; private: /** \brief The PCL filter implementation used. */ @@ -94,6 +76,8 @@ class PassThrough : public Filter public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit PassThrough(const rclcpp::NodeOptions & options); }; } // 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 3c25846e5..639289c4d 100644 --- a/pcl_ros/include/pcl_ros/filters/project_inliers.hpp +++ b/pcl_ros/include/pcl_ros/filters/project_inliers.hpp @@ -38,8 +38,10 @@ #ifndef PCL_ROS__FILTERS__PROJECT_INLIERS_HPP_ #define PCL_ROS__FILTERS__PROJECT_INLIERS_HPP_ +// PCL includes #include #include +#include #include "pcl_ros/filters/filter.hpp" @@ -55,8 +57,7 @@ namespace sync_policies = message_filters::sync_policies; class ProjectInliers : public Filter { public: - ProjectInliers() - : model_() {} + explicit ProjectInliers(const rclcpp::NodeOptions & options); protected: /** \brief Call the actual filter. @@ -66,20 +67,8 @@ class ProjectInliers : public Filter */ inline void filter( - const PointCloud2::ConstPtr & input, const IndicesPtr & indices, - PointCloud2 & output) - { - pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); - pcl_conversions::toPCL(*(input), *(pcl_input)); - impl_.setInputCloud(pcl_input); - impl_.setIndices(indices); - pcl::ModelCoefficients::Ptr pcl_model(new pcl::ModelCoefficients); - pcl_conversions::toPCL(*(model_), *(pcl_model)); - impl_.setModelCoefficients(pcl_model); - pcl::PCLPointCloud2 pcl_output; - impl_.filter(pcl_output); - pcl_conversions::moveFromPCL(pcl_output, output); - } + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) override; private: /** \brief A pointer to the vector of model coefficients. */ @@ -89,25 +78,20 @@ class ProjectInliers : public Filter message_filters::Subscriber sub_model_; /** \brief Synchronized input, indices, and model coefficients.*/ - boost::shared_ptr>> sync_input_indices_model_e_; - boost::shared_ptr>> sync_input_indices_model_a_; /** \brief The PCL filter implementation used. */ pcl::ProjectInliers impl_; - /** \brief Nodelet initialization routine. */ - virtual void - onInit(); - - /** \brief NodeletLazy connection routine. */ - void subscribe(); - void unsubscribe(); + void subscribe() override; + void unsubscribe() override; /** \brief PointCloud2 + Indices + Model data callback. */ void input_indices_model_callback( - const PointCloud2::ConstPtr & cloud, + const PointCloud2::ConstSharedPtr & cloud, const PointIndicesConstPtr & indices, const ModelCoefficientsConstPtr & model); 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 420f4df5a..5f7333df4 100644 --- a/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp +++ b/pcl_ros/include/pcl_ros/filters/radius_outlier_removal.hpp @@ -40,11 +40,9 @@ // PCL includes #include +#include #include "pcl_ros/filters/filter.hpp" -// Dynamic reconfigure -#include "pcl_ros/RadiusOutlierRemovalConfig.hpp" - namespace pcl_ros { /** \brief @b RadiusOutlierRemoval is a simple filter that removes outliers if the number of neighbors in a certain @@ -55,9 +53,6 @@ namespace pcl_ros class RadiusOutlierRemoval : public Filter { protected: - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr> srv_; - /** \brief Call the actual filter. * \param input the input point cloud dataset * \param indices the input set of indices to use from \a input @@ -65,29 +60,16 @@ class RadiusOutlierRemoval : public Filter */ inline void filter( - const PointCloud2::ConstPtr & input, const IndicesPtr & indices, - PointCloud2 & output) - { - pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); - 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); - } + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) override; - /** \brief Child initialization routine. - * \param nh ROS node handle - * \param has_service set to true if the child has a Dynamic Reconfigure service + /** \brief Parameter callback + * \param params parameter values to set */ - virtual inline bool child_init(ros::NodeHandle & nh, bool & has_service); + rcl_interfaces::msg::SetParametersResult + config_callback(const std::vector & params); - /** \brief Dynamic reconfigure callback - * \param config the config object - * \param level the dynamic reconfigure level - */ - void config_callback(pcl_ros::RadiusOutlierRemovalConfig & config, uint32_t level); + OnSetParametersCallbackHandle::SharedPtr callback_handle_; private: /** \brief The PCL filter implementation used. */ @@ -95,6 +77,8 @@ class RadiusOutlierRemoval : public Filter public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit RadiusOutlierRemoval(const rclcpp::NodeOptions & options); }; } // 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 80f4ea64f..f8e14a2ba 100644 --- a/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp +++ b/pcl_ros/include/pcl_ros/filters/statistical_outlier_removal.hpp @@ -40,11 +40,9 @@ // PCL includes #include +#include #include "pcl_ros/filters/filter.hpp" -// Dynamic reconfigure -#include "pcl_ros/StatisticalOutlierRemovalConfig.hpp" - namespace pcl_ros { /** \brief @b StatisticalOutlierRemoval uses point neighborhood statistics to filter outlier data. For more @@ -61,9 +59,6 @@ namespace pcl_ros class StatisticalOutlierRemoval : public Filter { protected: - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr> srv_; - /** \brief Call the actual filter. * \param input the input point cloud dataset * \param indices the input set of indices to use from \a input @@ -71,30 +66,16 @@ class StatisticalOutlierRemoval : public Filter */ inline void filter( - const PointCloud2::ConstPtr & input, const IndicesPtr & indices, - PointCloud2 & output) - { - boost::mutex::scoped_lock lock(mutex_); - pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); - 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); - } + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) override; - /** \brief Child initialization routine. - * \param nh ROS node handle - * \param has_service set to true if the child has a Dynamic Reconfigure service + /** \brief Parameter callback + * \param params parameter values to set */ - bool child_init(ros::NodeHandle & nh, bool & has_service); + rcl_interfaces::msg::SetParametersResult + config_callback(const std::vector & params); - /** \brief Dynamic reconfigure callback - * \param config the config object - * \param level the dynamic reconfigure level - */ - void config_callback(pcl_ros::StatisticalOutlierRemovalConfig & config, uint32_t level); + OnSetParametersCallbackHandle::SharedPtr callback_handle_; private: /** \brief The PCL filter implementation used. */ @@ -102,6 +83,8 @@ class StatisticalOutlierRemoval : public Filter public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit StatisticalOutlierRemoval(const rclcpp::NodeOptions & options); }; } // 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 54c7c4596..4b51113c7 100644 --- a/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp +++ b/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp @@ -40,11 +40,9 @@ // PCL includes #include +#include #include "pcl_ros/filters/filter.hpp" -// Dynamic reconfigure -#include "pcl_ros/VoxelGridConfig.hpp" - namespace pcl_ros { /** \brief @b VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data. @@ -53,38 +51,32 @@ namespace pcl_ros class VoxelGrid : public Filter { protected: - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr> srv_; - - /** \brief The PCL filter implementation used. */ - pcl::VoxelGrid impl_; - /** \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 */ - virtual void + inline void filter( - const PointCloud2::ConstPtr & input, const IndicesPtr & indices, - PointCloud2 & output); + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) override; - /** \brief Child initialization routine. - * \param nh ROS node handle - * \param has_service set to true if the child has a Dynamic Reconfigure service + /** \brief Parameter callback + * \param params parameter values to set */ - bool - child_init(ros::NodeHandle & nh, bool & has_service); + rcl_interfaces::msg::SetParametersResult + config_callback(const std::vector & params); - /** \brief Dynamic reconfigure callback - * \param config the config object - * \param level the dynamic reconfigure level - */ - void - config_callback(pcl_ros::VoxelGridConfig & config, uint32_t level); + OnSetParametersCallbackHandle::SharedPtr callback_handle_; + +private: + /** \brief The PCL filter implementation used. */ + pcl::VoxelGrid impl_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit VoxelGrid(const rclcpp::NodeOptions & options); }; } // namespace pcl_ros diff --git a/pcl_ros/include/pcl_ros/pcl_node.hpp b/pcl_ros/include/pcl_ros/pcl_node.hpp new file mode 100644 index 000000000..4567bc5c2 --- /dev/null +++ b/pcl_ros/include/pcl_ros/pcl_node.hpp @@ -0,0 +1,295 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, 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: pcl_node.h 33238 2010-03-11 00:46:58Z rusu $ + * + */ + +/** + +\author Radu Bogdan Rusu + +**/ + +#ifndef PCL_ROS__PCL_NODE_HPP_ +#define PCL_ROS__PCL_NODE_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +// #include "pcl_ros/point_cloud.hpp" + +using pcl_conversions::fromPCL; + +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 +{ +public: + typedef sensor_msgs::msg::PointCloud2 PointCloud2; + + typedef pcl::PointCloud PointCloud; + typedef PointCloud::Ptr PointCloudPtr; + typedef PointCloud::ConstPtr PointCloudConstPtr; + + typedef pcl_msgs::msg::PointIndices PointIndices; + typedef PointIndices::SharedPtr PointIndicesPtr; + typedef PointIndices::ConstSharedPtr PointIndicesConstPtr; + + typedef pcl_msgs::msg::ModelCoefficients ModelCoefficients; + typedef ModelCoefficients::SharedPtr ModelCoefficientsPtr; + typedef ModelCoefficients::ConstSharedPtr ModelCoefficientsConstPtr; + + typedef pcl::IndicesPtr IndicesPtr; + typedef pcl::IndicesConstPtr IndicesConstPtr; + + /** \brief Empty constructor. */ + PCLNode(std::string node_name, const rclcpp::NodeOptions & options) + : rclcpp::Node(node_name, options), + use_indices_(false), transient_local_indices_(false), + max_queue_size_(3), approximate_sync_(false), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_) + { + { + rcl_interfaces::msg::ParameterDescriptor desc; + desc.name = "max_queue_size"; + desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + desc.description = "QoS History depth"; + desc.read_only = true; + max_queue_size_ = declare_parameter(desc.name, max_queue_size_, desc); + } + + { + rcl_interfaces::msg::ParameterDescriptor desc; + desc.name = "use_indices"; + desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + desc.description = "Only process a subset of the point cloud from an indices topic"; + desc.read_only = true; + use_indices_ = declare_parameter(desc.name, use_indices_, 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); + } + + { + rcl_interfaces::msg::ParameterDescriptor desc; + desc.name = "approximate_sync"; + desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + 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); + } + + RCLCPP_DEBUG( + this->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", + (approximate_sync_) ? "true" : "false", + (use_indices_) ? "true" : "false", + (transient_local_indices_) ? "true" : "false", + max_queue_size_); + } + +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 True if we use an approximate time synchronizer + * versus an exact one (false by default). + **/ + bool approximate_sync_; + + /** \brief TF listener object. */ + 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") + { + 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; + } + 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") + { + 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; + } + 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") + { + /*if (indices->indices.empty ()) + { + 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; + } + + /** \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") + { + /*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; + } + + /** \brief Lazy transport subscribe/unsubscribe routine. + * It is optional for backward compatibility. + **/ + virtual void subscribe() {} + virtual void unsubscribe() {} + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; +} // namespace pcl_ros + +#endif // PCL_ROS__PCL_NODE_HPP_ diff --git a/pcl_ros/src/pcl_ros/filters/crop_box.cpp b/pcl_ros/src/pcl_ros/filters/crop_box.cpp index bcba2a987..2c982a91b 100644 --- a/pcl_ros/src/pcl_ros/filters/crop_box.cpp +++ b/pcl_ros/src/pcl_ros/filters/crop_box.cpp @@ -37,86 +37,216 @@ */ #include "pcl_ros/filters/crop_box.hpp" -#include -////////////////////////////////////////////////////////////////////////////////////////////// -bool -pcl_ros::CropBox::child_init(ros::NodeHandle & nh, bool & has_service) +pcl_ros::CropBox::CropBox(const rclcpp::NodeOptions & options) +: Filter("CropBoxNode", options) { - // Enable the dynamic reconfigure service - has_service = true; - srv_ = boost::make_shared>(nh); - dynamic_reconfigure::Server::CallbackType f = boost::bind( - &CropBox::config_callback, this, _1, _2); - srv_->setCallback(f); - - return true; + // 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; + min_x_desc.description = + "Minimum x value below which points will be removed"; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = -1000.0; + float_range.to_value = 1000.0; + min_x_desc.floating_point_range.push_back(float_range); + } + declare_parameter(min_x_desc.name, rclcpp::ParameterValue(-1.0), min_x_desc); + + rcl_interfaces::msg::ParameterDescriptor max_x_desc; + max_x_desc.name = "max_x"; + max_x_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + max_x_desc.description = + "Maximum x value above which points will be removed"; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = -1000.0; + float_range.to_value = 1000.0; + max_x_desc.floating_point_range.push_back(float_range); + } + declare_parameter(max_x_desc.name, rclcpp::ParameterValue(1.0), max_x_desc); + + rcl_interfaces::msg::ParameterDescriptor min_y_desc; + min_y_desc.name = "min_y"; + min_y_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + min_y_desc.description = + "Minimum y value below which points will be removed"; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = -1000.0; + float_range.to_value = 1000.0; + min_y_desc.floating_point_range.push_back(float_range); + } + declare_parameter(min_y_desc.name, rclcpp::ParameterValue(-1.0), min_y_desc); + + rcl_interfaces::msg::ParameterDescriptor max_y_desc; + max_y_desc.name = "max_y"; + max_y_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + max_y_desc.description = + "Maximum y value above which points will be removed"; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = -1000.0; + float_range.to_value = 1000.0; + max_y_desc.floating_point_range.push_back(float_range); + } + declare_parameter(max_y_desc.name, rclcpp::ParameterValue(1.0), max_y_desc); + + rcl_interfaces::msg::ParameterDescriptor min_z_desc; + min_z_desc.name = "min_z"; + min_z_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + min_z_desc.description = + "Minimum z value below which points will be removed"; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = -1000.0; + float_range.to_value = 1000.0; + min_z_desc.floating_point_range.push_back(float_range); + } + declare_parameter(min_z_desc.name, rclcpp::ParameterValue(-1.0), min_z_desc); + + rcl_interfaces::msg::ParameterDescriptor max_z_desc; + max_z_desc.name = "max_z"; + max_z_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + max_z_desc.description = + "Maximum z value above which points will be removed"; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = -1000.0; + float_range.to_value = 1000.0; + max_z_desc.floating_point_range.push_back(float_range); + } + declare_parameter(max_z_desc.name, rclcpp::ParameterValue(1.0), max_z_desc); + + rcl_interfaces::msg::ParameterDescriptor keep_organized_desc; + keep_organized_desc.name = "keep_organized"; + keep_organized_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + keep_organized_desc.description = + "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); + + rcl_interfaces::msg::ParameterDescriptor negative_desc; + negative_desc.name = "negative"; + negative_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + 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::config_callback(pcl_ros::CropBoxConfig & config, uint32_t level) +pcl_ros::CropBox::filter( + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) +{ + std::lock_guard lock(mutex_); + pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); + 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::CropBox::config_callback(const std::vector & params) { - boost::mutex::scoped_lock lock(mutex_); + std::lock_guard lock(mutex_); Eigen::Vector4f min_point, max_point; min_point = impl_.getMin(); max_point = impl_.getMax(); - Eigen::Vector4f new_min_point, new_max_point; - new_min_point << config.min_x, config.min_y, config.min_z, 0.0; - new_max_point << config.max_x, config.max_y, config.max_z, 0.0; - - // Check the current values for minimum point - if (min_point != new_min_point) { - NODELET_DEBUG( - "[%s::config_callback] Setting the minimum point to: %f %f %f.", - getName().c_str(), new_min_point(0), new_min_point(1), new_min_point(2)); - // Set the filter min point if different - impl_.setMin(new_min_point); - } - // Check the current values for the maximum point - if (max_point != new_max_point) { - NODELET_DEBUG( - "[%s::config_callback] Setting the maximum point to: %f %f %f.", - getName().c_str(), new_max_point(0), new_max_point(1), new_max_point(2)); - // Set the filter max point if different - impl_.setMax(new_max_point); + for (const rclcpp::Parameter & param : params) { + if (param.get_name() == "min_x") { + min_point(0) = param.as_double(); + } + if (param.get_name() == "max_x") { + max_point(0) = param.as_double(); + } + if (param.get_name() == "min_y") { + min_point(1) = param.as_double(); + } + if (param.get_name() == "max_y") { + max_point(1) = param.as_double(); + } + if (param.get_name() == "min_z") { + min_point(2) = param.as_double(); + } + if (param.get_name() == "max_z") { + max_point(2) = param.as_double(); + } + if (param.get_name() == "negative") { + // Check the current value for the negative flag + if (impl_.getNegative() != param.as_bool()) { + RCLCPP_DEBUG( + get_logger(), "Setting the filter negative flag to: %s.", + param.as_bool() ? "true" : "false"); + // Call the virtual method in the child + impl_.setNegative(param.as_bool()); + } + } + if (param.get_name() == "keep_organized") { + // Check the current value for keep_organized + if (impl_.getKeepOrganized() != param.as_bool()) { + RCLCPP_DEBUG( + get_logger(), "Setting the filter keep_organized value to: %s.", + param.as_bool() ? "true" : "false"); + // Call the virtual method in the child + impl_.setKeepOrganized(param.as_bool()); + } + } } - // Check the current value for keep_organized - if (impl_.getKeepOrganized() != config.keep_organized) { - NODELET_DEBUG( - "[%s::config_callback] Setting the filter keep_organized value to: %s.", - getName().c_str(), config.keep_organized ? "true" : "false"); - // Call the virtual method in the child - impl_.setKeepOrganized(config.keep_organized); + // Check the current values for minimum point + if (min_point != impl_.getMin()) { + RCLCPP_DEBUG( + get_logger(), "Setting the minimum point to: %f %f %f.", + min_point(0), min_point(1), min_point(2)); + impl_.setMin(min_point); } - // Check the current value for the negative flag - if (impl_.getNegative() != config.negative) { - NODELET_DEBUG( - "[%s::config_callback] Setting the filter negative flag to: %s.", - getName().c_str(), config.negative ? "true" : "false"); - // Call the virtual method in the child - impl_.setNegative(config.negative); + // Check the current values for the maximum point + if (max_point != impl_.getMax()) { + RCLCPP_DEBUG( + get_logger(), "Setting the maximum point to: %f %f %f.", + max_point(0), max_point(1), max_point(2)); + impl_.setMax(max_point); } - // The following parameters are updated automatically for all PCL_ROS Nodelet Filters - // as they are inexistent in PCL - if (tf_input_frame_ != config.input_frame) { - tf_input_frame_ = config.input_frame; - NODELET_DEBUG( - "[%s::config_callback] Setting the input TF frame to: %s.", - getName().c_str(), tf_input_frame_.c_str()); - } - if (tf_output_frame_ != config.output_frame) { - tf_output_frame_ = config.output_frame; - NODELET_DEBUG( - "[%s::config_callback] Setting the output TF frame to: %s.", - getName().c_str(), tf_output_frame_.c_str()); - } + // Range constraints are enforced by rclcpp::Parameter. + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; } -typedef pcl_ros::CropBox CropBox; -PLUGINLIB_EXPORT_CLASS(CropBox, nodelet::Nodelet); +#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 7ee29be0c..9ba366ada 100644 --- a/pcl_ros/src/pcl_ros/filters/extract_indices.cpp +++ b/pcl_ros/src/pcl_ros/filters/extract_indices.cpp @@ -35,37 +35,68 @@ * */ -#include #include "pcl_ros/filters/extract_indices.hpp" -////////////////////////////////////////////////////////////////////////////////////////////// -bool -pcl_ros::ExtractIndices::child_init(ros::NodeHandle & nh, bool & has_service) +pcl_ros::ExtractIndices::ExtractIndices(const rclcpp::NodeOptions & options) +: Filter("ExtractIndicesNode", options) { - has_service = true; + 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)); - srv_ = boost::make_shared>(nh); - dynamic_reconfigure::Server::CallbackType f = boost::bind( - &ExtractIndices::config_callback, this, _1, _2); - srv_->setCallback(f); + std::vector param_names{neg_desc.name}; + auto result = config_callback(get_parameters(param_names)); + if (!result.successful) { + throw std::runtime_error(result.reason); + } - use_indices_ = true; - return true; + createPublishers(); } -////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::ExtractIndices::config_callback(pcl_ros::ExtractIndicesConfig & config, uint32_t level) +pcl_ros::ExtractIndices::filter( + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) +{ + std::lock_guard lock(mutex_); + pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); + 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::ExtractIndices::config_callback(const std::vector & params) { - boost::mutex::scoped_lock lock(mutex_); + std::lock_guard lock(mutex_); - if (impl_.getNegative() != config.negative) { - impl_.setNegative(config.negative); - NODELET_DEBUG( - "[%s::config_callback] Setting the extraction to: %s.", getName().c_str(), - (config.negative ? "indices" : "everything but the indices")); + for (const rclcpp::Parameter & param : params) { + if (param.get_name() == "negative") { + // Check the current value for the negative flag + if (impl_.getNegative() != param.as_bool()) { + RCLCPP_DEBUG( + get_logger(), "Setting the filter negative flag to: %s.", + param.as_bool() ? "true" : "false"); + // Call the virtual method in the child + impl_.setNegative(param.as_bool()); + } + } } + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; } -typedef pcl_ros::ExtractIndices ExtractIndices; -PLUGINLIB_EXPORT_CLASS(ExtractIndices, nodelet::Nodelet); +#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 index 7d011c02c..a6248e779 100644 --- a/pcl_ros/src/pcl_ros/filters/filter.cpp +++ b/pcl_ros/src/pcl_ros/filters/filter.cpp @@ -36,8 +36,7 @@ */ #include "pcl_ros/filters/filter.hpp" -#include -#include +#include #include "pcl_ros/transforms.hpp" /*//#include @@ -62,42 +61,49 @@ /////////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::Filter::computePublish(const PointCloud2::ConstPtr & input, const IndicesPtr & indices) +pcl_ros::Filter::computePublish( + const PointCloud2::ConstSharedPtr & input, + const IndicesPtr & indices) { PointCloud2 output; // Call the virtual method in the child - filter(input, indices, output); + if (!input->data.empty()) { + filter(input, indices, output); + } else { + RCLCPP_DEBUG(this->get_logger(), "Received empty input point cloud"); + output = *input; + } - PointCloud2::Ptr cloud_tf(new PointCloud2(output)); // set the output by default + 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_) { - NODELET_DEBUG( - "[%s::computePublish] Transforming output dataset from %s to %s.", - getName().c_str(), output.header.frame_id.c_str(), tf_output_frame_.c_str()); + 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_listener_)) { - NODELET_ERROR( - "[%s::computePublish] Error converting output dataset from %s to %s.", - getName().c_str(), output.header.frame_id.c_str(), tf_output_frame_.c_str()); + 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 - NODELET_DEBUG( - "[%s::computePublish] Transforming output dataset from %s back to %s.", - getName().c_str(), output.header.frame_id.c_str(), tf_input_orig_frame_.c_str()); + 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_listener_)) + tf_buffer_)) { - NODELET_ERROR( - "[%s::computePublish] Error converting output dataset from %s back to %s.", - getName().c_str(), output.header.frame_id.c_str(), tf_input_orig_frame_.c_str()); + 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)); @@ -106,8 +112,8 @@ pcl_ros::Filter::computePublish(const PointCloud2::ConstPtr & input, const Indic // Copy timestamp to keep it cloud_tf->header.stamp = input->header.stamp; - // Publish a boost shared ptr - pub_output_.publish(cloud_tf); + // Publish the unique ptr + pub_output_->publish(move(cloud_tf)); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -117,28 +123,39 @@ pcl_ros::Filter::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_); + auto sensor_qos_profile = rclcpp::SensorDataQoS().keep_last(max_queue_size_); + sub_input_filter_.subscribe(this, "input", sensor_qos_profile.get_rmw_qos_profile()); + sub_indices_filter_.subscribe(this, "indices", sensor_qos_profile.get_rmw_qos_profile()); if (approximate_sync_) { sync_input_indices_a_ = - boost::make_shared>>(max_queue_size_); + std::make_shared>>(max_queue_size_); sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); - sync_input_indices_a_->registerCallback(bind(&Filter::input_indices_callback, this, _1, _2)); + sync_input_indices_a_->registerCallback( + std::bind( + &Filter::input_indices_callback, this, + std::placeholders::_1, std::placeholders::_2)); } else { sync_input_indices_e_ = - boost::make_shared>>(max_queue_size_); + std::make_shared>>(max_queue_size_); sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); - sync_input_indices_e_->registerCallback(bind(&Filter::input_indices_callback, this, _1, _2)); + 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_ = - pnh_->subscribe( - "input", max_queue_size_, - bind(&Filter::input_indices_callback, this, _1, pcl_msgs::PointIndicesConstPtr())); + this->create_subscription( + "input", rclcpp::SensorDataQoS().keep_last(max_queue_size_), + callback); } } @@ -150,114 +167,189 @@ pcl_ros::Filter::unsubscribe() sub_input_filter_.unsubscribe(); sub_indices_filter_.unsubscribe(); } else { - sub_input_.shutdown(); + sub_input_.reset(); } } ////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::Filter::onInit() +pcl_ros::Filter::Filter(std::string node_name, const rclcpp::NodeOptions & options) +: PCLNode(node_name, options) { - // Call the super onInit () - PCLNodelet::onInit(); + RCLCPP_DEBUG(this->get_logger(), "Node successfully created."); +} - // Call the child's local init - bool has_service = false; - if (!child_init(*pnh_, has_service)) { - NODELET_ERROR("[%s::onInit] Initialization failed.", getName().c_str()); - return; +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::Filter::createPublishers() +{ + auto pub_options = rclcpp::PublisherOptions(); + 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); +} - pub_output_ = advertise(*pnh_, "output", max_queue_size_); +////////////////////////////////////////////////////////////////////////////////////////////// +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); - // Enable the dynamic reconfigure service - if (!has_service) { - srv_ = boost::make_shared>(*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind( - &Filter::config_callback, this, _1, _2); - srv_->setCallback(f); + // 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); - NODELET_DEBUG("[%s::onInit] Nodelet successfully created.", getName().c_str()); + 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 + }; } ////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::Filter::config_callback(pcl_ros::FilterConfig & config, uint32_t level) +rcl_interfaces::msg::SetParametersResult +pcl_ros::Filter::config_callback(const std::vector & params) { - // The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are - // inexistent in PCL - if (tf_input_frame_ != config.input_frame) { - tf_input_frame_ = config.input_frame; - NODELET_DEBUG( - "[%s::config_callback] Setting the input TF frame to: %s.", - getName().c_str(), tf_input_frame_.c_str()); - } - if (tf_output_frame_ != config.output_frame) { - tf_output_frame_ = config.output_frame; - NODELET_DEBUG( - "[%s::config_callback] Setting the output TF frame to: %s.", - getName().c_str(), tf_output_frame_.c_str()); + 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::ConstPtr & cloud, - const PointIndicesConstPtr & indices) + const PointCloud2::ConstSharedPtr & cloud, + const PointIndices::ConstSharedPtr & indices) { // If cloud is given, check if it's valid if (!isValid(cloud)) { - NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str()); + RCLCPP_ERROR(this->get_logger(), "Invalid input!"); 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()); + RCLCPP_ERROR(this->get_logger(), "Invalid indices!"); return; } /// DEBUG if (indices) { - 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(), + 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.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()); + 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 { - NODELET_DEBUG( - "[%s::input_indices_callback] PointCloud with %d data points and frame %s on " - "topic %s received.", - getName().c_str(), cloud->width * cloud->height, - cloud->header.frame_id.c_str(), pnh_->resolveName("input").c_str()); + 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::ConstPtr cloud_tf; + PointCloud2::ConstSharedPtr cloud_tf; if (!tf_input_frame_.empty() && cloud->header.frame_id != tf_input_frame_) { - NODELET_DEBUG( - "[%s::input_indices_callback] Transforming input dataset from %s to %s.", - getName().c_str(), cloud->header.frame_id.c_str(), tf_input_frame_.c_str()); + 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_listener_)) { - NODELET_ERROR( - "[%s::input_indices_callback] Error converting input dataset from %s to %s.", - getName().c_str(), cloud->header.frame_id.c_str(), tf_input_frame_.c_str()); + 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 = boost::make_shared(cloud_transformed); + cloud_tf = std::make_shared(cloud_transformed); } else { cloud_tf = cloud; } diff --git a/pcl_ros/src/pcl_ros/filters/passthrough.cpp b/pcl_ros/src/pcl_ros/filters/passthrough.cpp index 8ee183d26..8d3cec524 100644 --- a/pcl_ros/src/pcl_ros/filters/passthrough.cpp +++ b/pcl_ros/src/pcl_ros/filters/passthrough.cpp @@ -36,95 +36,121 @@ */ #include "pcl_ros/filters/passthrough.hpp" -#include -////////////////////////////////////////////////////////////////////////////////////////////// -bool -pcl_ros::PassThrough::child_init(ros::NodeHandle & nh, bool & has_service) +pcl_ros::PassThrough::PassThrough(const rclcpp::NodeOptions & options) +: Filter("PassThroughNode", options) { - // Enable the dynamic reconfigure service - has_service = true; - srv_ = boost::make_shared>(nh); - dynamic_reconfigure::Server::CallbackType f = boost::bind( - &PassThrough::config_callback, this, _1, _2); - srv_->setCallback(f); + use_frame_params(); + std::vector common_param_names = add_common_params(); + + rcl_interfaces::msg::ParameterDescriptor keep_organized_desc; + keep_organized_desc.name = "keep_organized"; + keep_organized_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + keep_organized_desc.description = + "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)); - return true; + createPublishers(); +} + +void pcl_ros::PassThrough::filter( + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) +{ + std::lock_guard lock(mutex_); + pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); + 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); } ////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::PassThrough::config_callback(pcl_ros::FilterConfig & config, uint32_t level) +rcl_interfaces::msg::SetParametersResult +pcl_ros::PassThrough::config_callback(const std::vector & params) { - boost::mutex::scoped_lock lock(mutex_); + std::lock_guard lock(mutex_); double filter_min, filter_max; impl_.getFilterLimits(filter_min, filter_max); - // Check the current values for filter min-max - if (filter_min != config.filter_limit_min) { - filter_min = config.filter_limit_min; - NODELET_DEBUG( - "[%s::config_callback] Setting the minimum filtering value a point will be " - "considered from to: %f.", - getName().c_str(), filter_min); - // Set the filter min-max if different - impl_.setFilterLimits(filter_min, filter_max); - } - // Check the current values for filter min-max - if (filter_max != config.filter_limit_max) { - filter_max = config.filter_limit_max; - NODELET_DEBUG( - "[%s::config_callback] Setting the maximum filtering value a point will be " - "considered from to: %f.", - getName().c_str(), filter_max); - // Set the filter min-max if different - impl_.setFilterLimits(filter_min, filter_max); - } - - // Check the current value for the filter field - // std::string filter_field = impl_.getFilterFieldName (); - if (impl_.getFilterFieldName() != config.filter_field_name) { - // Set the filter field if different - impl_.setFilterFieldName(config.filter_field_name); - NODELET_DEBUG( - "[%s::config_callback] Setting the filter field name to: %s.", - getName().c_str(), config.filter_field_name.c_str()); - } - - // Check the current value for keep_organized - if (impl_.getKeepOrganized() != config.keep_organized) { - NODELET_DEBUG( - "[%s::config_callback] Setting the filter keep_organized value to: %s.", - getName().c_str(), config.keep_organized ? "true" : "false"); - // Call the virtual method in the child - impl_.setKeepOrganized(config.keep_organized); - } - - // Check the current value for the negative flag - if (impl_.getFilterLimitsNegative() != config.filter_limit_negative) { - NODELET_DEBUG( - "[%s::config_callback] Setting the filter negative flag to: %s.", - getName().c_str(), config.filter_limit_negative ? "true" : "false"); - // Call the virtual method in the child - impl_.setFilterLimitsNegative(config.filter_limit_negative); - } - - // The following parameters are updated automatically for all PCL_ROS Nodelet Filters - // as they are inexistent in PCL - if (tf_input_frame_ != config.input_frame) { - tf_input_frame_ = config.input_frame; - NODELET_DEBUG( - "[%s::config_callback] Setting the input TF frame to: %s.", - getName().c_str(), tf_input_frame_.c_str()); - } - if (tf_output_frame_ != config.output_frame) { - tf_output_frame_ = config.output_frame; - NODELET_DEBUG( - "[%s::config_callback] Setting the output TF frame to: %s.", - getName().c_str(), tf_output_frame_.c_str()); + for (const rclcpp::Parameter & param : params) { + if (param.get_name() == "filter_field_name") { + // Check the current value for the filter field + if (impl_.getFilterFieldName() != param.as_string()) { + // Set the filter field if different + impl_.setFilterFieldName(param.as_string()); + RCLCPP_DEBUG( + get_logger(), "Setting the filter field name to: %s.", + param.as_string().c_str()); + } + } + if (param.get_name() == "filter_limit_min") { + // Check the current values for filter min-max + if (filter_min != param.as_double()) { + filter_min = param.as_double(); + RCLCPP_DEBUG( + get_logger(), + "Setting the minimum filtering value a point will be considered from to: %f.", + filter_min); + // Set the filter min-max if different + impl_.setFilterLimits(filter_min, filter_max); + } + } + if (param.get_name() == "filter_limit_max") { + // Check the current values for filter min-max + if (filter_max != param.as_double()) { + filter_max = param.as_double(); + RCLCPP_DEBUG( + get_logger(), + "Setting the maximum filtering value a point will be considered from to: %f.", + filter_max); + // Set the filter min-max if different + impl_.setFilterLimits(filter_min, filter_max); + } + } + if (param.get_name() == "filter_limit_negative") { + // Check the current value for the negative flag + if (impl_.getNegative() != param.as_bool()) { + RCLCPP_DEBUG( + get_logger(), "Setting the filter negative flag to: %s.", + param.as_bool() ? "true" : "false"); + // Call the virtual method in the child + impl_.setNegative(param.as_bool()); + } + } + if (param.get_name() == "keep_organized") { + // Check the current value for keep_organized + if (impl_.getKeepOrganized() != param.as_bool()) { + RCLCPP_DEBUG( + get_logger(), "Setting the filter keep_organized value to: %s.", + param.as_bool() ? "true" : "false"); + // Call the virtual method in the child + impl_.setKeepOrganized(param.as_bool()); + } + } } + // TODO(sloretz) constraint validation + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; } -typedef pcl_ros::PassThrough PassThrough; -PLUGINLIB_EXPORT_CLASS(PassThrough, nodelet::Nodelet); +#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 23a3111d6..27679cd67 100644 --- a/pcl_ros/src/pcl_ros/filters/project_inliers.cpp +++ b/pcl_ros/src/pcl_ros/filters/project_inliers.cpp @@ -36,46 +36,38 @@ */ #include "pcl_ros/filters/project_inliers.hpp" -#include -#include -#include ////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::ProjectInliers::onInit() +pcl_ros::ProjectInliers::ProjectInliers(const rclcpp::NodeOptions & options) +: Filter("ProjectInliersNode", options), model_() { - PCLNodelet::onInit(); - // ---[ Mandatory parameters // The type of model to use (user given parameter). + declare_parameter("model_type", rclcpp::ParameterType::PARAMETER_INTEGER); 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()); + if (!get_parameter("model_type", model_type)) { + RCLCPP_ERROR( + get_logger(), + "[onConstruct] Need a 'model_type' parameter to be set before continuing!"); return; } // ---[ Optional parameters // True if all data will be returned, false if only the projected inliers. Default: false. - bool copy_all_data = false; + declare_parameter("copy_all_data", rclcpp::ParameterValue(false)); + bool copy_all_data = get_parameter("copy_all_data").as_bool(); // True if all fields will be returned, false if only XYZ. Default: true. - bool copy_all_fields = true; - - pnh_->getParam("copy_all_data", copy_all_data); - pnh_->getParam("copy_all_fields", copy_all_fields); + declare_parameter("copy_all_fields", rclcpp::ParameterValue(true)); + bool copy_all_fields = get_parameter("copy_all_fields").as_bool(); - pub_output_ = advertise(*pnh_, "output", max_queue_size_); + pub_output_ = create_publisher("output", max_queue_size_); - // Subscribe to the input using a filter - sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_); - - NODELET_DEBUG( - "[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - model_type : %d\n" - " - copy_all_data : %s\n" - " - copy_all_fields : %s", - getName().c_str(), + 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 @@ -83,42 +75,66 @@ pcl_ros::ProjectInliers::onInit() impl_.setCopyAllFields(copy_all_fields); impl_.setCopyAllData(copy_all_data); - onInitPostProcess(); + createPublishers(); +} + +void +pcl_ros::ProjectInliers::filter( + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) +{ + pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); + pcl_conversions::toPCL(*(input), *(pcl_input)); + impl_.setInputCloud(pcl_input); + impl_.setIndices(indices); + pcl::ModelCoefficients::Ptr pcl_model(new pcl::ModelCoefficients); + 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_) {*/ - sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); - - sub_model_.subscribe(*pnh_, "model", max_queue_size_); + auto qos_profile = rclcpp::QoS( + rclcpp::KeepLast(max_queue_size_), + rmw_qos_profile_default).get_rmw_qos_profile(); + auto sensor_qos_profile = rclcpp::QoS( + rclcpp::KeepLast(max_queue_size_), + rmw_qos_profile_sensor_data).get_rmw_qos_profile(); + 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_ = - boost::make_shared>>(max_queue_size_); + 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( - bind( - &ProjectInliers::input_indices_model_callback, - this, _1, _2, _3)); + std::bind( + &ProjectInliers::input_indices_model_callback, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } else { - sync_input_indices_model_e_ = - boost::make_shared>>(max_queue_size_); + 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( - bind( - &ProjectInliers::input_indices_model_callback, - this, _1, _2, _3)); + std::bind( + &ProjectInliers::input_indices_model_callback, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } } @@ -130,42 +146,39 @@ 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::ConstPtr & cloud, + const PointCloud2::ConstSharedPtr & cloud, const PointIndicesConstPtr & indices, const ModelCoefficientsConstPtr & model) { - if (pub_output_.getNumSubscribers() <= 0) { + if (pub_output_->get_subscription_count() == 0) { return; } if (!isValid(model) || !isValid(indices) || !isValid(cloud)) { - NODELET_ERROR("[%s::input_indices_model_callback] Invalid input!", getName().c_str()); + RCLCPP_ERROR( + this->get_logger(), "[%s::input_indices_model_callback] Invalid input!", this->get_name()); return; } - NODELET_DEBUG( + RCLCPP_DEBUG( + this->get_logger(), "[%s::input_indices_model_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.\n" - " - ModelCoefficients with %zu values, stamp %f, and " - "frame %s on topic %s received.", - getName().c_str(), - cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), - 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("inliers").c_str(), - model->values.size(), model->header.stamp.toSec(), - model->header.frame_id.c_str(), pnh_->resolveName("model").c_str()); + " - 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; @@ -178,5 +191,5 @@ pcl_ros::ProjectInliers::input_indices_model_callback( computePublish(cloud, vindices); } -typedef pcl_ros::ProjectInliers ProjectInliers; -PLUGINLIB_EXPORT_CLASS(ProjectInliers, nodelet::Nodelet); +#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 0a01b0b10..1e487bf68 100644 --- a/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp +++ b/pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp @@ -35,46 +35,98 @@ * */ -#include #include "pcl_ros/filters/radius_outlier_removal.hpp" -////////////////////////////////////////////////////////////////////////////////////////////// -bool -pcl_ros::RadiusOutlierRemoval::child_init(ros::NodeHandle & nh, bool & has_service) +pcl_ros::RadiusOutlierRemoval::RadiusOutlierRemoval(const rclcpp::NodeOptions & options) +: Filter("RadiusOutlierRemovalNode", options) { - // Enable the dynamic reconfigure service - has_service = true; - srv_ = boost::make_shared>(nh); - dynamic_reconfigure::Server::CallbackType f = boost::bind( - &RadiusOutlierRemoval::config_callback, this, _1, _2); - srv_->setCallback(f); + rcl_interfaces::msg::ParameterDescriptor min_neighbors_desc; + min_neighbors_desc.name = "min_neighbors"; + min_neighbors_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + min_neighbors_desc.description = + "The number of neighbors that need to be present in order to be classified as an inlier."; + { + rcl_interfaces::msg::IntegerRange int_range; + int_range.from_value = 0; + int_range.to_value = 1000; + min_neighbors_desc.integer_range.push_back(int_range); + } + declare_parameter(min_neighbors_desc.name, rclcpp::ParameterValue(5), min_neighbors_desc); + + rcl_interfaces::msg::ParameterDescriptor radius_search_desc; + radius_search_desc.name = "radius_search"; + radius_search_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + radius_search_desc.description = + "Radius of the sphere that will determine which points are neighbors."; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = 0.0; + float_range.to_value = 10.0; + 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)); - return true; + createPublishers(); } -////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::RadiusOutlierRemoval::config_callback( - pcl_ros::RadiusOutlierRemovalConfig & config, - uint32_t level) +pcl_ros::RadiusOutlierRemoval::filter( + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) { - boost::mutex::scoped_lock lock(mutex_); + std::lock_guard lock(mutex_); + pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); + 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); +} - if (impl_.getMinNeighborsInRadius() != config.min_neighbors) { - impl_.setMinNeighborsInRadius(config.min_neighbors); - NODELET_DEBUG( - "[%s::config_callback] Setting the number of neighbors in radius: %d.", - getName().c_str(), config.min_neighbors); - } +////////////////////////////////////////////////////////////////////////////////////////////// +rcl_interfaces::msg::SetParametersResult +pcl_ros::RadiusOutlierRemoval::config_callback(const std::vector & params) +{ + std::lock_guard lock(mutex_); - if (impl_.getRadiusSearch() != config.radius_search) { - impl_.setRadiusSearch(config.radius_search); - NODELET_DEBUG( - "[%s::config_callback] Setting the radius to search neighbors: %f.", - getName().c_str(), config.radius_search); + for (const rclcpp::Parameter & param : params) { + if (param.get_name() == "min_neighbors") { + if (impl_.getMinNeighborsInRadius() != param.as_int()) { + RCLCPP_DEBUG( + get_logger(), "Setting the number of neighbors in radius: %ld.", + param.as_int()); + impl_.setMinNeighborsInRadius(param.as_int()); + } + } + if (param.get_name() == "radius_search") { + if (impl_.getRadiusSearch() != param.as_double()) { + RCLCPP_DEBUG( + get_logger(), "Setting the radius to search neighbors: %f.", + param.as_double()); + impl_.setRadiusSearch(param.as_double()); + } + } } -} + // Range constraints are enforced by rclcpp::Parameter. + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; +} -typedef pcl_ros::RadiusOutlierRemoval RadiusOutlierRemoval; -PLUGINLIB_EXPORT_CLASS(RadiusOutlierRemoval, nodelet::Nodelet); +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::RadiusOutlierRemoval) diff --git a/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp b/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp index e7a48a9b5..fe356d0ab 100644 --- a/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp +++ b/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp @@ -35,53 +35,118 @@ * */ -#include #include "pcl_ros/filters/statistical_outlier_removal.hpp" -////////////////////////////////////////////////////////////////////////////////////////////// -bool -pcl_ros::StatisticalOutlierRemoval::child_init(ros::NodeHandle & nh, bool & has_service) +pcl_ros::StatisticalOutlierRemoval::StatisticalOutlierRemoval(const rclcpp::NodeOptions & options) +: Filter("StatisticalOutlierRemovalNode", options) { - // Enable the dynamic reconfigure service - has_service = true; - srv_ = boost::make_shared>( - nh); - dynamic_reconfigure::Server::CallbackType f = - boost::bind(&StatisticalOutlierRemoval::config_callback, this, _1, _2); - srv_->setCallback(f); + rcl_interfaces::msg::ParameterDescriptor mean_k_desc; + mean_k_desc.name = "mean_k"; + mean_k_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + mean_k_desc.description = + "The number of points (k) to use for mean distance estimation."; + { + rcl_interfaces::msg::IntegerRange int_range; + int_range.from_value = 2; + int_range.to_value = 100; + mean_k_desc.integer_range.push_back(int_range); + } + declare_parameter(mean_k_desc.name, rclcpp::ParameterValue(2), mean_k_desc); + + rcl_interfaces::msg::ParameterDescriptor stddev_desc; + stddev_desc.name = "stddev"; + stddev_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + stddev_desc.description = + "The standard deviation multiplier threshold." + "All points outside the mean +- sigma * std_mul will be considered outliers."; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = 0.0; + float_range.to_value = 5.0; + stddev_desc.floating_point_range.push_back(float_range); + } + declare_parameter(stddev_desc.name, rclcpp::ParameterValue(0.0), stddev_desc); + + rcl_interfaces::msg::ParameterDescriptor negative_desc; + negative_desc.name = "negative"; + negative_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + 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 { + 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)); - return true; + config_callback(get_parameters(param_names)); + + createPublishers(); } -////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::StatisticalOutlierRemoval::config_callback( - pcl_ros::StatisticalOutlierRemovalConfig & config, uint32_t level) +pcl_ros::StatisticalOutlierRemoval::filter( + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) { - boost::mutex::scoped_lock lock(mutex_); + std::lock_guard lock(mutex_); + pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); + 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); +} - if (impl_.getMeanK() != config.mean_k) { - impl_.setMeanK(config.mean_k); - NODELET_DEBUG( - "[%s::config_callback] Setting the number of points (k) to use for mean " - "distance estimation to: %d.", - getName().c_str(), config.mean_k); - } +////////////////////////////////////////////////////////////////////////////////////////////// +rcl_interfaces::msg::SetParametersResult +pcl_ros::StatisticalOutlierRemoval::config_callback(const std::vector & params) +{ + std::lock_guard lock(mutex_); - if (impl_.getStddevMulThresh() != config.stddev) { - impl_.setStddevMulThresh(config.stddev); - NODELET_DEBUG( - "[%s::config_callback] Setting the standard deviation multiplier threshold to: %f.", - getName().c_str(), config.stddev); + for (const rclcpp::Parameter & param : params) { + if (param.get_name() == "mean_k") { + if (impl_.getMeanK() != param.as_int()) { + RCLCPP_DEBUG( + get_logger(), + "Setting the number of points (k) to use for mean distance estimation to: %ld.", + param.as_int()); + impl_.setMeanK(param.as_int()); + } + } + if (param.get_name() == "stddev") { + if (impl_.getStddevMulThresh() != param.as_double()) { + RCLCPP_DEBUG( + get_logger(), + "Setting the standard deviation multiplier threshold to: %f.", + param.as_double()); + impl_.setStddevMulThresh(param.as_double()); + } + } + if (param.get_name() == "negative") { + if (impl_.getNegative() != param.as_bool()) { + RCLCPP_DEBUG( + get_logger(), + "Returning only inliers: %s.", + (param.as_bool() ? "false" : "true")); + impl_.setNegative(param.as_bool()); + } + } } - if (impl_.getNegative() != config.negative) { - impl_.setNegative(config.negative); - NODELET_DEBUG( - "[%s::config_callback] Returning only inliers: %s.", - getName().c_str(), config.negative ? "false" : "true"); - } + // Range constraints are enforced by rclcpp::Parameter. + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; } -typedef pcl_ros::StatisticalOutlierRemoval StatisticalOutlierRemoval; -PLUGINLIB_EXPORT_CLASS(StatisticalOutlierRemoval, nodelet::Nodelet); +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::StatisticalOutlierRemoval) diff --git a/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp b/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp index 39b797a0a..8ac9c66c4 100644 --- a/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp +++ b/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp @@ -35,31 +35,65 @@ * */ -#include #include "pcl_ros/filters/voxel_grid.hpp" ////////////////////////////////////////////////////////////////////////////////////////////// -bool -pcl_ros::VoxelGrid::child_init(ros::NodeHandle & nh, bool & has_service) + +pcl_ros::VoxelGrid::VoxelGrid(const rclcpp::NodeOptions & options) +: Filter("VoxelGridNode", options) { - // Enable the dynamic reconfigure service - has_service = true; - srv_ = boost::make_shared>(nh); - dynamic_reconfigure::Server::CallbackType f = boost::bind( - &VoxelGrid::config_callback, this, _1, _2); - srv_->setCallback(f); - - return true; + std::vector common_param_names = add_common_params(); + + rcl_interfaces::msg::ParameterDescriptor leaf_size_desc; + leaf_size_desc.name = "leaf_size"; + leaf_size_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + leaf_size_desc.description = + "The size of a leaf (on x,y,z) used for downsampling"; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = 0.0; + float_range.to_value = 1.0; + leaf_size_desc.floating_point_range.push_back(float_range); + } + declare_parameter(leaf_size_desc.name, rclcpp::ParameterValue(0.01), leaf_size_desc); + + rcl_interfaces::msg::ParameterDescriptor min_points_per_voxel_desc; + min_points_per_voxel_desc.name = "min_points_per_voxel"; + min_points_per_voxel_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + min_points_per_voxel_desc.description = + "The minimum number of points required for a voxel to be used."; + { + rcl_interfaces::msg::IntegerRange int_range; + int_range.from_value = 1; + int_range.to_value = 100000; + min_points_per_voxel_desc.integer_range.push_back(int_range); + } + 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::ConstPtr & input, - const IndicesPtr & indices, + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, PointCloud2 & output) { - boost::mutex::scoped_lock lock(mutex_); + std::lock_guard lock(mutex_); pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); pcl_conversions::toPCL(*(input), *(pcl_input)); impl_.setInputCloud(pcl_input); @@ -70,65 +104,88 @@ pcl_ros::VoxelGrid::filter( } ////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::VoxelGrid::config_callback(pcl_ros::VoxelGridConfig & config, uint32_t level) +rcl_interfaces::msg::SetParametersResult +pcl_ros::VoxelGrid::config_callback(const std::vector & params) { - boost::mutex::scoped_lock lock(mutex_); - - Eigen::Vector3f leaf_size = impl_.getLeafSize(); - - if (leaf_size[0] != config.leaf_size) { - leaf_size.setConstant(config.leaf_size); - NODELET_DEBUG("[config_callback] Setting the downsampling leaf size to: %f.", leaf_size[0]); - impl_.setLeafSize(leaf_size[0], leaf_size[1], leaf_size[2]); - } + std::lock_guard lock(mutex_); double filter_min, filter_max; impl_.getFilterLimits(filter_min, filter_max); - if (filter_min != config.filter_limit_min) { - filter_min = config.filter_limit_min; - NODELET_DEBUG( - "[config_callback] Setting the minimum filtering value a point will be considered " - "from to: %f.", - filter_min); - } - if (filter_max != config.filter_limit_max) { - filter_max = config.filter_limit_max; - NODELET_DEBUG( - "[config_callback] Setting the maximum filtering value a point will be considered " - "from to: %f.", - filter_max); - } - impl_.setFilterLimits(filter_min, filter_max); - if (impl_.getFilterLimitsNegative() != config.filter_limit_negative) { - impl_.setFilterLimitsNegative(config.filter_limit_negative); - NODELET_DEBUG( - "[%s::config_callback] Setting the filter negative flag to: %s.", - getName().c_str(), config.filter_limit_negative ? "true" : "false"); - } + Eigen::Vector3f leaf_size = impl_.getLeafSize(); - if (impl_.getFilterFieldName() != config.filter_field_name) { - impl_.setFilterFieldName(config.filter_field_name); - NODELET_DEBUG( - "[config_callback] Setting the filter field name to: %s.", - config.filter_field_name.c_str()); - } + unsigned int minPointsPerVoxel = impl_.getMinimumPointsNumberPerVoxel(); - // ---[ These really shouldn't be here, and as soon as dynamic_reconfigure improves, - // we'll remove them and inherit from Filter - 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()); + for (const rclcpp::Parameter & param : params) { + if (param.get_name() == "filter_field_name") { + // Check the current value for the filter field + if (impl_.getFilterFieldName() != param.as_string()) { + // Set the filter field if different + impl_.setFilterFieldName(param.as_string()); + RCLCPP_DEBUG( + get_logger(), "Setting the filter field name to: %s.", + param.as_string().c_str()); + } + } + if (param.get_name() == "filter_limit_min") { + // Check the current values for filter min-max + if (filter_min != param.as_double()) { + filter_min = param.as_double(); + RCLCPP_DEBUG( + get_logger(), + "Setting the minimum filtering value a point will be considered from to: %f.", + filter_min); + // Set the filter min-max if different + impl_.setFilterLimits(filter_min, filter_max); + } + } + if (param.get_name() == "filter_limit_max") { + // Check the current values for filter min-max + if (filter_max != param.as_double()) { + filter_max = param.as_double(); + RCLCPP_DEBUG( + get_logger(), + "Setting the maximum filtering value a point will be considered from to: %f.", + filter_max); + // Set the filter min-max if different + impl_.setFilterLimits(filter_min, filter_max); + } + } + if (param.get_name() == "filter_limit_negative") { + bool new_filter_limits_negative = param.as_bool(); + if (impl_.getFilterLimitsNegative() != new_filter_limits_negative) { + RCLCPP_DEBUG( + get_logger(), + "Setting the filter negative flag to: %s.", + (new_filter_limits_negative ? "true" : "false")); + impl_.setFilterLimitsNegative(new_filter_limits_negative); + } + } + if (param.get_name() == "min_points_per_voxel") { + if (minPointsPerVoxel != ((unsigned int) param.as_int())) { + RCLCPP_DEBUG( + get_logger(), + "Setting the minimum points per voxel to: %u.", + minPointsPerVoxel); + impl_.setMinimumPointsNumberPerVoxel(param.as_int()); + } + } + if (param.get_name() == "leaf_size") { + leaf_size.setConstant(param.as_double()); + if (impl_.getLeafSize() != leaf_size) { + RCLCPP_DEBUG( + get_logger(), "Setting the downsampling leaf size to: %f %f %f.", + leaf_size[0], leaf_size[1], leaf_size[2]); + impl_.setLeafSize(leaf_size[0], leaf_size[1], leaf_size[2]); + } + } } - 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()); - } - // ]--- + + // Range constraints are enforced by rclcpp::Parameter. + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; } -typedef pcl_ros::VoxelGrid VoxelGrid; -PLUGINLIB_EXPORT_CLASS(VoxelGrid, nodelet::Nodelet); +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::VoxelGrid) diff --git a/pcl_ros/tests/filters/CMakeLists.txt b/pcl_ros/tests/filters/CMakeLists.txt new file mode 100644 index 000000000..669b94eba --- /dev/null +++ b/pcl_ros/tests/filters/CMakeLists.txt @@ -0,0 +1,112 @@ +find_package(ament_cmake_pytest REQUIRED) + +# build dummy publisher node and component +add_library(dummy_topics SHARED + dummy_topics.cpp +) +target_link_libraries(dummy_topics ${PCL_LIBRARIES}) +ament_target_dependencies(dummy_topics + rclcpp + rclcpp_components + pcl_conversions + sensor_msgs + PCL +) + +# generate test ament index to locate dummy_topics comonent from tests +set(components "") +set(components "${components}pcl_ros_tests_filters::DummyTopics;$\n") +file(GENERATE +OUTPUT +"${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/share/ament_index/resource_index/rclcpp_components/pcl_ros_tests_filters" +CONTENT "${components}") + +# test components +ament_add_pytest_test(test_pcl_ros::ExtractIndices + test_filter_component.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_PLUGIN=pcl_ros::ExtractIndices + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) +ament_add_pytest_test(test_pcl_ros::PassThrough + test_filter_component.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_PLUGIN=pcl_ros::PassThrough + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) +ament_add_pytest_test(test_pcl_ros::ProjectInliers + test_filter_component.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_PLUGIN=pcl_ros::ProjectInliers + PARAMETERS={'model_type':0} + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) +ament_add_pytest_test(test_pcl_ros::RadiusOutlierRemoval + test_filter_component.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_PLUGIN=pcl_ros::RadiusOutlierRemoval + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) +ament_add_pytest_test(test_pcl_ros::StatisticalOutlierRemoval + test_filter_component.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_PLUGIN=pcl_ros::StatisticalOutlierRemoval + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) +ament_add_pytest_test(test_pcl_ros::CropBox + test_filter_component.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_PLUGIN=pcl_ros::CropBox + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) +ament_add_pytest_test(test_pcl_ros::VoxelGrid + test_filter_component.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_PLUGIN=pcl_ros::VoxelGrid + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) + +# test executables +ament_add_pytest_test(test_filter_extract_indices_node + test_filter_executable.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_EXECUTABLE=filter_extract_indices_node + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) +ament_add_pytest_test(test_filter_passthrough_node + test_filter_executable.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_EXECUTABLE=filter_passthrough_node + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) +ament_add_pytest_test(test_filter_project_inliers_node + test_filter_executable.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_EXECUTABLE=filter_project_inliers_node + PARAMETERS={'model_type':0} + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) +ament_add_pytest_test(test_filter_radius_outlier_removal_node + test_filter_executable.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_EXECUTABLE=filter_radius_outlier_removal_node + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) +ament_add_pytest_test(test_filter_statistical_outlier_removal_node + test_filter_executable.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_EXECUTABLE=filter_statistical_outlier_removal_node + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) +ament_add_pytest_test(test_filter_crop_box_node + test_filter_executable.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_EXECUTABLE=filter_crop_box_node + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) +ament_add_pytest_test(test_filter_voxel_grid_node + test_filter_executable.py + ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics + FILTER_EXECUTABLE=filter_voxel_grid_node + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index +) diff --git a/pcl_ros/tests/filters/dummy_topics.cpp b/pcl_ros/tests/filters/dummy_topics.cpp new file mode 100644 index 000000000..0611ff8cc --- /dev/null +++ b/pcl_ros/tests/filters/dummy_topics.cpp @@ -0,0 +1,109 @@ +/* + * Copyright (c) 2022 Carnegie Mellon University + * 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 Carnegie Mellon University 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. + */ + + +#include +#include +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; + +namespace pcl_ros_tests_filters +{ +class DummyTopics : public rclcpp::Node +{ +public: + explicit DummyTopics(const rclcpp::NodeOptions & options); + +private: + void timer_callback(); + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr point_cloud2_pub_; + rclcpp::Publisher::SharedPtr indices_pub_; + rclcpp::Publisher::SharedPtr model_pub_; + size_t count_; +}; + +DummyTopics::DummyTopics(const rclcpp::NodeOptions & options) +: Node("dummy_point_cloud2_publisher", options), count_(0) +{ + point_cloud2_pub_ = this->create_publisher("point_cloud2", 10); + indices_pub_ = this->create_publisher("indices", 10); + model_pub_ = this->create_publisher("model", 10); + timer_ = this->create_wall_timer( + 100ms, std::bind(&DummyTopics::timer_callback, this)); +} + +void DummyTopics::timer_callback() +{ + builtin_interfaces::msg::Time now_msg; + now_msg = get_clock()->now(); + + sensor_msgs::msg::PointCloud2 point_cloud2_msg; + + pcl::PointCloud random_pcl; + + unsigned int global_seed = 100; + for (int v = 0; v < 1000; ++v) { + pcl::PointXYZ newPoint; + newPoint.x = (rand_r(&global_seed) * 100.0) / RAND_MAX; + newPoint.y = (rand_r(&global_seed) * 100.0) / RAND_MAX; + newPoint.z = (rand_r(&global_seed) * 100.0) / RAND_MAX; + random_pcl.points.push_back(newPoint); + } + + // publish point cloud + pcl::toROSMsg(random_pcl, point_cloud2_msg); + point_cloud2_msg.header.stamp = now_msg; + point_cloud2_pub_->publish(point_cloud2_msg); + + // publish indices + pcl_msgs::msg::PointIndices indices_msg; + indices_msg.header.stamp = now_msg; + indices_msg.indices.push_back(0); + indices_pub_->publish(indices_msg); + + // publish model + pcl_msgs::msg::ModelCoefficients model_msg; + model_msg.header.stamp = now_msg; + model_msg.values.push_back(0); + model_msg.values.push_back(0); + model_msg.values.push_back(1); + model_msg.values.push_back(0); + model_pub_->publish(model_msg); +} + +} // namespace pcl_ros_tests_filters + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros_tests_filters::DummyTopics) diff --git a/pcl_ros/tests/filters/test_filter_component.py b/pcl_ros/tests/filters/test_filter_component.py new file mode 100644 index 000000000..2e01e0e85 --- /dev/null +++ b/pcl_ros/tests/filters/test_filter_component.py @@ -0,0 +1,85 @@ +# +# Copyright (c) 2022 Carnegie Mellon University +# 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 Carnegie Mellon University 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. +# + +import ast +import os +import unittest + +import launch +import launch.actions +import launch_ros.actions +import launch_testing.actions +import launch_testing.markers +from launch_testing_ros import WaitForTopics +import pytest +from sensor_msgs.msg import PointCloud2 + + +@pytest.mark.launch_test +@launch_testing.markers.keep_alive +def generate_test_description(): + dummy_plugin = os.getenv('DUMMY_PLUGIN') + filter_plugin = os.getenv('FILTER_PLUGIN') + parameters = ast.literal_eval(os.getenv('PARAMETERS')) if 'PARAMETERS' in os.environ else {} + + print(parameters) + + return launch.LaunchDescription([ + launch_ros.actions.ComposableNodeContainer( + name='filter_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + launch_ros.descriptions.ComposableNode( + package='pcl_ros_tests_filters', + plugin=dummy_plugin, + name='dummy_publisher', + ), + launch_ros.descriptions.ComposableNode( + package='pcl_ros', + plugin=filter_plugin, + name='filter_node', + remappings=[('/input', '/point_cloud2')], + parameters=[parameters], + ), + ], + output='screen', + ), + launch_testing.actions.ReadyToTest() + ]) + + +class TestFilter(unittest.TestCase): + + def test_filter_output(self): + wait_for_topics = WaitForTopics([('output', PointCloud2)], timeout=5.0) + assert wait_for_topics.wait() + assert 'output' in wait_for_topics.topics_received(), "Didn't receive message" + wait_for_topics.shutdown() diff --git a/pcl_ros/tests/filters/test_filter_executable.py b/pcl_ros/tests/filters/test_filter_executable.py new file mode 100644 index 000000000..b5e8e7c99 --- /dev/null +++ b/pcl_ros/tests/filters/test_filter_executable.py @@ -0,0 +1,87 @@ +# +# Copyright (c) 2022 Carnegie Mellon University +# 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 Carnegie Mellon University 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. +# + +import ast +import os +import unittest + +import launch +import launch.actions +import launch_ros.actions +import launch_testing.actions +import launch_testing.markers +from launch_testing_ros import WaitForTopics +import pytest +from sensor_msgs.msg import PointCloud2 + + +@pytest.mark.launch_test +@launch_testing.markers.keep_alive +def generate_test_description(): + dummy_plugin = os.getenv('DUMMY_PLUGIN') + filter_executable = os.getenv('FILTER_EXECUTABLE') + parameters = ast.literal_eval(os.getenv('PARAMETERS')) if 'PARAMETERS' in os.environ else {} + + ros_arguments = ['-r', 'input:=point_cloud2'] + for key in parameters.keys(): + ros_arguments.extend(['-p', '{}:={}'.format(key, parameters[key])]) + + return launch.LaunchDescription([ + + launch_ros.actions.ComposableNodeContainer( + name='filter_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + launch_ros.descriptions.ComposableNode( + package='pcl_ros_tests_filters', + plugin=dummy_plugin, + name='dummy_publisher', + ), + ], + output='screen', + ), + launch_ros.actions.Node( + package='pcl_ros', + executable=filter_executable, + output='screen', + ros_arguments=ros_arguments + ), + launch_testing.actions.ReadyToTest() + ]) + + +class TestFilter(unittest.TestCase): + + def test_filter_output(self): + wait_for_topics = WaitForTopics([('output', PointCloud2)], timeout=5.0) + assert wait_for_topics.wait() + assert 'output' in wait_for_topics.topics_received(), "Didn't receive message" + wait_for_topics.shutdown()