From 387f5b158ba7772d96f883718eb22d311eaa201b Mon Sep 17 00:00:00 2001 From: Daisuke Sato <43101027+daisukes@users.noreply.github.com> Date: Mon, 19 Dec 2022 14:13:06 -0500 Subject: [PATCH] migrate abstract filter node (#388) * migrate abstract filter node Signed-off-by: Daisuke Sato * remove use_frame_params from constructor and make a function for the logic Signed-off-by: Daisuke Sato Signed-off-by: Daisuke Sato --- pcl_ros/CMakeLists.txt | 14 +- pcl_ros/include/pcl_ros/filters/filter.hpp | 63 +++---- pcl_ros/src/pcl_ros/filters/filter.cpp | 202 +++++++++++---------- 3 files changed, 143 insertions(+), 136 deletions(-) diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 29efc189..5952e859 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -80,19 +80,19 @@ ament_target_dependencies(pcl_ros_tf # # ### Declare the pcl_ros_filters library -#add_library(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/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) +) +target_link_libraries(pcl_ros_filters pcl_ros_tf ${PCL_LIBRARIES}) +ament_target_dependencies(pcl_ros_filters ${dependencies}) +class_loader_hide_library_symbols(pcl_ros_filters) # ### Declare the pcl_ros_segmentation library #add_library (pcl_ros_segmentation @@ -184,7 +184,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/filter.hpp b/pcl_ros/include/pcl_ros/filters/filter.hpp index e589c0cb..a0cd0d20 100644 --- a/pcl_ros/include/pcl_ros/filters/filter.hpp +++ b/pcl_ros/include/pcl_ros/filters/filter.hpp @@ -39,10 +39,10 @@ #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 +52,27 @@ 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 The input PointCloud subscriber. */ - ros::Subscriber sub_input_; + rclcpp::Subscription::SharedPtr sub_input_; message_filters::Subscriber sub_input_filter_; @@ -96,18 +104,7 @@ protected: 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 +113,7 @@ protected: */ 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 +124,34 @@ protected: virtual void unsubscribe(); - /** \brief Nodelet initialization routine. */ - virtual void - onInit(); - /** \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/src/pcl_ros/filters/filter.cpp b/pcl_ros/src/pcl_ros/filters/filter.cpp index ad5fb75d..5e60ed84 100644 --- a/pcl_ros/src/pcl_ros/filters/filter.cpp +++ b/pcl_ros/src/pcl_ros/filters/filter.cpp @@ -37,7 +37,6 @@ #include "pcl_ros/filters/filter.hpp" #include -#include #include "pcl_ros/transforms.hpp" /*//#include @@ -62,42 +61,42 @@ /////////////////////////////////////////////////////////////////////////////////////////////////// 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); - 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 +105,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 +116,32 @@ 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_); + 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()); 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( + this->create_subscription( "input", max_queue_size_, - bind(&Filter::input_indices_callback, this, _1, pcl_msgs::PointIndicesConstPtr())); + callback); } } @@ -150,114 +153,123 @@ 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(); - - // 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; - } - - pub_output_ = advertise(*pnh_, "output", max_queue_size_); - - // 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); - } - - NODELET_DEBUG("[%s::onInit] Nodelet successfully created.", getName().c_str()); + pub_output_ = create_publisher("output", max_queue_size_); + // TODO(daisukes): lazy subscription after rclcpp#2060 + subscribe(); + RCLCPP_DEBUG(this->get_logger(), "Node successfully created."); } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::Filter::config_callback(pcl_ros::FilterConfig & config, uint32_t level) +pcl_ros::Filter::use_frame_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()); + rcl_interfaces::msg::ParameterDescriptor input_frame_desc; + input_frame_desc.name = "input_frame"; + input_frame_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + input_frame_desc.description = "The input TF frame the data should be transformed into before processing, if input.header.frame_id is different."; + declare_parameter(input_frame_desc.name, rclcpp::ParameterValue(""), input_frame_desc); + + rcl_interfaces::msg::ParameterDescriptor output_frame_desc; + output_frame_desc.name = "output_frame"; + output_frame_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + output_frame_desc.description = "The output TF frame the data should be transformed into after processing, if input.header.frame_id is different."; + declare_parameter(output_frame_desc.name, rclcpp::ParameterValue(""), output_frame_desc); + + // Validate initial values using same callback + callback_handle_ = + add_on_set_parameters_callback(std::bind(&Filter::config_callback, this, std::placeholders::_1)); + + std::vector param_names{input_frame_desc.name, output_frame_desc.name}; + auto result = config_callback(get_parameters(param_names)); + if (!result.successful) { + throw std::runtime_error(result.reason); } - 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()); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +rcl_interfaces::msg::SetParametersResult +pcl_ros::Filter::config_callback(const std::vector & params) +{ + std::lock_guard lock(mutex_); + + for (const rclcpp::Parameter & param : params) { + if (param.get_name() == "input_frame") { + if (tf_input_frame_ != param.as_string()) { + tf_input_frame_ = param.as_string(); + RCLCPP_DEBUG(get_logger(), "Setting the input frame to: %s.", tf_input_frame_.c_str()); + } + } + if (param.get_name() == "output_frame") { + if (tf_output_frame_ != param.as_string()) { + tf_output_frame_ = param.as_string(); + RCLCPP_DEBUG(get_logger(), "Setting the output frame to: %s.", tf_output_frame_.c_str()); + } + } } + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; } ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::Filter::input_indices_callback( - const PointCloud2::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; }