From de09161924ae8c04499610ed7d47fc1e8f7af9cb Mon Sep 17 00:00:00 2001 From: Daisuke Sato <43101027+daisukes@users.noreply.github.com> Date: Mon, 30 Jan 2023 21:36:12 -0500 Subject: [PATCH] migrate passthrough and project_inliers filters (#395) * migrate passthrough and project_inliers filters Signed-off-by: Daisuke Sato * - remove throwing runtime_error (result always true) - use get_subscription_count() Signed-off-by: Daisuke Sato * change comparison operator Signed-off-by: Daisuke Sato --------- Signed-off-by: Daisuke Sato --- pcl_ros/CMakeLists.txt | 12 +- pcl_ros/include/pcl_ros/filters/filter.hpp | 3 + .../include/pcl_ros/filters/passthrough.hpp | 28 ++-- .../pcl_ros/filters/project_inliers.hpp | 22 +-- .../src/pcl_ros/filters/extract_indices.cpp | 2 + pcl_ros/src/pcl_ros/filters/filter.cpp | 90 ++++++++-- pcl_ros/src/pcl_ros/filters/passthrough.cpp | 155 +++++++++--------- .../src/pcl_ros/filters/project_inliers.cpp | 126 +++++++------- pcl_ros/tests/filters/CMakeLists.txt | 31 +++- .../tests/filters/test_filter_component.py | 9 +- .../tests/filters/test_filter_executable.py | 39 ++++- 11 files changed, 319 insertions(+), 198 deletions(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 5e79bf6b..c51d3b1f 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -84,8 +84,8 @@ ament_target_dependencies(pcl_ros_tf 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/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 @@ -97,6 +97,14 @@ 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 +) class_loader_hide_library_symbols(pcl_ros_filters) # ### Declare the pcl_ros_segmentation library diff --git a/pcl_ros/include/pcl_ros/filters/filter.hpp b/pcl_ros/include/pcl_ros/filters/filter.hpp index a0cd0d20..bbb0999a 100644 --- a/pcl_ros/include/pcl_ros/filters/filter.hpp +++ b/pcl_ros/include/pcl_ros/filters/filter.hpp @@ -71,6 +71,9 @@ protected: void use_frame_params(); + /** \brief Add common parameters */ + std::vector add_common_params(); + /** \brief The input PointCloud subscriber. */ rclcpp::Subscription::SharedPtr sub_input_; diff --git a/pcl_ros/include/pcl_ros/filters/passthrough.hpp b/pcl_ros/include/pcl_ros/filters/passthrough.hpp index f85d3810..8ae292cb 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,10 +59,10 @@ protected: */ inline void filter( - const PointCloud2::ConstPtr & input, const IndicesPtr & indices, - PointCloud2 & output) + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointCloud2 & output) override { - 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); @@ -74,19 +72,13 @@ protected: pcl_conversions::moveFromPCL(pcl_output, output); } - /** \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 +86,8 @@ private: 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 3c25846e..bee70d38 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,7 +67,7 @@ protected: */ inline void filter( - const PointCloud2::ConstPtr & input, const IndicesPtr & indices, + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, PointCloud2 & output) { pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2); @@ -89,25 +90,20 @@ private: 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/src/pcl_ros/filters/extract_indices.cpp b/pcl_ros/src/pcl_ros/filters/extract_indices.cpp index 94425611..4106c81e 100644 --- a/pcl_ros/src/pcl_ros/filters/extract_indices.cpp +++ b/pcl_ros/src/pcl_ros/filters/extract_indices.cpp @@ -56,6 +56,8 @@ pcl_ros::ExtractIndices::ExtractIndices(const rclcpp::NodeOptions & options) if (!result.successful) { throw std::runtime_error(result.reason); } + // TODO(daisukes): lazy subscription after rclcpp#2060 + subscribe(); } ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/pcl_ros/src/pcl_ros/filters/filter.cpp b/pcl_ros/src/pcl_ros/filters/filter.cpp index 5e60ed84..9ea418f3 100644 --- a/pcl_ros/src/pcl_ros/filters/filter.cpp +++ b/pcl_ros/src/pcl_ros/filters/filter.cpp @@ -61,7 +61,9 @@ /////////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::Filter::computePublish(const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices) +pcl_ros::Filter::computePublish( + const PointCloud2::ConstSharedPtr & input, + const IndicesPtr & indices) { PointCloud2 output; // Call the virtual method in the child @@ -116,21 +118,30 @@ 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(this, "input", rclcpp::QoS(max_queue_size_).get_rmw_qos_profile()); - sub_indices_filter_.subscribe(this, "indices", rclcpp::QoS(max_queue_size_).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", sensor_qos_profile); if (approximate_sync_) { sync_input_indices_a_ = std::make_shared>>(max_queue_size_); sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); - sync_input_indices_a_->registerCallback(std::bind(&Filter::input_indices_callback, this, std::placeholders::_1, std::placeholders::_2)); + sync_input_indices_a_->registerCallback( + std::bind( + &Filter::input_indices_callback, this, + std::placeholders::_1, std::placeholders::_2)); } else { sync_input_indices_e_ = std::make_shared>>(max_queue_size_); sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); - sync_input_indices_e_->registerCallback(std::bind(&Filter::input_indices_callback, this, std::placeholders::_1, std::placeholders::_2)); + 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 @@ -162,8 +173,6 @@ pcl_ros::Filter::Filter(std::string node_name, const rclcpp::NodeOptions & optio : PCLNode(node_name, options) { pub_output_ = create_publisher("output", max_queue_size_); - // TODO(daisukes): lazy subscription after rclcpp#2060 - subscribe(); RCLCPP_DEBUG(this->get_logger(), "Node successfully created."); } @@ -174,18 +183,25 @@ 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."; + 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."; + output_frame_desc.description = + "The output TF frame the data should be transformed into after processing, " + "if input.header.frame_id is different."; declare_parameter(output_frame_desc.name, rclcpp::ParameterValue(""), output_frame_desc); // Validate initial values using same callback callback_handle_ = - add_on_set_parameters_callback(std::bind(&Filter::config_callback, this, std::placeholders::_1)); + 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)); @@ -194,6 +210,60 @@ pcl_ros::Filter::use_frame_params() } } +////////////////////////////////////////////////////////////////////////////////////////////// +std::vector +pcl_ros::Filter::add_common_params() +{ + rcl_interfaces::msg::ParameterDescriptor ffn_desc; + ffn_desc.name = "filter_field_name"; + ffn_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + ffn_desc.description = "The field name used for filtering"; + declare_parameter(ffn_desc.name, rclcpp::ParameterValue("z"), ffn_desc); + + rcl_interfaces::msg::ParameterDescriptor flmin_desc; + flmin_desc.name = "filter_limit_min"; + flmin_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + flmin_desc.description = "The minimum allowed field value a point will be considered from"; + rcl_interfaces::msg::FloatingPointRange flmin_range; + flmin_range.from_value = -100000.0; + flmin_range.to_value = 100000.0; + flmin_desc.floating_point_range.push_back(flmin_range); + declare_parameter(flmin_desc.name, rclcpp::ParameterValue(0.0), flmin_desc); + + rcl_interfaces::msg::ParameterDescriptor flmax_desc; + flmax_desc.name = "filter_limit_max"; + flmax_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + flmax_desc.description = "The maximum allowed field value a point will be considered from"; + rcl_interfaces::msg::FloatingPointRange flmax_range; + flmax_range.from_value = -100000.0; + flmax_range.to_value = 100000.0; + flmax_desc.floating_point_range.push_back(flmax_range); + declare_parameter(flmax_desc.name, rclcpp::ParameterValue(1.0), flmax_desc); + + rcl_interfaces::msg::ParameterDescriptor flneg_desc; + flneg_desc.name = "filter_limit_negative"; + flneg_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + flneg_desc.description = + "Set to true if we want to return the data outside [filter_limit_min; filter_limit_max]."; + declare_parameter(flneg_desc.name, rclcpp::ParameterValue(false), flneg_desc); + + 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); + + return std::vector { + ffn_desc.name, + flmin_desc.name, + flmax_desc.name, + flneg_desc.name, + keep_organized_desc.name + }; +} + ////////////////////////////////////////////////////////////////////////////////////////////// rcl_interfaces::msg::SetParametersResult pcl_ros::Filter::config_callback(const std::vector & params) diff --git a/pcl_ros/src/pcl_ros/filters/passthrough.cpp b/pcl_ros/src/pcl_ros/filters/passthrough.cpp index 8ee183d2..461966b5 100644 --- a/pcl_ros/src/pcl_ros/filters/passthrough.cpp +++ b/pcl_ros/src/pcl_ros/filters/passthrough.cpp @@ -36,95 +36,94 @@ */ #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 param_names = add_common_params(); - return true; + callback_handle_ = + add_on_set_parameters_callback( + std::bind( + &PassThrough::config_callback, this, + std::placeholders::_1)); + + config_callback(get_parameters(param_names)); + // TODO(daisukes): lazy subscription after rclcpp#2060 + subscribe(); } ////////////////////////////////////////////////////////////////////////////////////////////// -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 4fc3f70e..05cf7d4b 100644 --- a/pcl_ros/src/pcl_ros/filters/project_inliers.cpp +++ b/pcl_ros/src/pcl_ros/filters/project_inliers.cpp @@ -36,46 +36,39 @@ */ #include "pcl_ros/filters/project_inliers.hpp" -#include -#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; + declare_parameter("copy_all_fields", rclcpp::ParameterValue(true)); + bool copy_all_fields = get_parameter("copy_all_fields").as_bool(); - pnh_->getParam("copy_all_data", copy_all_data); - pnh_->getParam("copy_all_fields", copy_all_fields); + pub_output_ = create_publisher("output", max_queue_size_); - pub_output_ = advertise(*pnh_, "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 +76,50 @@ pcl_ros::ProjectInliers::onInit() impl_.setCopyAllFields(copy_all_fields); impl_.setCopyAllData(copy_all_data); - onInitPostProcess(); + // TODO(daisukes): lazy subscription after rclcpp#2060 + subscribe(); } ////////////////////////////////////////////////////////////////////////////////////////////// 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 +131,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 +176,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/tests/filters/CMakeLists.txt b/pcl_ros/tests/filters/CMakeLists.txt index cb3088f6..edbf1dae 100644 --- a/pcl_ros/tests/filters/CMakeLists.txt +++ b/pcl_ros/tests/filters/CMakeLists.txt @@ -21,15 +21,44 @@ OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/share/ament_index/resource_index/rclcpp_components/pcl_ros_tests_filters" CONTENT "${components}") -ament_add_pytest_test(test_filter_extract_indices_node_component +# 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 +) + +# 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 +) diff --git a/pcl_ros/tests/filters/test_filter_component.py b/pcl_ros/tests/filters/test_filter_component.py index 53faf9ff..2c31983e 100644 --- a/pcl_ros/tests/filters/test_filter_component.py +++ b/pcl_ros/tests/filters/test_filter_component.py @@ -27,6 +27,7 @@ # POSSIBILITY OF SUCH DAMAGE. # +import ast import os import unittest @@ -46,14 +47,8 @@ from sensor_msgs.msg import PointCloud2 def generate_test_description(): dummy_plugin = os.getenv('DUMMY_PLUGIN') filter_plugin = os.getenv('FILTER_PLUGIN') - use_indices = os.getenv('USE_INDICES') - approximate_sync = os.getenv('APPROXIMATE_SYNC') + parameters = ast.literal_eval(os.getenv('PARAMETERS')) if 'PARAMETERS' in os.environ else {} - parameters = {} - if use_indices: - parameters['use_indices'] = (use_indices == 'true') - if approximate_sync: - parameters['approximate_sync'] = (approximate_sync == 'true') print(parameters) return launch.LaunchDescription([ diff --git a/pcl_ros/tests/filters/test_filter_executable.py b/pcl_ros/tests/filters/test_filter_executable.py index 57d08913..11bcdb6d 100644 --- a/pcl_ros/tests/filters/test_filter_executable.py +++ b/pcl_ros/tests/filters/test_filter_executable.py @@ -1,3 +1,33 @@ +# +# 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 @@ -17,14 +47,11 @@ from sensor_msgs.msg import PointCloud2 def generate_test_description(): dummy_plugin = os.getenv('DUMMY_PLUGIN') filter_executable = os.getenv('FILTER_EXECUTABLE') - use_indices = os.getenv('USE_INDICES') - approximate_sync = os.getenv('APPROXIMATE_SYNC') + parameters = ast.literal_eval(os.getenv('PARAMETERS')) if 'PARAMETERS' in os.environ else {} ros_arguments = ["-r", "input:=point_cloud2"] - if use_indices: - ros_arguments.extend(["-p", "use_indices:={}".format(use_indices)]) - if approximate_sync: - ros_arguments.extend(["-p", "approximate_sync:={}".format(approximate_sync)]) + for key in parameters.keys(): + ros_arguments.extend(["-p", "{}:={}".format(key, parameters[key])]) return launch.LaunchDescription([