diff --git a/pcl_ros/include/pcl_ros/filters/extract_indices.hpp b/pcl_ros/include/pcl_ros/filters/extract_indices.hpp index 95740f600..c8d89d659 100644 --- a/pcl_ros/include/pcl_ros/filters/extract_indices.hpp +++ b/pcl_ros/include/pcl_ros/filters/extract_indices.hpp @@ -38,62 +38,53 @@ #ifndef PCL_ROS__FILTERS__EXTRACT_INDICES_HPP_ #define PCL_ROS__FILTERS__EXTRACT_INDICES_HPP_ -// PCL includes #include +#include +#include #include "pcl_ros/filters/filter.hpp" -#include "pcl_ros/ExtractIndicesConfig.hpp" namespace pcl_ros { -/** \brief @b ExtractIndices extracts a set of indices from a PointCloud as a separate PointCloud. +/** \brief @b ExtractIndices extracts the given set of indices from an input point cloud dataset. * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored. * \author Radu Bogdan Rusu */ class ExtractIndices : public Filter { protected: - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr> srv_; + /** \brief The PCL filter implementation used. */ + pcl::ExtractIndices impl_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW - /** \brief Call the actual filter. +protected: + /** \brief Call the actual filter. * \param input the input point cloud dataset * \param indices the input set of indices to use from \a input * \param output the resultant filtered dataset */ - inline void - filter( - const PointCloud2::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); - } + void filter( + const PointCloud2ConstPtr & 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 */ - virtual bool - child_init(ros::NodeHandle & nh, bool & has_service); + bool child_init(bool has_service = false) override; - /** \brief Dynamic reconfigure service callback. */ - void - config_callback(pcl_ros::ExtractIndicesConfig & config, uint32_t level); + /** \brief Parameter callback + * \param parameters the changed parameters + */ + rcl_interfaces::msg::SetParametersResult config_callback( + const std::vector & parameters) override; private: - /** \brief The PCL filter implementation used. */ - pcl::ExtractIndices impl_; - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /** \brief Parameter for negative extraction */ + bool negative_; }; } // namespace pcl_ros -#endif // PCL_ROS__FILTERS__EXTRACT_INDICES_HPP_ +#endif // PCL_ROS__FILTERS__EXTRACT_INDICES_HPP_ \ No newline at end of file diff --git a/pcl_ros/include/pcl_ros/filters/filter.hpp b/pcl_ros/include/pcl_ros/filters/filter.hpp index e589c0cb7..56fcc9aff 100644 --- a/pcl_ros/include/pcl_ros/filters/filter.hpp +++ b/pcl_ros/include/pcl_ros/filters/filter.hpp @@ -38,129 +38,127 @@ #ifndef PCL_ROS__FILTERS__FILTER_HPP_ #define PCL_ROS__FILTERS__FILTER_HPP_ -#include -#include +#include +#include +#include + +// TF +#include +#include + +// Message filters +#include +#include +#include +#include + +#include #include +#include + #include "pcl_ros/pcl_nodelet.hpp" -#include "pcl_ros/FilterConfig.hpp" namespace pcl_ros { namespace sync_policies = message_filters::sync_policies; -/** \brief @b Filter represents the base filter class. Some generic 3D operations that are - * applicable to all filters are defined here as static methods. +//////////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b Filter represents the base filter class. All filters must inherit from this class. * \author Radu Bogdan Rusu */ class Filter : public PCLNodelet { public: - typedef sensor_msgs::PointCloud2 PointCloud2; + typedef sensor_msgs::msg::PointCloud2 PointCloud2; + typedef PointCloud2::SharedPtr PointCloud2Ptr; + typedef PointCloud2::ConstSharedPtr PointCloud2ConstPtr; - typedef pcl::IndicesPtr IndicesPtr; - typedef pcl::IndicesConstPtr IndicesConstPtr; + typedef pcl_msgs::msg::PointIndices PointIndices; + typedef PointIndices::SharedPtr PointIndicesPtr; + typedef PointIndices::ConstSharedPtr PointIndicesConstPtr; - Filter() {} + /** \brief Empty constructor. */ + Filter() : PCLNodelet("filter_node"), tf_input_frame_(""), tf_output_frame_("") {} -protected: - /** \brief The input PointCloud subscriber. */ - ros::Subscriber sub_input_; + /** \brief Compute the actual filtering and publish the result. + * \param input the input point cloud dataset. + * \param indices the input set of indices to use from \a input + */ + void computePublish(const PointCloud2ConstPtr & input, const IndicesPtr & indices); - message_filters::Subscriber sub_input_filter_; +protected: + /** \brief The input PointCloud2 subscriber. */ + rclcpp::Subscription::SharedPtr sub_input_; /** \brief The desired user filter field name. */ std::string filter_field_name_; - /** \brief The minimum allowed filter value a point will be considered from. */ + /** \brief The minimum allowed filter value a point will be filtered with. */ double filter_limit_min_; - /** \brief The maximum allowed filter value a point will be considered from. */ + /** \brief The maximum allowed filter value a point will be filtered with. */ double filter_limit_max_; - /** \brief Set to true if we want to return the data outside - * (\a filter_limit_min_;\a filter_limit_max_). Default: false. - */ + /** \brief Set to true if point filtering should be limited to a certain values range. */ bool filter_limit_negative_; - /** \brief The input TF frame the data should be transformed into, - * if input.header.frame_id is different. - */ + /** \brief The input TF frame the data should be transformed into, if input.header.frame_id is different. */ std::string tf_input_frame_; /** \brief The original data input TF frame. */ std::string tf_input_orig_frame_; - /** \brief The output TF frame the data should be transformed into, - * if input.header.frame_id is different. - */ + /** \brief The output TF frame the data should be transformed into, if input.header.frame_id is different. */ std::string tf_output_frame_; - /** \brief Internal mutex. */ - boost::mutex mutex_; + /** \brief TF2 buffer and listener for transforms. */ + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; - /** \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; - } - - /** \brief Virtual abstract filter method. To be implemented by every child. - * \param input the input point cloud dataset. - * \param indices a pointer to the vector of point indices to use. - * \param output the resultant filtered PointCloud2 - */ - virtual void - filter( - const PointCloud2::ConstPtr & input, const IndicesPtr & indices, - PointCloud2 & output) = 0; + /** \brief The message filter subscriber for PointCloud2. */ + message_filters::Subscriber sub_input_filter_; - /** \brief Lazy transport subscribe routine. */ - virtual void - subscribe(); + /** \brief The message filter subscriber for PointIndices. */ + message_filters::Subscriber sub_indices_filter_; - /** \brief Lazy transport unsubscribe routine. */ - virtual void - unsubscribe(); + /** \brief Synchronized input, and indices.*/ + std::shared_ptr>> sync_input_indices_e_; + std::shared_ptr>> sync_input_indices_a_; - /** \brief Nodelet initialization routine. */ - virtual void - onInit(); + /** \brief Parameter callback handle. */ + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; - /** \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. + /** \brief Child initialization routine. Internal method. */ + virtual bool child_init(bool has_service = false) = 0; + + /** \brief Filter a Point Cloud. + * \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 */ - void - computePublish(const PointCloud2::ConstPtr & input, const IndicesPtr & indices); + virtual void filter(const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) = 0; -private: - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr> srv_; + /** \brief Parameter callback + * \param parameters the changed parameters + */ + virtual rcl_interfaces::msg::SetParametersResult config_callback(const std::vector & parameters); - /** \brief Synchronized input, and indices.*/ - boost::shared_ptr>> sync_input_indices_e_; - boost::shared_ptr>> sync_input_indices_a_; + /** \brief PointCloud2 + Indices data callback. */ + void input_indices_callback(const PointCloud2ConstPtr & cloud, const PointIndicesConstPtr & indices); - /** \brief Dynamic reconfigure service callback. */ - virtual void - config_callback(pcl_ros::FilterConfig & config, uint32_t level); +private: + /** \brief Nodelet initialization routine. */ + void onInit(); - /** \brief PointCloud2 + Indices data callback. */ - void - input_indices_callback( - const PointCloud2::ConstPtr & cloud, - const PointIndicesConstPtr & indices); + /** \brief LazyNodelet connection routine. */ + void subscribe(); + void unsubscribe(); public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace pcl_ros -#endif // PCL_ROS__FILTERS__FILTER_HPP_ +#endif // PCL_ROS__FILTERS__FILTER_HPP_ \ No newline at end of file diff --git a/pcl_ros/include/pcl_ros/filters/passthrough.hpp b/pcl_ros/include/pcl_ros/filters/passthrough.hpp index f85d38102..6ae10746a 100644 --- a/pcl_ros/include/pcl_ros/filters/passthrough.hpp +++ b/pcl_ros/include/pcl_ros/filters/passthrough.hpp @@ -52,7 +52,7 @@ class PassThrough : public Filter { protected: /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr> srv_; + // boost::shared_ptr> srv_; /** \brief Call the actual filter. * \param input the input point cloud dataset @@ -64,7 +64,7 @@ class PassThrough : public Filter const PointCloud2::ConstPtr & 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); @@ -78,15 +78,15 @@ class PassThrough : public Filter * \param nh ROS node handle * \param has_service set to true if the child has a Dynamic Reconfigure service */ - bool - child_init(ros::NodeHandle & nh, bool & has_service); + // bool + // child_init(ros::NodeHandle & nh, bool & has_service); /** \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); + // void + // config_callback(pcl_ros::FilterConfig & config, uint32_t level); private: /** \brief The PCL filter implementation used. */ diff --git a/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp b/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp index 54c7c4596..d4a561031 100644 --- a/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp +++ b/pcl_ros/include/pcl_ros/filters/voxel_grid.hpp @@ -38,54 +38,64 @@ #ifndef PCL_ROS__FILTERS__VOXEL_GRID_HPP_ #define PCL_ROS__FILTERS__VOXEL_GRID_HPP_ -// PCL includes #include -#include "pcl_ros/filters/filter.hpp" +#include +#include -// Dynamic reconfigure -#include "pcl_ros/VoxelGridConfig.hpp" +#include "pcl_ros/filters/filter.hpp" namespace pcl_ros { /** \brief @b VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data. + * + * The @b VoxelGrid class creates a *3D voxel grid* (think about a voxel + * grid as a set of tiny 3D boxes in space) over the input point cloud data. + * Then, in each *voxel* (i.e., 3D box), all the points present will be + * approximated (i.e., *downsampled*) with their centroid. This approach is + * a bit slower than approximating them with the center of the voxel, but it + * represents the underlying surface more accurately. + * * \author Radu Bogdan Rusu */ 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. +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +protected: + /** \brief Call the actual filter. * \param input the input point cloud dataset * \param indices the input set of indices to use from \a input * \param output the resultant filtered dataset */ - virtual void - filter( - const PointCloud2::ConstPtr & input, const IndicesPtr & indices, - PointCloud2 & output); + void filter( + const PointCloud2ConstPtr & 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 */ - bool - child_init(ros::NodeHandle & nh, bool & has_service); + bool child_init(bool has_service = false) override; - /** \brief Dynamic reconfigure callback - * \param config the config object - * \param level the dynamic reconfigure level + /** \brief Parameter callback + * \param parameters the changed parameters */ - void - config_callback(pcl_ros::VoxelGridConfig & config, uint32_t level); + rcl_interfaces::msg::SetParametersResult config_callback( + const std::vector & parameters) override; -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW +private: + /** \brief Voxel grid parameters */ + double leaf_size_; + double filter_limit_min_; + double filter_limit_max_; + bool filter_limit_negative_; + std::string filter_field_name_; }; } // namespace pcl_ros -#endif // PCL_ROS__FILTERS__VOXEL_GRID_HPP_ +#endif // PCL_ROS__FILTERS__VOXEL_GRID_HPP_ \ No newline at end of file diff --git a/pcl_ros/include/pcl_ros/pcl_nodelet.hpp b/pcl_ros/include/pcl_ros/pcl_nodelet.hpp index 4806338f7..00c2d86d7 100644 --- a/pcl_ros/include/pcl_ros/pcl_nodelet.hpp +++ b/pcl_ros/include/pcl_ros/pcl_nodelet.hpp @@ -31,230 +31,313 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * - * $Id: pcl_nodelet.h 33238 2010-03-11 00:46:58Z rusu $ - * */ -/** - -\author Radu Bogdan Rusu - -**/ - #ifndef PCL_ROS__PCL_NODELET_HPP_ #define PCL_ROS__PCL_NODELET_HPP_ -#include -// PCL includes -#include -#include -#include -#include -#include -// ROS Nodelet includes -#include +// ROS 2 includes +#include +#include +#include +#include +#include + +// Message filters #include #include #include #include -// Include TF -#include +// TF2 +#include +#include +#include -// STL -#include +// PCL +#include +#include +#include -#include "pcl_ros/point_cloud.hpp" +// PCL ROS +#include -using pcl_conversions::fromPCL; +// STL +#include +#include +#include namespace pcl_ros { -//////////////////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////////////////// -/** \brief @b PCLNodelet represents the base PCL Nodelet class. All PCL nodelets should - * inherit from this class. - **/ -class PCLNodelet : public nodelet_topic_tools::NodeletLazy + +/** \brief @b PCLNodelet represents the base PCL Node class for ROS 2. + * All PCL nodes should inherit from this class. + */ +class PCLNodelet : public rclcpp::Node { public: - typedef sensor_msgs::PointCloud2 PointCloud2; + // ROS 2 message types + typedef sensor_msgs::msg::PointCloud2 PointCloud2; + typedef PointCloud2::SharedPtr PointCloud2Ptr; + typedef PointCloud2::ConstSharedPtr PointCloud2ConstPtr; + + 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; + + // PCL types with TypeAdapter support typedef pcl::PointCloud PointCloud; - typedef boost::shared_ptr PointCloudPtr; - typedef boost::shared_ptr PointCloudConstPtr; + typedef std::shared_ptr PointCloudPtr; + typedef std::shared_ptr PointCloudConstPtr; - typedef pcl_msgs::PointIndices PointIndices; - typedef PointIndices::Ptr PointIndicesPtr; - typedef PointIndices::ConstPtr PointIndicesConstPtr; + typedef pcl::PointCloud PointCloudRGB; + typedef std::shared_ptr PointCloudRGBPtr; + typedef std::shared_ptr PointCloudRGBConstPtr; - typedef pcl_msgs::ModelCoefficients ModelCoefficients; - typedef ModelCoefficients::Ptr ModelCoefficientsPtr; - typedef ModelCoefficients::ConstPtr ModelCoefficientsConstPtr; + typedef pcl::PointCloud PointCloudNormal; + typedef std::shared_ptr PointCloudNormalPtr; + typedef std::shared_ptr PointCloudNormalConstPtr; typedef pcl::IndicesPtr IndicesPtr; typedef pcl::IndicesConstPtr IndicesConstPtr; - /** \brief Empty constructor. */ - PCLNodelet() - : use_indices_(false), latched_indices_(false), - max_queue_size_(3), approximate_sync_(false) {} + /** \brief Constructor. */ + explicit PCLNodelet(const std::string & node_name, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : rclcpp::Node(node_name, options), + use_indices_(false), + latched_indices_(false), + max_queue_size_(3), + approximate_sync_(false), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_) + { + // Initialize the node but don't call onInit here - let derived classes do it + } + + virtual ~PCLNodelet() = default; 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 latched_indices_ and approximate_sync. - **/ + /** \brief Set to true if point indices are used. */ bool use_indices_; - /** \brief Set to true if the indices topic is latched. - * - * 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. - **/ + + /** \brief Set to true if the indices topic is latched. */ bool latched_indices_; /** \brief The message filter subscriber for PointCloud2. */ - message_filters::Subscriber sub_input_filter_; + message_filters::Subscriber sub_input_filter_; /** \brief The message filter subscriber for PointIndices. */ message_filters::Subscriber sub_indices_filter_; - /** \brief The output PointCloud publisher. */ - ros::Publisher pub_output_; + /** \brief The output PointCloud publisher (can use PCL types directly). */ + rclcpp::Publisher::SharedPtr 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). - **/ + /** \brief True if we use an approximate time synchronizer. */ bool approximate_sync_; - /** \brief TF listener object. */ - tf::TransformListener tf_listener_; + /** \brief TF2 buffer. */ + tf2_ros::Buffer tf_buffer_; - /** \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") - */ + /** \brief TF2 listener object. */ + tf2_ros::TransformListener tf_listener_; + + /** \brief Mutex for thread safety. */ + std::mutex mutex_; + + /** \brief Test whether a PointCloud2 message is valid. */ inline bool - isValid(const PointCloud2::ConstPtr & cloud, const std::string & topic_name = "input") + isValid(const PointCloud2ConstPtr & cloud, const std::string & topic_name = "input") { - if (cloud->width * cloud->height * cloud->point_step != cloud->data.size()) { - NODELET_WARN( - "[%s] Invalid PointCloud (data = %zu, width = %d, height = %d, step = %d) " - "with stamp %f, and frame %s on topic %s received!", - getName().c_str(), - cloud->data.size(), cloud->width, cloud->height, cloud->point_step, - cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( - topic_name).c_str()); + if (!cloud) { + RCLCPP_WARN(this->get_logger(), "[%s] Null PointCloud2 on topic %s received!", + this->get_name(), topic_name.c_str()); + return false; + } + if (cloud->width == 0 || cloud->height == 0) { + RCLCPP_WARN(this->get_logger(), + "[%s] Empty PointCloud2 (width = %d, height = %d) " + "with frame %s on topic %s received!", + this->get_name(), cloud->width, cloud->height, + cloud->header.frame_id.c_str(), topic_name.c_str()); return false; } + + if (cloud->data.empty()) { + RCLCPP_WARN(this->get_logger(), + "[%s] PointCloud2 with no data (width = %d, height = %d) " + "with frame %s on topic %s received!", + this->get_name(), cloud->width, cloud->height, + 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") - */ + /** \brief Test whether a PCL PointCloud is valid. */ inline bool isValid(const PointCloudConstPtr & cloud, const std::string & topic_name = "input") { - if (cloud->width * cloud->height != cloud->points.size()) { - NODELET_WARN( - "[%s] Invalid PointCloud (points = %zu, width = %d, height = %d) " - "with stamp %f, and frame %s on topic %s received!", - getName().c_str(), cloud->points.size(), cloud->width, cloud->height, - fromPCL(cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), - pnh_->resolveName(topic_name).c_str()); + if (!cloud) { + RCLCPP_WARN(this->get_logger(), "[%s] Null PCL PointCloud on topic %s received!", + this->get_name(), topic_name.c_str()); + return false; + } + + if (cloud->points.empty()) { + RCLCPP_WARN(this->get_logger(), + "[%s] Empty PCL PointCloud (points = %zu, width = %d, height = %d) " + "with frame %s on topic %s received!", + this->get_name(), cloud->points.size(), cloud->width, cloud->height, + cloud->header.frame_id.c_str(), topic_name.c_str()); + return false; + } + if (cloud->width * cloud->height != cloud->points.size()) { + RCLCPP_WARN(this->get_logger(), + "[%s] Invalid PCL PointCloud (points = %zu, width = %d, height = %d) " + "with frame %s on topic %s received!", + this->get_name(), cloud->points.size(), cloud->width, cloud->height, + 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") - */ + /** \brief Test whether a PointIndices message is valid. */ inline bool isValid(const PointIndicesConstPtr & indices, const std::string & topic_name = "indices") { - /*if (indices->indices.empty ()) - { - NODELET_WARN ("[%s] Empty indices (values = %zu) " - "with stamp %f, and frame %s on topic %s received!", - getName ().c_str (), indices->indices.size (), indices->header.stamp.toSec (), - indices->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ()); - return (true); - }*/ + if (!indices) { + RCLCPP_WARN(this->get_logger(), "[%s] Null PointIndices on topic %s received!", + this->get_name(), topic_name.c_str()); + return false; + } + + if (indices->indices.empty()) { + RCLCPP_DEBUG(this->get_logger(), + "[%s] Empty PointIndices (size = %zu) " + "with frame %s on topic %s received!", + this->get_name(), indices->indices.size(), + indices->header.frame_id.c_str(), topic_name.c_str()); + return true; + } + 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") - */ + /** \brief Test whether a ModelCoefficients message is valid. */ inline bool isValid(const ModelCoefficientsConstPtr & model, const std::string & topic_name = "model") { - /*if (model->values.empty ()) - { - NODELET_WARN ("[%s] Empty model (values = %zu) with stamp %f, " - "and frame %s on topic %s received!", - getName ().c_str (), model->values.size (), model->header.stamp.toSec (), - model->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ()); - return (false); - }*/ + if (!model) { + RCLCPP_WARN(this->get_logger(), "[%s] Null ModelCoefficients on topic %s received!", + this->get_name(), topic_name.c_str()); + return false; + } + + if (model->values.empty()) { + RCLCPP_WARN(this->get_logger(), + "[%s] Empty ModelCoefficients (size = %zu) " + "with frame %s on topic %s received!", + this->get_name(), model->values.size(), + 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. - **/ + /** \brief Subscribe/unsubscribe routines. Override in derived classes. */ virtual void subscribe() {} virtual void unsubscribe() {} - /** \brief Nodelet initialization routine. Reads in global parameters used by all nodelets. */ - virtual void - onInit() + /** \brief Node initialization routine. */ + virtual void onInit() { - nodelet_topic_tools::NodeletLazy::onInit(); - - // Parameters that we care about only at startup - pnh_->getParam("max_queue_size", max_queue_size_); - - // ---[ Optional parameters - pnh_->getParam("use_indices", use_indices_); - pnh_->getParam("latched_indices", latched_indices_); - pnh_->getParam("approximate_sync", approximate_sync_); - - NODELET_DEBUG( - "[%s::onInit] PCL Nodelet successfully created with the following parameters:\n" - " - approximate_sync : %s\n" - " - use_indices : %s\n" - " - latched_indices : %s\n" - " - max_queue_size : %d", - getName().c_str(), - (approximate_sync_) ? "true" : "false", - (use_indices_) ? "true" : "false", - (latched_indices_) ? "true" : "false", - max_queue_size_); + // Declare parameters with default values + this->declare_parameter("max_queue_size", 3); + this->declare_parameter("use_indices", false); + this->declare_parameter("latched_indices", false); + this->declare_parameter("approximate_sync", false); + + // Get parameter values + max_queue_size_ = this->get_parameter("max_queue_size").as_int(); + use_indices_ = this->get_parameter("use_indices").as_bool(); + latched_indices_ = this->get_parameter("latched_indices").as_bool(); + approximate_sync_ = this->get_parameter("approximate_sync").as_bool(); + + RCLCPP_DEBUG(this->get_logger(), + "[%s::onInit] PCL Node successfully created with parameters:\n" + " - approximate_sync : %s\n" + " - use_indices : %s\n" + " - latched_indices : %s\n" + " - max_queue_size : %d", + this->get_name(), + approximate_sync_ ? "true" : "false", + use_indices_ ? "true" : "false", + latched_indices_ ? "true" : "false", + max_queue_size_); + + onInitPostProcess(); + } + + /** \brief Post-initialization routine. */ + virtual void onInitPostProcess() + { + subscribe(); + } + + /** \brief Helper function to create a PCL publisher. */ + template + typename rclcpp::Publisher>::SharedPtr + advertise(const std::string & topic, const rclcpp::QoS & qos) + { + return this->create_publisher>(topic, qos); + } + + /** \brief Helper function to create a publisher with queue size. */ + template + typename rclcpp::Publisher::SharedPtr + advertise(const std::string & topic, size_t queue_size) + { + return this->create_publisher(topic, rclcpp::QoS(queue_size)); + } + + /** \brief Helper to publish a PCL point cloud directly. */ + void publish(const PointCloud & pcl_cloud) + { + if (pub_output_) { + pub_output_->publish(pcl_cloud); + } + } + + /** \brief Helper to publish with frame ID and timestamp. */ + void publish(const PointCloud & pcl_cloud, const std::string & frame_id) + { + if (pub_output_) { + PointCloud cloud_copy = pcl_cloud; + cloud_copy.header.frame_id = frame_id; + rclcpp::Time now = this->get_clock()->now(); + cloud_copy.header.stamp = static_cast(now.nanoseconds()) / 1000ull; + pub_output_->publish(cloud_copy); + } } public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; + } // namespace pcl_ros -#endif // PCL_ROS__PCL_NODELET_HPP_ +#endif // PCL_ROS__PCL_NODELET_HPP_ \ No newline at end of file diff --git a/pcl_ros/include/pcl_ros/point_cloud.hpp b/pcl_ros/include/pcl_ros/point_cloud.hpp index 435cc7170..52208d9f7 100644 --- a/pcl_ros/include/pcl_ros/point_cloud.hpp +++ b/pcl_ros/include/pcl_ros/point_cloud.hpp @@ -32,341 +32,26 @@ * POSSIBILITY OF SUCH DAMAGE. */ + #ifndef PCL_ROS__POINT_CLOUD_HPP__ #define PCL_ROS__POINT_CLOUD_HPP__ -// test if testing machinery can be implemented -#if defined(__cpp_rvalue_references) && defined(__cpp_constexpr) -#define ROS_POINTER_COMPATIBILITY_IMPLEMENTED 1 -#else -#define ROS_POINTER_COMPATIBILITY_IMPLEMENTED 0 -#endif - #include -#include // for PCL_VERSION_COMPARE -#if PCL_VERSION_COMPARE(>=, 1, 11, 0) -#include -#else -#include -#endif // PCL_VERSION_COMPARE(>=, 1, 11, 0) -#include +#include #include -#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED -#if PCL_VERSION_COMPARE(>=, 1, 11, 0) -#include -#elif PCL_VERSION_COMPARE(>=, 1, 10, 0) -#include -#endif -#endif +#include #include #include -#include -#include +#include #include -#include -#include -#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED -#include #include -#endif +#include namespace pcl { namespace detail { -template -struct FieldStreamer -{ - explicit FieldStreamer(Stream & stream) - : stream_(stream) {} - - template - void operator()() - { - const char * name = pcl::traits::name::value; - std::uint32_t name_length = strlen(name); - stream_.next(name_length); - if (name_length > 0) { - memcpy(stream_.advance(name_length), name, name_length); - } - - std::uint32_t offset = pcl::traits::offset::value; - stream_.next(offset); - - std::uint8_t datatype = pcl::traits::datatype::value; - stream_.next(datatype); - - std::uint32_t count = pcl::traits::datatype::size; - stream_.next(count); - } - - Stream & stream_; -}; - -template -struct FieldsLength -{ - FieldsLength() - : length(0) {} - - template - void operator()() - { - std::uint32_t name_length = strlen(pcl::traits::name::value); - length += name_length + 13; - } - - std::uint32_t length; -}; -} // namespace detail -} // namespace pcl - -namespace ros -{ -// In ROS 1.3.1+, we can specialize the functor used to create PointCloud objects -// on the subscriber side. This allows us to generate the mapping between message -// data and object fields only once and reuse it. -#if 0 //ROS_VERSION_MINIMUM(1, 3, 1) -template -struct DefaultMessageCreator> -{ - boost::shared_ptr mapping_; - DefaultMessageCreator() - : mapping_(boost::make_shared() ) - { - } - - boost::shared_ptr> operator()() - { - boost::shared_ptr> msg(new pcl::PointCloud()); - pcl::detail::getMapping(*msg) = mapping_; - return msg; - } -}; -#endif - -namespace message_traits -{ -template -struct MD5Sum> -{ - static const char * value() {return MD5Sum::value();} - static const char * value(const pcl::PointCloud &) {return value();} - - static const uint64_t static_value1 = MD5Sum::static_value1; - static const uint64_t static_value2 = MD5Sum::static_value2; - - // If the definition of sensor_msgs/PointCloud2 changes, we'll get a compile error here. - static_assert(static_value1 == 0x1158d486dd51d683ULL); - static_assert(static_value2 == 0xce2f1be655c3c181ULL); -}; - -template -struct DataType> -{ - static const char * value() {return DataType::value();} - static const char * value(const pcl::PointCloud &) {return value();} -}; - -template -struct Definition> -{ - static const char * value() {return Definition::value();} - static const char * value(const pcl::PointCloud &) {return value();} -}; - -// pcl point clouds message don't have a ROS compatible header -// the specialized meta functions below (TimeStamp and FrameId) -// can be used to get the header data. -template -struct HasHeader>: FalseType {}; - -template -struct TimeStamp> -{ - // This specialization could be dangerous, but it's the best I can do. - // If this TimeStamp struct is destroyed before they are done with the - // pointer returned by the first functions may go out of scope, but there - // isn't a lot I can do about that. This is a good reason to refuse to - // returning pointers like this... - static rclcpp::Time * pointer(typename pcl::PointCloud & m) - { - header_.reset(new std_msgs::msg::Header()); - pcl_conversions::fromPCL(m.header, *(header_)); - return &(header_->stamp); - } - static rclcpp::Time const * pointer(const typename pcl::PointCloud & m) - { - header_const_.reset(new std_msgs::msg::Header()); - pcl_conversions::fromPCL(m.header, *(header_const_)); - return &(header_const_->stamp); - } - static rclcpp::Time value(const typename pcl::PointCloud & m) - { - return pcl_conversions::fromPCL(m.header).stamp; - } - -private: - static boost::shared_ptr header_; - static boost::shared_ptr header_const_; -}; - -template -struct FrameId> -{ - static std::string * pointer(pcl::PointCloud & m) {return &m.header.frame_id;} - static std::string const * pointer(const pcl::PointCloud & m) {return &m.header.frame_id;} - static std::string value(const pcl::PointCloud & m) {return m.header.frame_id;} -}; - -} // namespace message_traits - -namespace serialization -{ -template -struct Serializer> -{ - template - inline static void write(Stream & stream, const pcl::PointCloud & m) - { - stream.next(m.header); - - // Ease the user's burden on specifying width/height for unorganized datasets - uint32_t height = m.height, width = m.width; - if (height == 0 && width == 0) { - width = m.points.size(); - height = 1; - } - stream.next(height); - stream.next(width); - - // Stream out point field metadata - typedef typename pcl::traits::fieldList::type FieldList; - uint32_t fields_size = boost::mpl::size::value; - stream.next(fields_size); - pcl::for_each_type(pcl::detail::FieldStreamer(stream)); - - // Assume little-endian... - uint8_t is_bigendian = false; - stream.next(is_bigendian); - - // Write out point data as binary blob - uint32_t point_step = sizeof(T); - stream.next(point_step); - uint32_t row_step = point_step * width; - stream.next(row_step); - uint32_t data_size = row_step * height; - stream.next(data_size); - memcpy(stream.advance(data_size), m.points.data(), data_size); - - uint8_t is_dense = m.is_dense; - stream.next(is_dense); - } - - template - inline static void read(Stream & stream, pcl::PointCloud & m) - { - std_msgs::msg::Header header; - stream.next(header); - pcl_conversions::toPCL(header, m.header); - stream.next(m.height); - stream.next(m.width); - - /// @todo Check that fields haven't changed! - std::vector fields; - stream.next(fields); - - // Construct field mapping if deserializing for the first time - boost::shared_ptr & mapping_ptr = pcl::detail::getMapping(m); - if (!mapping_ptr) { - // This normally should get allocated by DefaultMessageCreator, but just in case - mapping_ptr = boost::make_shared(); - } - pcl::MsgFieldMap & mapping = *mapping_ptr; - if (mapping.empty()) { - pcl::createMapping(fields, mapping); - } - - uint8_t is_bigendian; - stream.next(is_bigendian); // ignoring... - uint32_t point_step, row_step; - stream.next(point_step); - stream.next(row_step); - - // Copy point data - uint32_t data_size; - stream.next(data_size); - assert(data_size == m.height * m.width * point_step); - m.points.resize(m.height * m.width); - uint8_t * m_data = reinterpret_cast(m.points.data()); - // If the data layouts match, can copy a whole row in one memcpy - if (mapping.size() == 1 && - mapping[0].serialized_offset == 0 && - mapping[0].struct_offset == 0 && - point_step == sizeof(T)) - { - uint32_t m_row_step = sizeof(T) * m.width; - // And if the row steps match, can copy whole point cloud in one memcpy - if (m_row_step == row_step) { - memcpy(m_data, stream.advance(data_size), data_size); - } else { - for (uint32_t i = 0; i < m.height; ++i, m_data += m_row_step) { - memcpy(m_data, stream.advance(row_step), m_row_step); - } - } - } else { - // If not, do a lot of memcpys to copy over the fields - for (uint32_t row = 0; row < m.height; ++row) { - const uint8_t * stream_data = stream.advance(row_step); - for (uint32_t col = 0; col < m.width; ++col, stream_data += point_step) { - BOOST_FOREACH(const pcl::detail::FieldMapping & fm, mapping) { - memcpy(m_data + fm.struct_offset, stream_data + fm.serialized_offset, fm.size); - } - m_data += sizeof(T); - } - } - } - - uint8_t is_dense; - stream.next(is_dense); - m.is_dense = is_dense; - } - - inline static uint32_t serializedLength(const pcl::PointCloud & m) - { - uint32_t length = 0; - - length += serializationLength(m.header); - length += 8; // height/width - - pcl::detail::FieldsLength fl; - typedef typename pcl::traits::fieldList::type FieldList; - pcl::for_each_type(boost::ref(fl)); - length += 4; // size of 'fields' - length += fl.length; - - length += 1; // is_bigendian - length += 4; // point_step - length += 4; // row_step - length += 4; // size of 'data' - length += m.points.size() * sizeof(T); // data - length += 1; // is_dense - - return length; - } -}; -} // namespace serialization - -/// @todo Printer specialization in message_operations - -} // namespace ros - -namespace pcl -{ -namespace detail -{ -#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED #if PCL_VERSION_COMPARE(>=, 1, 10, 0) template constexpr static bool pcl_uses_boost = std::is_same, @@ -381,14 +66,11 @@ struct Holder { SharedPointer p; - explicit Holder(const SharedPointer & p) - : p(p) {} - Holder(const Holder & other) - : p(other.p) {} - Holder(Holder && other) - : p(std::move(other.p)) {} + explicit Holder(const SharedPointer & p) : p(p) {} + Holder(const Holder & other) : p(other.p) {} + Holder(Holder && other) : p(std::move(other.p)) {} - void operator()(...) {p.reset();} + void operator()(...) { p.reset(); } }; template @@ -412,24 +94,23 @@ inline boost::shared_ptr to_boost_ptr(const std::shared_ptr & p) return boost::shared_ptr(p.get(), Holder>(p)); } } -#endif + } // namespace detail -// add functions to convert to smart pointer used by ROS +// Pointer compatibility helpers + template inline boost::shared_ptr ros_ptr(const boost::shared_ptr & p) { return p; } -#if ROS_POINTER_COMPATIBILITY_IMPLEMENTED template inline boost::shared_ptr ros_ptr(const std::shared_ptr & p) { return detail::to_boost_ptr(p); } -// add functions to convert to smart pointer used by PCL, based on PCL's own pointer template>::type> inline std::shared_ptr pcl_ptr(const std::shared_ptr & p) { @@ -453,13 +134,44 @@ inline boost::shared_ptr pcl_ptr(const boost::shared_ptr & p) { return p; } -#else -template -inline boost::shared_ptr pcl_ptr(const boost::shared_ptr & p) -{ - return p; -} -#endif + } // namespace pcl -#endif // PCL_ROS__POINT_CLOUD_HPP__ +// ROS 2 TypeAdapter between pcl::PointCloud and sensor_msgs::msg::PointCloud2 + +template +struct rclcpp::TypeAdapter, sensor_msgs::msg::PointCloud2> +{ + using is_specialized = std::true_type; + using custom_type = pcl::PointCloud; + using ros_message_type = sensor_msgs::msg::PointCloud2; + + static void convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + pcl::toROSMsg(source, destination); + } + + static void convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + pcl::fromROSMsg(source, destination); + } +}; + +// Register TypeAdapter specializations +RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE( + pcl::PointCloud, + sensor_msgs::msg::PointCloud2); + +RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE( + pcl::PointCloud, + sensor_msgs::msg::PointCloud2); + +RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE( + pcl::PointCloud, + sensor_msgs::msg::PointCloud2); + +#endif // PCL_ROS__POINT_CLOUD_HPP__ \ No newline at end of file diff --git a/pcl_ros/include/pcl_ros/publisher.hpp b/pcl_ros/include/pcl_ros/publisher.hpp index 9f92745f9..be7c959ea 100644 --- a/pcl_ros/include/pcl_ros/publisher.hpp +++ b/pcl_ros/include/pcl_ros/publisher.hpp @@ -45,13 +45,14 @@ #ifndef PCL_ROS__PUBLISHER_HPP_ #define PCL_ROS__PUBLISHER_HPP_ -#include -#include +#include +#include #include #include #include +#include namespace pcl_ros { @@ -59,36 +60,42 @@ class BasePublisher { public: void - advertise(ros::NodeHandle & nh, const std::string & topic, uint32_t queue_size) + advertise(rclcpp::Node::SharedPtr node, const std::string & topic, const rclcpp::QoS & qos) { - pub_ = nh.advertise(topic, queue_size); + pub_ = node->create_publisher(topic, qos); + } + + void + advertise(rclcpp::Node::SharedPtr node, const std::string & topic, size_t queue_size) + { + advertise(node, topic, rclcpp::QoS(queue_size)); } std::string - getTopic() + getTopic() const { - return pub_.getTopic(); + return pub_->get_topic_name(); } - uint32_t + size_t getNumSubscribers() const { - return pub_.getNumSubscribers(); + return pub_->get_subscription_count(); } void shutdown() { - pub_.shutdown(); + pub_.reset(); } - operator void *() const + operator bool() const { - return (pub_) ? reinterpret_cast(1) : reinterpret_cast(0); + return pub_ != nullptr; } protected: - ros::Publisher pub_; + rclcpp::Publisher::SharedPtr pub_; }; template @@ -97,15 +104,20 @@ class Publisher : public BasePublisher public: Publisher() {} - Publisher(ros::NodeHandle & nh, const std::string & topic, uint32_t queue_size) + Publisher(rclcpp::Node::SharedPtr node, const std::string & topic, const rclcpp::QoS & qos) + { + advertise(node, topic, qos); + } + + Publisher(rclcpp::Node::SharedPtr node, const std::string & topic, size_t queue_size) { - advertise(nh, topic, queue_size); + advertise(node, topic, queue_size); } ~Publisher() {} inline void - publish(const boost::shared_ptr> & point_cloud) const + publish(const std::shared_ptr> & point_cloud) const { publish(*point_cloud); } @@ -114,38 +126,41 @@ class Publisher : public BasePublisher publish(const pcl::PointCloud & point_cloud) const { // Fill point cloud binary data - sensor_msgs::PointCloud2::Ptr msg_ptr(new sensor_msgs::PointCloud2); + auto msg_ptr = std::make_shared(); pcl::toROSMsg(point_cloud, *msg_ptr); - pub_.publish(msg_ptr); + pub_->publish(*msg_ptr); } }; template<> -class Publisher: public BasePublisher +class Publisher: public BasePublisher { public: Publisher() {} - Publisher(ros::NodeHandle & nh, const std::string & topic, uint32_t queue_size) + Publisher(rclcpp::Node::SharedPtr node, const std::string & topic, const rclcpp::QoS & qos) + { + advertise(node, topic, qos); + } + + Publisher(rclcpp::Node::SharedPtr node, const std::string & topic, size_t queue_size) { - advertise(nh, topic, queue_size); + advertise(node, topic, queue_size); } ~Publisher() {} void - publish(const sensor_msgs::PointCloud2Ptr & point_cloud) const + publish(const std::shared_ptr & point_cloud) const { - pub_.publish(point_cloud); - // pub_.publish (*point_cloud); + pub_->publish(*point_cloud); } void - publish(const sensor_msgs::PointCloud2 & point_cloud) const + publish(const sensor_msgs::msg::PointCloud2 & point_cloud) const { - pub_.publish(boost::make_shared(point_cloud)); - // pub_.publish (point_cloud); + pub_->publish(point_cloud); } }; } // namespace pcl_ros -#endif // PCL_ROS__PUBLISHER_HPP_ +#endif // PCL_ROS__PUBLISHER_HPP_ \ No newline at end of file diff --git a/pcl_ros/include/pcl_ros/segmentation/extract_clusters.hpp b/pcl_ros/include/pcl_ros/segmentation/extract_clusters.hpp index 581bab575..34a705641 100644 --- a/pcl_ros/include/pcl_ros/segmentation/extract_clusters.hpp +++ b/pcl_ros/include/pcl_ros/segmentation/extract_clusters.hpp @@ -38,11 +38,10 @@ #ifndef PCL_ROS__SEGMENTATION__EXTRACT_CLUSTERS_HPP_ #define PCL_ROS__SEGMENTATION__EXTRACT_CLUSTERS_HPP_ -#include +#include #include #include #include "pcl_ros/pcl_nodelet.hpp" -#include "pcl_ros/EuclideanClusterExtractionConfig.hpp" namespace pcl_ros { @@ -58,7 +57,7 @@ class EuclideanClusterExtraction : public PCLNodelet public: /** \brief Empty constructor. */ EuclideanClusterExtraction() - : publish_indices_(false), max_clusters_(std::numeric_limits::max()) {} + : PCLNodelet("euclidean_cluster_extraction"), publish_indices_(false), max_clusters_(std::numeric_limits::max()) {} protected: // ROS nodelet attributes @@ -68,8 +67,8 @@ class EuclideanClusterExtraction : public PCLNodelet /** \brief Maximum number of clusters to publish. */ int max_clusters_; - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr> srv_; + /** \brief Parameter callback handle. */ + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; /** \brief Nodelet initialization routine. */ void onInit(); @@ -78,11 +77,11 @@ class EuclideanClusterExtraction : public PCLNodelet void subscribe(); void unsubscribe(); - /** \brief Dynamic reconfigure callback - * \param config the config object - * \param level the dynamic reconfigure level + /** \brief Parameter callback + * \param parameters the changed parameters */ - void config_callback(EuclideanClusterExtractionConfig & config, uint32_t level); + rcl_interfaces::msg::SetParametersResult config_callback( + const std::vector & parameters); /** \brief Input point cloud callback. * \param cloud the pointer to the input point cloud @@ -97,12 +96,12 @@ class EuclideanClusterExtraction : public PCLNodelet pcl::EuclideanClusterExtraction impl_; /** \brief The input PointCloud subscriber. */ - ros::Subscriber sub_input_; + rclcpp::Subscription::SharedPtr sub_input_; /** \brief Synchronized input, and indices.*/ - boost::shared_ptr>> sync_input_indices_e_; - boost::shared_ptr>> sync_input_indices_a_; public: @@ -110,4 +109,4 @@ class EuclideanClusterExtraction : public PCLNodelet }; } // namespace pcl_ros -#endif // PCL_ROS__SEGMENTATION__EXTRACT_CLUSTERS_HPP_ +#endif // PCL_ROS__SEGMENTATION__EXTRACT_CLUSTERS_HPP_ \ No newline at end of file diff --git a/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.hpp b/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.hpp index b60f7e40f..1abf2a13f 100644 --- a/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.hpp +++ b/pcl_ros/include/pcl_ros/segmentation/extract_polygonal_prism_data.hpp @@ -42,8 +42,7 @@ #include #include #include -#include -#include "pcl_ros/ExtractPolygonalPrismDataConfig.hpp" +#include #include "pcl_ros/pcl_nodelet.hpp" namespace pcl_ros @@ -60,41 +59,28 @@ namespace sync_policies = message_filters::sync_policies; class ExtractPolygonalPrismData : public PCLNodelet { typedef pcl::PointCloud PointCloud; - typedef boost::shared_ptr PointCloudPtr; - typedef boost::shared_ptr PointCloudConstPtr; + typedef std::shared_ptr PointCloudPtr; + typedef std::shared_ptr PointCloudConstPtr; protected: /** \brief The output PointIndices publisher. */ - ros::Publisher pub_output_; + rclcpp::Publisher::SharedPtr pub_output_; /** \brief The message filter subscriber for PointCloud2. */ message_filters::Subscriber sub_hull_filter_; /** \brief Synchronized input, planar hull, and indices.*/ - boost::shared_ptr>> sync_input_hull_indices_e_; - boost::shared_ptr>> sync_input_hull_indices_a_; - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr> srv_; + /** \brief Parameter callback handle. */ + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; - /** \brief Null passthrough filter, used for pushing empty elements in the - * synchronizer */ + /** \brief Pass-through filter for when indices are not used. */ message_filters::PassThrough nf_; - /** \brief Input point cloud callback. - * Because we want to use the same synchronizer object, we push back - * empty elements with the same timestamp. - */ - inline void - input_callback(const PointCloudConstPtr & input) - { - PointIndices cloud; - cloud.header.stamp = pcl_conversions::fromPCL(input->header).stamp; - nf_.add(boost::make_shared(cloud)); - } - /** \brief Nodelet initialization routine. */ void onInit(); @@ -102,13 +88,13 @@ class ExtractPolygonalPrismData : public PCLNodelet void subscribe(); void unsubscribe(); - /** \brief Dynamic reconfigure callback - * \param config the config object - * \param level the dynamic reconfigure level + /** \brief Parameter callback + * \param parameters the changed parameters */ - void config_callback(ExtractPolygonalPrismDataConfig & config, uint32_t level); + rcl_interfaces::msg::SetParametersResult config_callback( + const std::vector & parameters); - /** \brief Input point cloud callback. Used when \a use_indices is set. + /** \brief Input + planar hull + indices callback. * \param cloud the pointer to the input point cloud * \param hull the pointer to the planar hull point cloud * \param indices the pointer to the input point cloud indices @@ -127,4 +113,4 @@ class ExtractPolygonalPrismData : public PCLNodelet }; } // namespace pcl_ros -#endif // PCL_ROS__SEGMENTATION__EXTRACT_POLYGONAL_PRISM_DATA_HPP_ +#endif // PCL_ROS__SEGMENTATION__EXTRACT_POLYGONAL_PRISM_DATA_HPP_ \ No newline at end of file diff --git a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp index 4699eee69..9b5758ecf 100644 --- a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp +++ b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp @@ -38,264 +38,173 @@ #ifndef PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_ #define PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_ -#include #include -#include -#include +#include +#include +#include +#include +#include #include "pcl_ros/pcl_nodelet.hpp" -#include "pcl_ros/SACSegmentationConfig.hpp" -#include "pcl_ros/SACSegmentationFromNormalsConfig.hpp" namespace pcl_ros { -namespace sync_policies = message_filters::sync_policies; - -//////////////////////////////////////////////////////////////////////////////////////////// -/** \brief @b SACSegmentation represents the Nodelet segmentation class for Sample Consensus - * methods and models, in the sense that it just creates a Nodelet wrapper for generic-purpose - * SAC-based segmentation. - * \author Radu Bogdan Rusu - */ -class SACSegmentation : public PCLNodelet -{ - typedef pcl::PointCloud PointCloud; - typedef boost::shared_ptr PointCloudPtr; - typedef boost::shared_ptr PointCloudConstPtr; - -public: - /** \brief Constructor. */ - SACSegmentation() - : min_inliers_(0) {} - - /** \brief Set the input TF frame the data should be transformed into before processing, - * if input.header.frame_id is different. - * \param tf_frame the TF frame the input PointCloud should be transformed into before processing + namespace sync_policies = message_filters::sync_policies; + + /** \brief @b SACSegmentation represents the PCL nodelet segmentation class for Sample Consensus methods and models + * \author Radu Bogdan Rusu */ - inline void setInputTFframe(std::string tf_frame) {tf_input_frame_ = tf_frame;} + class SACSegmentation : public PCLNodelet + { + typedef pcl::PointCloud PointCloud; + typedef std::shared_ptr PointCloudPtr; + typedef std::shared_ptr PointCloudConstPtr; - /** \brief Get the TF frame the input PointCloud should be transformed into before processing. */ - inline std::string getInputTFframe() {return tf_input_frame_;} + typedef pcl::PointCloud PointCloudRGBA; - /** \brief Set the output TF frame the data should be transformed into after processing. - * \param tf_frame the TF frame the PointCloud should be transformed into after processing - */ - inline void setOutputTFframe(std::string tf_frame) {tf_output_frame_ = tf_frame;} + public: + /** \brief Constructor. */ + SACSegmentation() : PCLNodelet("sac_segmentation"), min_inliers_(0) {} - /** \brief Get the TF frame the PointCloud should be transformed into after processing. */ - inline std::string getOutputTFframe() {return tf_output_frame_;} + protected: + /** \brief The PCL implementation used. */ + pcl::SACSegmentation impl_; -protected: - // The minimum number of inliers a model must have in order to be considered valid. - int min_inliers_; + /** \brief The output PointIndices publisher. */ + rclcpp::Publisher::SharedPtr pub_indices_; - // ROS nodelet attributes - /** \brief The output PointIndices publisher. */ - ros::Publisher pub_indices_; + /** \brief The output ModelCoefficients publisher. */ + rclcpp::Publisher::SharedPtr pub_model_; - /** \brief The output ModelCoefficients publisher. */ - ros::Publisher pub_model_; + /** \brief The input PointCloud subscriber. */ + rclcpp::Subscription::SharedPtr sub_input_; - /** \brief The input PointCloud subscriber. */ - ros::Subscriber sub_input_; + /** \brief Synchronized input, and indices.*/ + std::shared_ptr>> sync_input_indices_e_; + std::shared_ptr>> sync_input_indices_a_; - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr> srv_; + /** \brief Parameter callback handle. */ + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; - /** \brief The input TF frame the data should be transformed into, - * if input.header.frame_id is different. - */ - std::string tf_input_frame_; + /** \brief Minimum number of inliers required. */ + int min_inliers_; - /** \brief The original data input TF frame. */ - std::string tf_input_orig_frame_; + /** \brief Input TF frame the data should be transformed into, if input.header.frame_id is different. */ + std::string tf_input_frame_; - /** \brief The output TF frame the data should be transformed into, - * if input.header.frame_id is different. - */ - std::string tf_output_frame_; + /** \brief The original data input TF frame. */ + std::string tf_input_orig_frame_; - /** \brief Null passthrough filter, used for pushing empty elements in the - * synchronizer */ - message_filters::PassThrough nf_pi_; + /** \brief Output TF frame the data should be transformed into, if input.header.frame_id is different. */ + std::string tf_output_frame_; - /** \brief Nodelet initialization routine. */ - virtual void onInit(); + /** \brief Mutex. */ + std::mutex mutex_; - /** \brief LazyNodelet connection routine. */ - virtual void subscribe(); - virtual void unsubscribe(); + /** \brief Nodelet initialization routine. */ + void onInit(); - /** \brief Dynamic reconfigure callback - * \param config the config object - * \param level the dynamic reconfigure level - */ - void config_callback(SACSegmentationConfig & config, uint32_t level); + /** \brief LazyNodelet connection routine. */ + void subscribe(); + void unsubscribe(); - /** \brief Input point cloud callback. Used when \a use_indices is set. - * \param cloud the pointer to the input point cloud - * \param indices the pointer to the input point cloud indices - */ - void input_indices_callback( - const PointCloudConstPtr & cloud, - const PointIndicesConstPtr & indices); + /** \brief Parameter callback + * \param parameters the changed parameters + */ + rcl_interfaces::msg::SetParametersResult config_callback( + const std::vector & parameters); - /** \brief Pointer to a set of indices stored internally. - * (used when \a latched_indices_ is set). - */ - PointIndices indices_; + /** \brief PointCloud + PointIndices callback. + * \param cloud the pointer to the input point cloud + * \param indices the pointer to the input point cloud indices + */ + void input_indices_callback( + const PointCloudConstPtr & cloud, + const PointIndicesConstPtr & indices); - /** \brief Indices callback. Used when \a latched_indices_ is set. - * \param indices the pointer to the input point cloud indices - */ - inline void - indices_callback(const PointIndicesConstPtr & indices) - { - indices_ = *indices; - } + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; - /** \brief Input callback. Used when \a latched_indices_ is set. - * \param input the pointer to the input point cloud + /** \brief @b SACSegmentationFromNormals represents the PCL nodelet segmentation class for Sample Consensus methods and models that require the use of surface normals for estimation. + * \author Radu Bogdan Rusu */ - inline void - input_callback(const PointCloudConstPtr & input) + class SACSegmentationFromNormals : public PCLNodelet { - indices_.header = fromPCL(input->header); - PointIndicesConstPtr indices; - indices.reset(new PointIndices(indices_)); - nf_pi_.add(indices); - } - -private: - /** \brief Internal mutex. */ - boost::mutex mutex_; - - /** \brief The PCL implementation used. */ - pcl::SACSegmentation impl_; - - /** \brief Synchronized input, and indices.*/ - boost::shared_ptr>> sync_input_indices_e_; - boost::shared_ptr>> sync_input_indices_a_; - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -//////////////////////////////////////////////////////////////////////////////////////////// -/** \brief @b SACSegmentationFromNormals represents the PCL nodelet segmentation class for - * Sample Consensus methods and models that require the use of surface normals for estimation. - */ -class SACSegmentationFromNormals : public SACSegmentation -{ - typedef pcl::PointCloud PointCloud; - typedef boost::shared_ptr PointCloudPtr; - typedef boost::shared_ptr PointCloudConstPtr; - - typedef pcl::PointCloud PointCloudN; - typedef boost::shared_ptr PointCloudNPtr; - typedef boost::shared_ptr PointCloudNConstPtr; - -public: - /** \brief Set the input TF frame the data should be transformed into before processing, - * if input.header.frame_id is different. - * \param tf_frame the TF frame the input PointCloud should be transformed into before processing - */ - inline void setInputTFframe(std::string tf_frame) {tf_input_frame_ = tf_frame;} + typedef pcl::PointCloud PointCloud; + typedef std::shared_ptr PointCloudPtr; + typedef std::shared_ptr PointCloudConstPtr; - /** \brief Get the TF frame the input PointCloud should be transformed into before processing. */ - inline std::string getInputTFframe() {return tf_input_frame_;} + typedef pcl::PointCloud PointCloudN; + typedef std::shared_ptr PointCloudNPtr; + typedef std::shared_ptr PointCloudNConstPtr; - /** \brief Set the output TF frame the data should be transformed into after processing. - * \param tf_frame the TF frame the PointCloud should be transformed into after processing - */ - inline void setOutputTFframe(std::string tf_frame) {tf_output_frame_ = tf_frame;} + public: + /** \brief Constructor. */ + SACSegmentationFromNormals() : PCLNodelet("sac_segmentation_from_normals"), min_inliers_(0) {} - /** \brief Get the TF frame the PointCloud should be transformed into after processing. */ - inline std::string getOutputTFframe() {return tf_output_frame_;} + protected: + /** \brief The PCL implementation used. */ + pcl::SACSegmentationFromNormals impl_; -protected: - // ROS nodelet attributes - /** \brief The normals PointCloud subscriber filter. */ - message_filters::Subscriber sub_normals_filter_; + /** \brief The output PointIndices publisher. */ + rclcpp::Publisher::SharedPtr pub_indices_; - /** \brief The input PointCloud subscriber. */ - ros::Subscriber sub_axis_; + /** \brief The output ModelCoefficients publisher. */ + rclcpp::Publisher::SharedPtr pub_model_; - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr> srv_; + /** \brief The normals PointCloud subscriber filter. */ + message_filters::Subscriber sub_normals_filter_; - /** \brief Input point cloud callback. - * Because we want to use the same synchronizer object, we push back - * empty elements with the same timestamp. - */ - inline void - input_callback(const PointCloudConstPtr & cloud) - { - PointIndices indices; - indices.header.stamp = fromPCL(cloud->header).stamp; - nf_.add(boost::make_shared(indices)); - } - - /** \brief Null passthrough filter, used for pushing empty elements in the - * synchronizer */ - message_filters::PassThrough nf_; - - /** \brief The input TF frame the data should be transformed into, - * if input.header.frame_id is different. - */ - std::string tf_input_frame_; - /** \brief The original data input TF frame. */ - std::string tf_input_orig_frame_; - /** \brief The output TF frame the data should be transformed into, - * if input.header.frame_id is different. - */ - std::string tf_output_frame_; + /** \brief The input PointCloud subscriber filter. */ + message_filters::Subscriber sub_input_filter_; - /** \brief Nodelet initialization routine. */ - virtual void onInit(); + /** \brief The axis subscriber. */ + rclcpp::Subscription::SharedPtr sub_axis_; - /** \brief LazyNodelet connection routine. */ - virtual void subscribe(); - virtual void unsubscribe(); + /** \brief Synchronized input, normals, and indices.*/ + std::shared_ptr>> sync_input_normals_indices_e_; + std::shared_ptr>> sync_input_normals_indices_a_; - /** \brief Model callback - * \param model the sample consensus model found - */ - void axis_callback(const pcl_msgs::ModelCoefficientsConstPtr & model); + /** \brief Parameter callback handle. */ + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; - /** \brief Dynamic reconfigure callback - * \param config the config object - * \param level the dynamic reconfigure level - */ - void config_callback(SACSegmentationFromNormalsConfig & config, uint32_t level); + /** \brief Minimum number of inliers required. */ + int min_inliers_; - /** \brief Input point cloud callback. - * \param cloud the pointer to the input point cloud - * \param cloud_normals the pointer to the input point cloud normals - * \param indices the pointer to the input point cloud indices - */ - void input_normals_indices_callback( - const PointCloudConstPtr & cloud, - const PointCloudNConstPtr & cloud_normals, - const PointIndicesConstPtr & indices); - -private: - /** \brief Internal mutex. */ - boost::mutex mutex_; - - /** \brief The PCL implementation used. */ - pcl::SACSegmentationFromNormals impl_; - - /** \brief Synchronized input, normals, and indices.*/ - boost::shared_ptr>> sync_input_normals_indices_a_; - boost::shared_ptr>> sync_input_normals_indices_e_; - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; -} // namespace pcl_ros - -#endif // PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_ + /** \brief Mutex. */ + std::mutex mutex_; + + /** \brief Nodelet initialization routine. */ + void onInit(); + + /** \brief LazyNodelet connection routine. */ + void subscribe(); + void unsubscribe(); + + /** \brief Parameter callback + * \param parameters the changed parameters + */ + rcl_interfaces::msg::SetParametersResult config_callback( + const std::vector & parameters); + + /** \brief ModelCoefficients callback (used for setting an axis). + * \param model a pointer to the model coefficients + */ + void axis_callback(const ModelCoefficientsConstPtr & model); + + /** \brief PointCloud + Normals + PointIndices callback. + * \param cloud the pointer to the input point cloud + * \param cloud_normals the pointer to the input normals + * \param indices the pointer to the input point cloud indices + */ + void input_normals_indices_callback( + const PointCloudConstPtr & cloud, + const PointCloudNConstPtr & cloud_normals, + const PointIndicesConstPtr & indices); + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} + +#endif // PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_ \ No newline at end of file diff --git a/pcl_ros/include/pcl_ros/segmentation/segment_differences.hpp b/pcl_ros/include/pcl_ros/segmentation/segment_differences.hpp index 9a4a9322c..033260a04 100644 --- a/pcl_ros/include/pcl_ros/segmentation/segment_differences.hpp +++ b/pcl_ros/include/pcl_ros/segmentation/segment_differences.hpp @@ -39,10 +39,14 @@ #define PCL_ROS__SEGMENTATION__SEGMENT_DIFFERENCES_HPP_ #include -#include -#include "pcl_ros/SegmentDifferencesConfig.hpp" -#include "pcl_ros/pcl_nodelet.hpp" +#include +#include +#include +#include +#include +#include +#include "pcl_ros/pcl_nodelet.hpp" namespace pcl_ros { @@ -58,26 +62,25 @@ namespace sync_policies = message_filters::sync_policies; class SegmentDifferences : public PCLNodelet { typedef pcl::PointCloud PointCloud; - typedef boost::shared_ptr PointCloudPtr; - typedef boost::shared_ptr PointCloudConstPtr; + typedef PointCloud::Ptr PointCloudPtr; + typedef PointCloud::ConstPtr PointCloudConstPtr; public: /** \brief Empty constructor. */ - SegmentDifferences() {} + SegmentDifferences() + : PCLNodelet("segment_differences") + {} protected: /** \brief The message filter subscriber for PointCloud2. */ message_filters::Subscriber sub_target_filter_; /** \brief Synchronized input, and planar hull.*/ - boost::shared_ptr>> sync_input_target_e_; - boost::shared_ptr>> sync_input_target_a_; - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr> srv_; - /** \brief Nodelet initialization routine. */ void onInit(); @@ -85,11 +88,10 @@ class SegmentDifferences : public PCLNodelet void subscribe(); void unsubscribe(); - /** \brief Dynamic reconfigure callback - * \param config the config object - * \param level the dynamic reconfigure level + /** \brief Parameter callback + * \param distance_threshold the distance threshold parameter */ - void config_callback(SegmentDifferencesConfig & config, uint32_t level); + void config_callback(); /** \brief Input point cloud callback. * \param cloud the pointer to the input point cloud @@ -103,9 +105,15 @@ class SegmentDifferences : public PCLNodelet /** \brief The PCL implementation used. */ pcl::SegmentDifferences impl_; + /** \brief Distance threshold parameter */ + double distance_threshold_; + + /** \brief Parameter callback handle */ + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace pcl_ros -#endif // PCL_ROS__SEGMENTATION__SEGMENT_DIFFERENCES_HPP_ +#endif // PCL_ROS__SEGMENTATION__SEGMENT_DIFFERENCES_HPP_ \ No newline at end of file diff --git a/pcl_ros/src/pcl_ros/filters/extract_indices.cpp b/pcl_ros/src/pcl_ros/filters/extract_indices.cpp index 7ee29be0c..470e325f6 100644 --- a/pcl_ros/src/pcl_ros/filters/extract_indices.cpp +++ b/pcl_ros/src/pcl_ros/filters/extract_indices.cpp @@ -35,37 +35,73 @@ * */ -#include +#include #include "pcl_ros/filters/extract_indices.hpp" ////////////////////////////////////////////////////////////////////////////////////////////// bool -pcl_ros::ExtractIndices::child_init(ros::NodeHandle & nh, bool & has_service) +pcl_ros::ExtractIndices::child_init(bool & has_service) { - has_service = true; - - srv_ = boost::make_shared>(nh); - dynamic_reconfigure::Server::CallbackType f = boost::bind( - &ExtractIndices::config_callback, this, _1, _2); - srv_->setCallback(f); + has_service = false; + // Declare parameter + negative_ = this->declare_parameter("negative", false); + + // Set the initial value + impl_.setNegative(negative_); + use_indices_ = true; return true; } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::ExtractIndices::config_callback(pcl_ros::ExtractIndicesConfig & config, uint32_t level) +pcl_ros::ExtractIndices::filter( + const PointCloud2ConstPtr & 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); + if (indices) { + impl_.setIndices(indices); + } + + pcl::PCLPointCloud2 pcl_output; + impl_.filter(pcl_output); + + pcl_conversions::moveFromPCL(pcl_output, output); +} - 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")); +////////////////////////////////////////////////////////////////////////////////////////////// +rcl_interfaces::msg::SetParametersResult +pcl_ros::ExtractIndices::config_callback(const std::vector & parameters) +{ + // Call parent callback first + auto result = Filter::config_callback(parameters); + if (!result.successful) { + return result; } + + std::lock_guard lock(mutex_); + + for (const auto & parameter : parameters) { + if (parameter.get_name() == "negative") { + negative_ = parameter.as_bool(); + impl_.setNegative(negative_); + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting the extraction to: %s.", + this->get_name(), + (negative_ ? "everything but the indices" : "indices")); + } + } + + return result; } -typedef pcl_ros::ExtractIndices ExtractIndices; -PLUGINLIB_EXPORT_CLASS(ExtractIndices, nodelet::Nodelet); +RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::ExtractIndices) \ No newline at end of file diff --git a/pcl_ros/src/pcl_ros/filters/filter.cpp b/pcl_ros/src/pcl_ros/filters/filter.cpp index 7d011c02c..269a71f50 100644 --- a/pcl_ros/src/pcl_ros/filters/filter.cpp +++ b/pcl_ros/src/pcl_ros/filters/filter.cpp @@ -40,74 +40,58 @@ #include #include "pcl_ros/transforms.hpp" -/*//#include -//#include -*/ - -/*//typedef pcl::PixelGrid PixelGrid; -//typedef pcl::FilterDimension FilterDimension; -*/ - -// Include the implementations instead of compiling them separately to speed up compile time -// #include "extract_indices.cpp" -// #include "passthrough.cpp" -// #include "project_inliers.cpp" -// #include "radius_outlier_removal.cpp" -// #include "statistical_outlier_removal.cpp" -// #include "voxel_grid.cpp" - -/*//PLUGINLIB_EXPORT_CLASS(PixelGrid,nodelet::Nodelet); -//PLUGINLIB_EXPORT_CLASS(FilterDimension,nodelet::Nodelet); -*/ - /////////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::Filter::computePublish(const PointCloud2::ConstPtr & input, const IndicesPtr & indices) +pcl_ros::Filter::computePublish(const PointCloud2ConstPtr & 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::SharedPtr cloud_tf = std::make_shared(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( + RCLCPP_DEBUG( + this->get_logger(), "[%s::computePublish] Transforming output dataset from %s to %s.", - getName().c_str(), output.header.frame_id.c_str(), tf_output_frame_.c_str()); + this->get_name(), 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( + if (!pcl_ros::transformPointCloud(tf_output_frame_, output, cloud_transformed, *tf_buffer_)) { + RCLCPP_ERROR( + this->get_logger(), "[%s::computePublish] Error converting output dataset from %s to %s.", - getName().c_str(), output.header.frame_id.c_str(), tf_output_frame_.c_str()); + this->get_name(), output.header.frame_id.c_str(), tf_output_frame_.c_str()); return; } - cloud_tf.reset(new PointCloud2(cloud_transformed)); + cloud_tf = std::make_shared(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( + RCLCPP_DEBUG( + this->get_logger(), "[%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()); + this->get_name(), 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( + RCLCPP_ERROR( + this->get_logger(), "[%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()); + this->get_name(), output.header.frame_id.c_str(), tf_input_orig_frame_.c_str()); return; } - cloud_tf.reset(new PointCloud2(cloud_transformed)); + cloud_tf = std::make_shared(cloud_transformed); } // Copy timestamp to keep it cloud_tf->header.stamp = input->header.stamp; - // Publish a boost shared ptr - pub_output_.publish(cloud_tf); + // Publish a shared ptr + pub_output_->publish(*cloud_tf); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -117,28 +101,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(*pnh_, "input", max_queue_size_); - sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); + sub_input_filter_.subscribe(*this, "input", rmw_qos_profile_sensor_data); + sub_indices_filter_.subscribe(*this, "indices", rmw_qos_profile_default); 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 { // 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())); + std::bind(&Filter::input_indices_callback, this, std::placeholders::_1, PointIndicesConstPtr())); } } @@ -150,7 +136,7 @@ pcl_ros::Filter::unsubscribe() sub_input_filter_.unsubscribe(); sub_indices_filter_.unsubscribe(); } else { - sub_input_.shutdown(); + sub_input_.reset(); } } @@ -161,103 +147,123 @@ pcl_ros::Filter::onInit() // Call the super onInit () PCLNodelet::onInit(); + // Initialize TF + tf_buffer_ = std::make_shared(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + // 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()); + if (!child_init(has_service)) { + RCLCPP_ERROR(this->get_logger(), "[%s::onInit] Initialization failed.", this->get_name()); return; } - pub_output_ = advertise(*pnh_, "output", max_queue_size_); + pub_output_ = advertise(*this, "output", max_queue_size_); + + // Declare parameters + this->declare_parameter("input_frame", ""); + this->declare_parameter("output_frame", ""); + + tf_input_frame_ = this->get_parameter("input_frame").as_string(); + tf_output_frame_ = this->get_parameter("output_frame").as_string(); - // Enable the dynamic reconfigure service + // Setup parameter callback if child doesn't have 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); + param_callback_handle_ = this->add_on_set_parameters_callback( + std::bind(&Filter::config_callback, this, std::placeholders::_1)); } - NODELET_DEBUG("[%s::onInit] Nodelet successfully created.", getName().c_str()); + RCLCPP_DEBUG(this->get_logger(), "[%s::onInit] Nodelet successfully created.", this->get_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 & parameters) { - // 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()); + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + for (const auto & parameter : parameters) { + // The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are + // inexistent in PCL + if (parameter.get_name() == "input_frame") { + tf_input_frame_ = parameter.as_string(); + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting the input TF frame to: %s.", + this->get_name(), tf_input_frame_.c_str()); + } else if (parameter.get_name() == "output_frame") { + tf_output_frame_ = parameter.as_string(); + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting the output TF frame to: %s.", + this->get_name(), tf_output_frame_.c_str()); + } } + + return result; } ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::Filter::input_indices_callback( - const PointCloud2::ConstPtr & cloud, + const PointCloud2ConstPtr & cloud, const PointIndicesConstPtr & 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(), "[%s::input_indices_callback] Invalid input!", this->get_name()); 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(), "[%s::input_indices_callback] Invalid indices!", this->get_name()); return; } /// DEBUG if (indices) { - NODELET_DEBUG( + RCLCPP_DEBUG( + this->get_logger(), "[%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(), + this->get_name(), 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()); + rclcpp::Time(cloud->header.stamp).seconds(), cloud->header.frame_id.c_str(), "input", + indices->indices.size(), rclcpp::Time(indices->header.stamp).seconds(), + indices->header.frame_id.c_str(), "indices"); } else { - NODELET_DEBUG( + RCLCPP_DEBUG( + this->get_logger(), "[%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()); + this->get_name(), 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; + PointCloud2ConstPtr cloud_tf; if (!tf_input_frame_.empty() && cloud->header.frame_id != tf_input_frame_) { - NODELET_DEBUG( + RCLCPP_DEBUG( + this->get_logger(), "[%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()); + this->get_name(), 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( + if (!pcl_ros::transformPointCloud(tf_input_frame_, *cloud, cloud_transformed, *tf_buffer_)) { + RCLCPP_ERROR( + this->get_logger(), "[%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()); + this->get_name(), 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; } @@ -269,4 +275,4 @@ pcl_ros::Filter::input_indices_callback( } computePublish(cloud_tf, vindices); -} +} \ No newline at end of file diff --git a/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp b/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp index 39b797a0a..19e32ddd6 100644 --- a/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp +++ b/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp @@ -35,19 +35,29 @@ * */ -#include +#include #include "pcl_ros/filters/voxel_grid.hpp" ////////////////////////////////////////////////////////////////////////////////////////////// bool -pcl_ros::VoxelGrid::child_init(ros::NodeHandle & nh, bool & has_service) +pcl_ros::VoxelGrid::child_init(bool & has_service) { - // 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); + has_service = false; + + // Declare parameters with default values + leaf_size_ = this->declare_parameter("leaf_size", 0.01); + filter_limit_min_ = this->declare_parameter("filter_limit_min", -std::numeric_limits::max()); + filter_limit_max_ = this->declare_parameter("filter_limit_max", std::numeric_limits::max()); + filter_limit_negative_ = this->declare_parameter("filter_limit_negative", false); + filter_field_name_ = this->declare_parameter("filter_field_name", std::string("")); + + // Set the initial values + impl_.setLeafSize(leaf_size_, leaf_size_, leaf_size_); + impl_.setFilterLimits(filter_limit_min_, filter_limit_max_); + impl_.setFilterLimitsNegative(filter_limit_negative_); + if (!filter_field_name_.empty()) { + impl_.setFilterFieldName(filter_field_name_); + } return true; } @@ -55,80 +65,80 @@ pcl_ros::VoxelGrid::child_init(ros::NodeHandle & nh, bool & has_service) ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::VoxelGrid::filter( - const PointCloud2::ConstPtr & input, + const PointCloud2ConstPtr & 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)); + pcl_conversions::toPCL(*input, *pcl_input); + impl_.setInputCloud(pcl_input); - impl_.setIndices(indices); + if (indices) { + impl_.setIndices(indices); + } + pcl::PCLPointCloud2 pcl_output; impl_.filter(pcl_output); + pcl_conversions::moveFromPCL(pcl_output, output); } ////////////////////////////////////////////////////////////////////////////////////////////// -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 & parameters) { - 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]); + // Call parent callback first + auto result = Filter::config_callback(parameters); + if (!result.successful) { + return result; } - 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"); - } + std::lock_guard lock(mutex_); - 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()); + for (const auto & parameter : parameters) { + if (parameter.get_name() == "leaf_size") { + leaf_size_ = parameter.as_double(); + impl_.setLeafSize(leaf_size_, leaf_size_, leaf_size_); + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting the downsampling leaf size to: %f.", + this->get_name(), leaf_size_); + } else if (parameter.get_name() == "filter_limit_min") { + filter_limit_min_ = parameter.as_double(); + impl_.setFilterLimits(filter_limit_min_, filter_limit_max_); + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting the minimum filtering value a point will be considered from to: %f.", + this->get_name(), filter_limit_min_); + } else if (parameter.get_name() == "filter_limit_max") { + filter_limit_max_ = parameter.as_double(); + impl_.setFilterLimits(filter_limit_min_, filter_limit_max_); + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting the maximum filtering value a point will be considered from to: %f.", + this->get_name(), filter_limit_max_); + } else if (parameter.get_name() == "filter_limit_negative") { + filter_limit_negative_ = parameter.as_bool(); + impl_.setFilterLimitsNegative(filter_limit_negative_); + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting the filter negative flag to: %s.", + this->get_name(), filter_limit_negative_ ? "true" : "false"); + } else if (parameter.get_name() == "filter_field_name") { + filter_field_name_ = parameter.as_string(); + if (!filter_field_name_.empty()) { + impl_.setFilterFieldName(filter_field_name_); + } + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting the filter field name to: %s.", + this->get_name(), filter_field_name_.c_str()); + } } - // ---[ 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()); - } - 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()); - } - // ]--- + return result; } -typedef pcl_ros::VoxelGrid VoxelGrid; -PLUGINLIB_EXPORT_CLASS(VoxelGrid, nodelet::Nodelet); +RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::VoxelGrid) \ No newline at end of file diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp index ead3a56aa..7fdef2abb 100644 --- a/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp @@ -35,14 +35,13 @@ * */ -#include +#include #include #include #include #include #include "pcl_ros/segmentation/extract_clusters.hpp" - using pcl_conversions::fromPCL; using pcl_conversions::moveFromPCL; using pcl_conversions::toPCL; @@ -55,47 +54,42 @@ pcl_ros::EuclideanClusterExtraction::onInit() PCLNodelet::onInit(); // ---[ Mandatory parameters - double cluster_tolerance; - if (!pnh_->getParam("cluster_tolerance", cluster_tolerance)) { - NODELET_ERROR( - "[%s::onInit] Need a 'cluster_tolerance' parameter to be set before continuing!", - getName().c_str()); - return; - } - int spatial_locator; - if (!pnh_->getParam("spatial_locator", spatial_locator)) { - NODELET_ERROR( - "[%s::onInit] Need a 'spatial_locator' parameter to be set before continuing!", - getName().c_str()); - return; - } + this->declare_parameter("cluster_tolerance", 0.02); + this->declare_parameter("spatial_locator", 0); + this->declare_parameter("publish_indices", false); + this->declare_parameter("cluster_min_size", 1); + this->declare_parameter("cluster_max_size", std::numeric_limits::max()); + this->declare_parameter("max_clusters", std::numeric_limits::max()); - // private_nh.getParam ("use_indices", use_indices_); - pnh_->getParam("publish_indices", publish_indices_); + double cluster_tolerance = this->get_parameter("cluster_tolerance").as_double(); + int spatial_locator = this->get_parameter("spatial_locator").as_int(); + publish_indices_ = this->get_parameter("publish_indices").as_bool(); + max_clusters_ = this->get_parameter("max_clusters").as_int(); if (publish_indices_) { - pub_output_ = advertise(*pnh_, "output", max_queue_size_); + pub_output_ = advertise(*this, "output", max_queue_size_); } else { - pub_output_ = advertise(*pnh_, "output", max_queue_size_); + pub_output_ = advertise(*this, "output", max_queue_size_); } - // Enable the dynamic reconfigure service - srv_ = boost::make_shared>(*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind( - &EuclideanClusterExtraction::config_callback, this, _1, _2); - srv_->setCallback(f); + // Setup parameter callback + param_callback_handle_ = this->add_on_set_parameters_callback( + std::bind(&EuclideanClusterExtraction::config_callback, this, std::placeholders::_1)); - NODELET_DEBUG( + RCLCPP_DEBUG( + this->get_logger(), "[%s::onInit] Nodelet successfully created with the following parameters:\n" " - max_queue_size : %d\n" " - use_indices : %s\n" " - cluster_tolerance : %f\n", - getName().c_str(), + this->get_name(), max_queue_size_, (use_indices_) ? "true" : "false", cluster_tolerance); // Set given parameters here impl_.setClusterTolerance(cluster_tolerance); + impl_.setMinClusterSize(this->get_parameter("cluster_min_size").as_int()); + impl_.setMaxClusterSize(this->get_parameter("cluster_max_size").as_int()); onInitPostProcess(); } @@ -107,35 +101,35 @@ pcl_ros::EuclideanClusterExtraction::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", rmw_qos_profile_sensor_data); + sub_indices_filter_.subscribe(*this, "indices", rmw_qos_profile_default); if (approximate_sync_) { sync_input_indices_a_ = - boost::make_shared>>(max_queue_size_); sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); sync_input_indices_a_->registerCallback( - bind( + std::bind( &EuclideanClusterExtraction:: - input_indices_callback, this, _1, _2)); + input_indices_callback, this, std::placeholders::_1, std::placeholders::_2)); } else { sync_input_indices_e_ = - boost::make_shared>>(max_queue_size_); sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); sync_input_indices_e_->registerCallback( - bind( + std::bind( &EuclideanClusterExtraction:: - input_indices_callback, this, _1, _2)); + input_indices_callback, this, std::placeholders::_1, std::placeholders::_2)); } } else { // Subscribe in an old fashion to input only (no filters) - sub_input_ = - pnh_->subscribe( - "input", max_queue_size_, - bind(&EuclideanClusterExtraction::input_indices_callback, this, _1, PointIndicesConstPtr())); + sub_input_ = this->create_subscription( + "input", rclcpp::SensorDataQoS(), + std::bind(&EuclideanClusterExtraction::input_indices_callback, this, + std::placeholders::_1, PointIndicesConstPtr())); } } @@ -147,40 +141,59 @@ pcl_ros::EuclideanClusterExtraction::unsubscribe() sub_input_filter_.unsubscribe(); sub_indices_filter_.unsubscribe(); } else { - sub_input_.shutdown(); + sub_input_.reset(); } } ////////////////////////////////////////////////////////////////////////////////////////////// -void +rcl_interfaces::msg::SetParametersResult pcl_ros::EuclideanClusterExtraction::config_callback( - EuclideanClusterExtractionConfig & config, - uint32_t level) + const std::vector & parameters) { - if (impl_.getClusterTolerance() != config.cluster_tolerance) { - impl_.setClusterTolerance(config.cluster_tolerance); - NODELET_DEBUG( - "[%s::config_callback] Setting new clustering tolerance to: %f.", - getName().c_str(), config.cluster_tolerance); - } - if (impl_.getMinClusterSize() != config.cluster_min_size) { - impl_.setMinClusterSize(config.cluster_min_size); - NODELET_DEBUG( - "[%s::config_callback] Setting the minimum cluster size to: %d.", - getName().c_str(), config.cluster_min_size); - } - if (impl_.getMaxClusterSize() != config.cluster_max_size) { - impl_.setMaxClusterSize(config.cluster_max_size); - NODELET_DEBUG( - "[%s::config_callback] Setting the maximum cluster size to: %d.", - getName().c_str(), config.cluster_max_size); - } - if (max_clusters_ != config.max_clusters) { - max_clusters_ = config.max_clusters; - NODELET_DEBUG( - "[%s::config_callback] Setting the maximum number of clusters to extract to: %d.", - getName().c_str(), config.max_clusters); + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + for (const auto & param : parameters) { + if (param.get_name() == "cluster_tolerance") { + double cluster_tolerance = param.as_double(); + if (impl_.getClusterTolerance() != cluster_tolerance) { + impl_.setClusterTolerance(cluster_tolerance); + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting new clustering tolerance to: %f.", + this->get_name(), cluster_tolerance); + } + } else if (param.get_name() == "cluster_min_size") { + int cluster_min_size = param.as_int(); + if (impl_.getMinClusterSize() != cluster_min_size) { + impl_.setMinClusterSize(cluster_min_size); + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting the minimum cluster size to: %d.", + this->get_name(), cluster_min_size); + } + } else if (param.get_name() == "cluster_max_size") { + int cluster_max_size = param.as_int(); + if (impl_.getMaxClusterSize() != cluster_max_size) { + impl_.setMaxClusterSize(cluster_max_size); + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting the maximum cluster size to: %d.", + this->get_name(), cluster_max_size); + } + } else if (param.get_name() == "max_clusters") { + int max_clusters = param.as_int(); + if (max_clusters_ != max_clusters) { + max_clusters_ = max_clusters; + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting the maximum number of clusters to extract to: %d.", + this->get_name(), max_clusters); + } + } } + + return result; } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -189,43 +202,45 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback( const PointCloudConstPtr & cloud, const PointIndicesConstPtr & indices) { // No subscribers, no work - if (pub_output_.getNumSubscribers() <= 0) { + if (pub_output_->get_subscription_count() <= 0) { return; } // 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(), "[%s::input_indices_callback] Invalid input!", this->get_name()); 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(), "[%s::input_indices_callback] Invalid indices!", this->get_name()); return; } /// DEBUG if (indices) { - std_msgs::Header cloud_header = fromPCL(cloud->header); - std_msgs::Header indices_header = indices->header; - NODELET_DEBUG( + std_msgs::msg::Header cloud_header = fromPCL(cloud->header); + std_msgs::msg::Header indices_header = indices->header; + RCLCPP_DEBUG( + this->get_logger(), "[%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(), + this->get_name(), 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()); + rclcpp::Time(cloud_header.stamp).seconds(), cloud_header.frame_id.c_str(), "input", + indices->indices.size(), rclcpp::Time(indices_header.stamp).seconds(), + indices_header.frame_id.c_str(), "indices"); } else { - NODELET_DEBUG( + RCLCPP_DEBUG( + this->get_logger(), "[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on " "topic %s received.", - getName().c_str(), cloud->width * cloud->height, fromPCL( - cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( - "input").c_str()); + this->get_name(), cloud->width * cloud->height, + rclcpp::Time(fromPCL(cloud->header).stamp).seconds(), + cloud->header.frame_id.c_str(), "input"); } /// @@ -247,15 +262,17 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback( } // TODO(xxx): HACK!!! We need to change the PointCloud2 message to add for an incremental // sequence ID number. - pcl_msgs::PointIndices ros_pi; + pcl_msgs::msg::PointIndices ros_pi; moveFromPCL(clusters[i], ros_pi); - ros_pi.header.stamp += ros::Duration(i * 0.001); - pub_output_.publish(ros_pi); + auto stamp = rclcpp::Time(ros_pi.header.stamp) + rclcpp::Duration::from_nanoseconds(i * 1000000); + ros_pi.header.stamp = stamp; + pub_output_->publish(ros_pi); } - NODELET_DEBUG( + RCLCPP_DEBUG( + this->get_logger(), "[segmentAndPublish] Published %zu clusters (PointIndices) on topic %s", - clusters.size(), pnh_->resolveName("output").c_str()); + clusters.size(), "output"); } else { for (size_t i = 0; i < clusters.size(); ++i) { if (static_cast(i) >= max_clusters_) { @@ -264,21 +281,21 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback( PointCloud output; copyPointCloud(*cloud, clusters[i].indices, output); - // PointCloud output_blob; // Convert from the templated output to the PointCloud blob - // pcl::toROSMsg (output, output_blob); // TODO(xxx): HACK!!! We need to change the PointCloud2 message to add for an incremental // sequence ID number. - std_msgs::Header header = fromPCL(output.header); - header.stamp += ros::Duration(i * 0.001); + std_msgs::msg::Header header = fromPCL(output.header); + auto stamp = rclcpp::Time(header.stamp) + rclcpp::Duration::from_nanoseconds(i * 1000000); + header.stamp = stamp; toPCL(header, output.header); - // Publish a Boost shared ptr const data - pub_output_.publish(ros_ptr(output.makeShared())); - NODELET_DEBUG( + // Publish a shared ptr const data + pub_output_->publish(output); + RCLCPP_DEBUG( + this->get_logger(), "[segmentAndPublish] Published cluster %zu (with %zu values and stamp %f) on topic %s", - i, clusters[i].indices.size(), header.stamp.toSec(), pnh_->resolveName("output").c_str()); + i, clusters[i].indices.size(), rclcpp::Time(header.stamp).seconds(), "output"); } } } typedef pcl_ros::EuclideanClusterExtraction EuclideanClusterExtraction; -PLUGINLIB_EXPORT_CLASS(EuclideanClusterExtraction, nodelet::Nodelet) +RCLCPP_COMPONENTS_REGISTER_NODE(EuclideanClusterExtraction) \ No newline at end of file diff --git a/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp index 96272d490..d481b61b0 100644 --- a/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp @@ -35,7 +35,7 @@ * */ -#include +#include #include #include #include @@ -52,14 +52,21 @@ pcl_ros::ExtractPolygonalPrismData::onInit() // Call the super onInit () PCLNodelet::onInit(); - // Enable the dynamic reconfigure service - srv_ = boost::make_shared>(*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind( - &ExtractPolygonalPrismData::config_callback, this, _1, _2); - srv_->setCallback(f); + // Declare parameters with default values + this->declare_parameter("height_min", -1.0); + this->declare_parameter("height_max", 1.0); + + // Get initial parameter values + double height_min = this->get_parameter("height_min").as_double(); + double height_max = this->get_parameter("height_max").as_double(); + impl_.setHeightLimits(height_min, height_max); + + // Setup parameter callback + param_callback_handle_ = this->add_on_set_parameters_callback( + std::bind(&ExtractPolygonalPrismData::config_callback, this, std::placeholders::_1)); // Advertise the output topics - pub_output_ = advertise(*pnh_, "output", max_queue_size_); + pub_output_ = advertise(*this, "output", max_queue_size_); onInitPostProcess(); } @@ -68,22 +75,22 @@ pcl_ros::ExtractPolygonalPrismData::onInit() void pcl_ros::ExtractPolygonalPrismData::subscribe() { - sub_hull_filter_.subscribe(*pnh_, "planar_hull", max_queue_size_); - sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_); + sub_hull_filter_.subscribe(*this, "planar_hull", rmw_qos_profile_sensor_data); + sub_input_filter_.subscribe(*this, "input", rmw_qos_profile_sensor_data); // Create the objects here if (approximate_sync_) { sync_input_hull_indices_a_ = - boost::make_shared>>(max_queue_size_); } else { sync_input_hull_indices_e_ = - boost::make_shared>>(max_queue_size_); } if (use_indices_) { - sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); + sub_indices_filter_.subscribe(*this, "indices", rmw_qos_profile_default); if (approximate_sync_) { sync_input_hull_indices_a_->connectInput( sub_input_filter_, sub_hull_filter_, @@ -94,8 +101,6 @@ pcl_ros::ExtractPolygonalPrismData::subscribe() sub_indices_filter_); } } else { - sub_input_filter_.registerCallback(bind(&ExtractPolygonalPrismData::input_callback, this, _1)); - if (approximate_sync_) { sync_input_hull_indices_a_->connectInput(sub_input_filter_, sub_hull_filter_, nf_); } else { @@ -105,14 +110,14 @@ pcl_ros::ExtractPolygonalPrismData::subscribe() // Register callbacks if (approximate_sync_) { sync_input_hull_indices_a_->registerCallback( - bind( + std::bind( &ExtractPolygonalPrismData:: - input_hull_indices_callback, this, _1, _2, _3)); + input_hull_indices_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } else { sync_input_hull_indices_e_->registerCallback( - bind( + std::bind( &ExtractPolygonalPrismData:: - input_hull_indices_callback, this, _1, _2, _3)); + input_hull_indices_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } } @@ -129,27 +134,36 @@ pcl_ros::ExtractPolygonalPrismData::unsubscribe() } ////////////////////////////////////////////////////////////////////////////////////////////// -void +rcl_interfaces::msg::SetParametersResult pcl_ros::ExtractPolygonalPrismData::config_callback( - ExtractPolygonalPrismDataConfig & config, - uint32_t level) + const std::vector & parameters) { - double height_min, height_max; - impl_.getHeightLimits(height_min, height_max); - if (height_min != config.height_min) { - height_min = config.height_min; - NODELET_DEBUG( - "[%s::config_callback] Setting new minimum height to the planar model to: %f.", - getName().c_str(), height_min); - impl_.setHeightLimits(height_min, height_max); - } - if (height_max != config.height_max) { - height_max = config.height_max; - NODELET_DEBUG( - "[%s::config_callback] Setting new maximum height to the planar model to: %f.", - getName().c_str(), height_max); - impl_.setHeightLimits(height_min, height_max); + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + for (const auto & param : parameters) { + if (param.get_name() == "height_min") { + double height_min = param.as_double(); + double height_max; + impl_.getHeightLimits(height_min, height_max); + impl_.setHeightLimits(height_min, height_max); + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting new minimum height to the planar model to: %f.", + this->get_name(), height_min); + } else if (param.get_name() == "height_max") { + double height_min, height_max; + height_max = param.as_double(); + impl_.getHeightLimits(height_min, height_max); + impl_.setHeightLimits(height_min, height_max); + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting new maximum height to the planar model to: %f.", + this->get_name(), height_max); + } } + + return result; } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -160,30 +174,31 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback( const PointIndicesConstPtr & indices) { // No subscribers, no work - if (pub_output_.getNumSubscribers() <= 0) { + if (pub_output_->get_subscription_count() <= 0) { return; } // Copy the header (stamp + frame_id) - pcl_msgs::PointIndices inliers; - inliers.header = fromPCL(cloud->header); + pcl_msgs::msg::PointIndices inliers; + inliers.header = pcl_conversions::fromPCL(cloud->header); // If cloud is given, check if it's valid if (!isValid(cloud) || !isValid(hull, "planar_hull")) { - NODELET_ERROR("[%s::input_hull_indices_callback] Invalid input!", getName().c_str()); - pub_output_.publish(inliers); + RCLCPP_ERROR(this->get_logger(), "[%s::input_hull_indices_callback] Invalid input!", this->get_name()); + pub_output_->publish(inliers); return; } // If indices are given, check if they are valid if (indices && !isValid(indices)) { - NODELET_ERROR("[%s::input_hull_indices_callback] Invalid indices!", getName().c_str()); - pub_output_.publish(inliers); + RCLCPP_ERROR(this->get_logger(), "[%s::input_hull_indices_callback] Invalid indices!", this->get_name()); + pub_output_->publish(inliers); return; } /// DEBUG if (indices) { - NODELET_DEBUG( + RCLCPP_DEBUG( + this->get_logger(), "[%s::input_indices_hull_callback]\n" " - PointCloud with %d data points (%s), stamp %f, and " "frame %s on topic %s received.\n" @@ -191,41 +206,43 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback( "frame %s on topic %s received.\n" " - PointIndices with %zu values, stamp %f, and " "frame %s on topic %s received.", - getName().c_str(), - cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( - cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( - "input").c_str(), - hull->width * hull->height, pcl::getFieldsList(*hull).c_str(), fromPCL( - hull->header).stamp.toSec(), hull->header.frame_id.c_str(), pnh_->resolveName( - "planar_hull").c_str(), - indices->indices.size(), indices->header.stamp.toSec(), - indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); + this->get_name(), + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), + rclcpp::Time(pcl_conversions::fromPCL(cloud->header).stamp).seconds(), + cloud->header.frame_id.c_str(), "input", + hull->width * hull->height, pcl::getFieldsList(*hull).c_str(), + rclcpp::Time(pcl_conversions::fromPCL(hull->header).stamp).seconds(), + hull->header.frame_id.c_str(), "planar_hull", + indices->indices.size(), rclcpp::Time(indices->header.stamp).seconds(), + indices->header.frame_id.c_str(), "indices"); } else { - NODELET_DEBUG( + RCLCPP_DEBUG( + this->get_logger(), "[%s::input_indices_hull_callback]\n" " - PointCloud with %d data points (%s), stamp %f, and " "frame %s on topic %s received.\n" " - PointCloud with %d data points (%s), stamp %f, and " "frame %s on topic %s received.", - getName().c_str(), - cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( - cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( - "input").c_str(), - hull->width * hull->height, pcl::getFieldsList(*hull).c_str(), fromPCL( - hull->header).stamp.toSec(), hull->header.frame_id.c_str(), pnh_->resolveName( - "planar_hull").c_str()); + this->get_name(), + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), + rclcpp::Time(pcl_conversions::fromPCL(cloud->header).stamp).seconds(), + cloud->header.frame_id.c_str(), "input", + hull->width * hull->height, pcl::getFieldsList(*hull).c_str(), + rclcpp::Time(pcl_conversions::fromPCL(hull->header).stamp).seconds(), + hull->header.frame_id.c_str(), "planar_hull"); } /// if (cloud->header.frame_id != hull->header.frame_id) { - NODELET_DEBUG( + RCLCPP_DEBUG( + this->get_logger(), "[%s::input_hull_callback] Planar hull has a different TF frame (%s) than the input " "point cloud (%s)! Using TF to transform.", - getName().c_str(), hull->header.frame_id.c_str(), cloud->header.frame_id.c_str()); + this->get_name(), hull->header.frame_id.c_str(), cloud->header.frame_id.c_str()); PointCloud planar_hull; - if (!pcl_ros::transformPointCloud(cloud->header.frame_id, *hull, planar_hull, tf_listener_)) { + if (!pcl_ros::transformPointCloud(cloud->header.frame_id, *hull, planar_hull, tf_buffer_)) { // Publish empty before return - pub_output_.publish(inliers); + pub_output_->publish(inliers); return; } impl_.setInputPlanarHull(pcl_ptr(planar_hull.makeShared())); @@ -250,12 +267,13 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback( moveFromPCL(pcl_inliers, inliers); } // Enforce that the TF frame and the timestamp are copied - inliers.header = fromPCL(cloud->header); - pub_output_.publish(inliers); - NODELET_DEBUG( + inliers.header = pcl_conversions::fromPCL(cloud->header); + pub_output_->publish(inliers); + RCLCPP_DEBUG( + this->get_logger(), "[%s::input_hull_callback] Publishing %zu indices.", - getName().c_str(), inliers.indices.size()); + this->get_name(), inliers.indices.size()); } typedef pcl_ros::ExtractPolygonalPrismData ExtractPolygonalPrismData; -PLUGINLIB_EXPORT_CLASS(ExtractPolygonalPrismData, nodelet::Nodelet) +RCLCPP_COMPONENTS_REGISTER_NODE(ExtractPolygonalPrismData) \ No newline at end of file diff --git a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp index b7fcd5399..57ecefab5 100644 --- a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp @@ -35,7 +35,7 @@ * */ -#include +#include #include #include #include @@ -50,80 +50,90 @@ pcl_ros::SACSegmentation::onInit() // Call the super onInit () PCLNodelet::onInit(); - // Advertise the output topics - pub_indices_ = advertise(*pnh_, "inliers", max_queue_size_); - pub_model_ = advertise(*pnh_, "model", max_queue_size_); + pub_indices_ = advertise(*this, "inliers", max_queue_size_); + pub_model_ = advertise(*this, "model", max_queue_size_); // ---[ Mandatory parameters - int model_type; - if (!pnh_->getParam("model_type", model_type)) { - NODELET_ERROR("[onInit] Need a 'model_type' parameter to be set before continuing!"); + this->declare_parameter("model_type", -1); + this->declare_parameter("distance_threshold", 0.0); + this->declare_parameter("method_type", 0); + this->declare_parameter("axis", std::vector{0.0, 0.0, 0.0}); + this->declare_parameter("min_inliers", 0); + this->declare_parameter("max_iterations", 50); + this->declare_parameter("probability", 0.99); + this->declare_parameter("eps_angle", 0.0); + this->declare_parameter("optimize_coefficients", true); + this->declare_parameter("radius_min", 0.0); + this->declare_parameter("radius_max", std::numeric_limits::max()); + this->declare_parameter("input_frame", std::string("")); + this->declare_parameter("output_frame", std::string("")); + + int model_type = this->get_parameter("model_type").as_int(); + if (model_type == -1) { + RCLCPP_ERROR(this->get_logger(), "[onInit] Need a 'model_type' parameter to be set before continuing!"); return; } - double threshold; // unused - set via dynamic reconfigure in the callback - if (!pnh_->getParam("distance_threshold", threshold)) { - NODELET_ERROR("[onInit] Need a 'distance_threshold' parameter to be set before continuing!"); + + double threshold = this->get_parameter("distance_threshold").as_double(); + if (threshold == 0.0) { + RCLCPP_ERROR(this->get_logger(), "[onInit] Need a 'distance_threshold' parameter to be set before continuing!"); return; } // ---[ Optional parameters - int method_type = 0; - pnh_->getParam("method_type", method_type); - - XmlRpc::XmlRpcValue axis_param; - pnh_->getParam("axis", axis_param); + int method_type = this->get_parameter("method_type").as_int(); + + std::vector axis_param = this->get_parameter("axis").as_double_array(); Eigen::Vector3f axis = Eigen::Vector3f::Zero(); - - switch (axis_param.getType()) { - case XmlRpc::XmlRpcValue::TypeArray: - { - if (axis_param.size() != 3) { - NODELET_ERROR( - "[%s::onInit] Parameter 'axis' given but with a different number of values (%d) " - "than required (3)!", - getName().c_str(), axis_param.size()); - return; - } - for (int i = 0; i < 3; ++i) { - if (axis_param[i].getType() != XmlRpc::XmlRpcValue::TypeDouble) { - NODELET_ERROR( - "[%s::onInit] Need floating point values for 'axis' parameter.", - getName().c_str()); - return; - } - double value = axis_param[i]; axis[i] = value; - } - break; - } - default: - { - break; - } + + if (axis_param.size() == 3) { + for (int i = 0; i < 3; ++i) { + axis[i] = axis_param[i]; + } + } else if (axis_param.size() != 0) { + RCLCPP_ERROR( + this->get_logger(), + "[%s::onInit] Parameter 'axis' given but with a different number of values (%zu) than required (3)!", + this->get_name(), axis_param.size()); + return; } + min_inliers_ = this->get_parameter("min_inliers").as_int(); + // Initialize the random number generator srand(time(0)); - // Enable the dynamic reconfigure service - srv_ = boost::make_shared>(*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind( - &SACSegmentation::config_callback, this, _1, _2); - srv_->setCallback(f); + // Setup parameter callback + param_callback_handle_ = this->add_on_set_parameters_callback( + std::bind(&SACSegmentation::config_callback, this, std::placeholders::_1)); - NODELET_DEBUG( + RCLCPP_DEBUG( + this->get_logger(), "[%s::onInit] Nodelet successfully created with the following parameters:\n" " - model_type : %d\n" " - method_type : %d\n" " - model_threshold : %f\n" " - axis : [%f, %f, %f]\n", - getName().c_str(), model_type, method_type, threshold, + this->get_name(), model_type, method_type, threshold, axis[0], axis[1], axis[2]); // Set given parameters here impl_.setModelType(model_type); impl_.setMethodType(method_type); impl_.setAxis(axis); + impl_.setDistanceThreshold(threshold); + impl_.setMaxIterations(this->get_parameter("max_iterations").as_int()); + impl_.setProbability(this->get_parameter("probability").as_double()); + impl_.setEpsAngle(this->get_parameter("eps_angle").as_double()); + impl_.setOptimizeCoefficients(this->get_parameter("optimize_coefficients").as_bool()); + + double radius_min = this->get_parameter("radius_min").as_double(); + double radius_max = this->get_parameter("radius_max").as_double(); + impl_.setRadiusLimits(radius_min, radius_max); + + tf_input_frame_ = this->get_parameter("input_frame").as_string(); + tf_output_frame_ = this->get_parameter("output_frame").as_string(); onInitPostProcess(); } @@ -135,8 +145,8 @@ pcl_ros::SACSegmentation::subscribe() // If we're supposed to look for PointIndices (indices) if (use_indices_) { // Subscribe to the input using a filter - sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_); - sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); + sub_input_filter_.subscribe(*this, "input", rmw_qos_profile_sensor_data); + sub_indices_filter_.subscribe(*this, "indices", rmw_qos_profile_default); // when "use_indices" is set to true, and "latched_indices" is set to true, // we'll subscribe and get a separate callback for PointIndices that will @@ -144,47 +154,47 @@ pcl_ros::SACSegmentation::subscribe() // will take care of meshing the new PointClouds with the old saved indices. if (latched_indices_) { // Subscribe to a callback that saves the indices - sub_indices_filter_.registerCallback(bind(&SACSegmentation::indices_callback, this, _1)); + sub_indices_filter_.registerCallback(std::bind(&SACSegmentation::indices_callback, this, std::placeholders::_1)); // Subscribe to a callback that sets the header of the saved indices to the cloud header - sub_input_filter_.registerCallback(bind(&SACSegmentation::input_callback, this, _1)); + sub_input_filter_.registerCallback(std::bind(&SACSegmentation::input_callback, this, std::placeholders::_1)); // Synchronize the two topics. No need for an approximate synchronizer here, as we'll // match the timestamps exactly sync_input_indices_e_ = - boost::make_shared>>(max_queue_size_); - sync_input_indices_e_->connectInput(sub_input_filter_, nf_pi_); + sync_input_indices_e_->connectInput(sub_input_filter_, nf_); sync_input_indices_e_->registerCallback( - bind( + std::bind( &SACSegmentation::input_indices_callback, this, - _1, _2)); + std::placeholders::_1, std::placeholders::_2)); } else { // "latched_indices" not set, proceed with regular pairs if (approximate_sync_) { sync_input_indices_a_ = - boost::make_shared>>(max_queue_size_); sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); sync_input_indices_a_->registerCallback( - bind( + std::bind( &SACSegmentation::input_indices_callback, this, - _1, _2)); + std::placeholders::_1, std::placeholders::_2)); } else { sync_input_indices_e_ = - boost::make_shared>>(max_queue_size_); sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); sync_input_indices_e_->registerCallback( - bind( + std::bind( &SACSegmentation::input_indices_callback, this, - _1, _2)); + std::placeholders::_1, std::placeholders::_2)); } } } else { // Subscribe in an old fashion to input only (no filters) sub_input_ = - pnh_->subscribe( + this->create_subscription( "input", max_queue_size_, - bind(&SACSegmentation::input_indices_callback, this, _1, PointIndicesConstPtr())); + std::bind(&SACSegmentation::input_indices_callback, this, std::placeholders::_1, PointIndicesConstPtr())); } } @@ -196,85 +206,87 @@ pcl_ros::SACSegmentation::unsubscribe() sub_input_filter_.unsubscribe(); sub_indices_filter_.unsubscribe(); } else { - sub_input_.shutdown(); + sub_input_.reset(); } } ////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::SACSegmentation::config_callback(SACSegmentationConfig & config, uint32_t level) +rcl_interfaces::msg::SetParametersResult +pcl_ros::SACSegmentation::config_callback(const std::vector & parameters) { - boost::mutex::scoped_lock lock(mutex_); - - if (impl_.getDistanceThreshold() != config.distance_threshold) { - // sac_->setDistanceThreshold (threshold_); - done in initSAC - impl_.setDistanceThreshold(config.distance_threshold); - NODELET_DEBUG( - "[%s::config_callback] Setting new distance to model threshold to: %f.", - getName().c_str(), config.distance_threshold); - } - // The maximum allowed difference between the model normal and the given axis _in radians_ - if (impl_.getEpsAngle() != config.eps_angle) { - impl_.setEpsAngle(config.eps_angle); - NODELET_DEBUG( - "[%s::config_callback] Setting new epsilon angle to model threshold to: %f (%f degrees).", - getName().c_str(), config.eps_angle, config.eps_angle * 180.0 / M_PI); - } - - // Number of inliers - if (min_inliers_ != config.min_inliers) { - min_inliers_ = config.min_inliers; - NODELET_DEBUG( - "[%s::config_callback] Setting new minimum number of inliers to: %d.", - getName().c_str(), min_inliers_); - } - - if (impl_.getMaxIterations() != config.max_iterations) { - // sac_->setMaxIterations (max_iterations_); - done in initSAC - impl_.setMaxIterations(config.max_iterations); - NODELET_DEBUG( - "[%s::config_callback] Setting new maximum number of iterations to: %d.", - getName().c_str(), config.max_iterations); - } - if (impl_.getProbability() != config.probability) { - // sac_->setProbability (probability_); - done in initSAC - impl_.setProbability(config.probability); - NODELET_DEBUG( - "[%s::config_callback] Setting new probability to: %f.", - getName().c_str(), config.probability); - } - if (impl_.getOptimizeCoefficients() != config.optimize_coefficients) { - impl_.setOptimizeCoefficients(config.optimize_coefficients); - NODELET_DEBUG( - "[%s::config_callback] Setting coefficient optimization to: %s.", - getName().c_str(), (config.optimize_coefficients) ? "true" : "false"); - } - - double radius_min, radius_max; - impl_.getRadiusLimits(radius_min, radius_max); - if (radius_min != config.radius_min) { - radius_min = config.radius_min; - NODELET_DEBUG("[config_callback] Setting minimum allowable model radius to: %f.", radius_min); - impl_.setRadiusLimits(radius_min, radius_max); - } - if (radius_max != config.radius_max) { - radius_max = config.radius_max; - NODELET_DEBUG("[config_callback] Setting maximum allowable model radius to: %f.", radius_max); - impl_.setRadiusLimits(radius_min, radius_max); - } - - if (tf_input_frame_ != config.input_frame) { - tf_input_frame_ = config.input_frame; - NODELET_DEBUG("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str()); - NODELET_WARN("input_frame TF not implemented yet!"); - } - if (tf_output_frame_ != config.output_frame) { - tf_output_frame_ = config.output_frame; - NODELET_DEBUG( - "[config_callback] Setting the output TF frame to: %s.", - tf_output_frame_.c_str()); - NODELET_WARN("output_frame TF not implemented yet!"); + std::lock_guard lock(mutex_); + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + for (const auto & param : parameters) { + if (param.get_name() == "distance_threshold") { + double distance_threshold = param.as_double(); + impl_.setDistanceThreshold(distance_threshold); + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting new distance to model threshold to: %f.", + this->get_name(), distance_threshold); + } else if (param.get_name() == "eps_angle") { + double eps_angle = param.as_double(); + impl_.setEpsAngle(eps_angle); + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting new epsilon angle to model threshold to: %f (%f degrees).", + this->get_name(), eps_angle, eps_angle * 180.0 / M_PI); + } else if (param.get_name() == "min_inliers") { + min_inliers_ = param.as_int(); + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting new minimum number of inliers to: %d.", + this->get_name(), min_inliers_); + } else if (param.get_name() == "max_iterations") { + int max_iterations = param.as_int(); + impl_.setMaxIterations(max_iterations); + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting new maximum number of iterations to: %d.", + this->get_name(), max_iterations); + } else if (param.get_name() == "probability") { + double probability = param.as_double(); + impl_.setProbability(probability); + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting new probability to: %f.", + this->get_name(), probability); + } else if (param.get_name() == "optimize_coefficients") { + bool optimize_coefficients = param.as_bool(); + impl_.setOptimizeCoefficients(optimize_coefficients); + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting coefficient optimization to: %s.", + this->get_name(), optimize_coefficients ? "true" : "false"); + } else if (param.get_name() == "radius_min") { + double radius_min = param.as_double(); + double radius_max; + impl_.getRadiusLimits(radius_min, radius_max); + impl_.setRadiusLimits(radius_min, radius_max); + RCLCPP_DEBUG(this->get_logger(), "[config_callback] Setting minimum allowable model radius to: %f.", radius_min); + } else if (param.get_name() == "radius_max") { + double radius_min, radius_max; + radius_max = param.as_double(); + impl_.getRadiusLimits(radius_min, radius_max); + impl_.setRadiusLimits(radius_min, radius_max); + RCLCPP_DEBUG(this->get_logger(), "[config_callback] Setting maximum allowable model radius to: %f.", radius_max); + } else if (param.get_name() == "input_frame") { + tf_input_frame_ = param.as_string(); + RCLCPP_DEBUG(this->get_logger(), "[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str()); + RCLCPP_WARN(this->get_logger(), "input_frame TF not implemented yet!"); + } else if (param.get_name() == "output_frame") { + tf_output_frame_ = param.as_string(); + RCLCPP_DEBUG( + this->get_logger(), + "[config_callback] Setting the output TF frame to: %s.", + tf_output_frame_.c_str()); + RCLCPP_WARN(this->get_logger(), "output_frame TF not implemented yet!"); + } } + + return result; } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -283,49 +295,51 @@ pcl_ros::SACSegmentation::input_indices_callback( const PointCloudConstPtr & cloud, const PointIndicesConstPtr & indices) { - boost::mutex::scoped_lock lock(mutex_); + std::lock_guard lock(mutex_); - pcl_msgs::PointIndices inliers; - pcl_msgs::ModelCoefficients model; + pcl_msgs::msg::PointIndices inliers; + pcl_msgs::msg::ModelCoefficients model; // Enforce that the TF frame and the timestamp are copied inliers.header = model.header = fromPCL(cloud->header); // If cloud is given, check if it's valid if (!isValid(cloud)) { - NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str()); - pub_indices_.publish(inliers); - pub_model_.publish(model); + RCLCPP_ERROR(this->get_logger(), "[%s::input_indices_callback] Invalid input!", this->get_name()); + pub_indices_->publish(inliers); + pub_model_->publish(model); return; } // If indices are given, check if they are valid if (indices && !isValid(indices)) { - NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str()); - pub_indices_.publish(inliers); - pub_model_.publish(model); + RCLCPP_ERROR(this->get_logger(), "[%s::input_indices_callback] Invalid indices!", this->get_name()); + pub_indices_->publish(inliers); + pub_model_->publish(model); return; } /// DEBUG if (indices && !indices->header.frame_id.empty()) { - NODELET_DEBUG( + RCLCPP_DEBUG( + this->get_logger(), "[%s::input_indices_callback]\n" " - PointCloud with %d data points (%s), stamp %f, and " "frame %s on topic %s received.\n" " - PointIndices with %zu values, stamp %f, and " "frame %s on topic %s received.", - getName().c_str(), - cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( - cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( - "input").c_str(), - indices->indices.size(), indices->header.stamp.toSec(), - indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); + this->get_name(), + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), + rclcpp::Time(fromPCL(cloud->header).stamp).seconds(), + cloud->header.frame_id.c_str(), "input", + indices->indices.size(), rclcpp::Time(indices->header.stamp).seconds(), + indices->header.frame_id.c_str(), "indices"); } else { - NODELET_DEBUG( + RCLCPP_DEBUG( + this->get_logger(), "[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and " "frame %s on topic %s received.", - getName().c_str(), cloud->width * cloud->height, fromPCL( - cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( - "input").c_str()); + this->get_name(), cloud->width * cloud->height, + rclcpp::Time(fromPCL(cloud->header).stamp).seconds(), + cloud->header.frame_id.c_str(), "input"); } /// @@ -334,7 +348,7 @@ pcl_ros::SACSegmentation::input_indices_callback( PointCloudConstPtr cloud_tf; /* if (!tf_input_frame_.empty () && cloud->header.frame_id != tf_input_frame_) { - NODELET_DEBUG ("[input_callback] Transforming input dataset from %s to %s.", + RCLCPP_DEBUG (this->get_logger(), "[input_callback] Transforming input dataset from %s to %s.", // cloud->header.frame_id.c_str (), tf_input_frame_.c_str ()); // Save the original frame ID // Convert the cloud into the different frame @@ -376,16 +390,17 @@ pcl_ros::SACSegmentation::input_indices_callback( } // Publish - pub_indices_.publish(boost::make_shared(inliers)); - pub_model_.publish(boost::make_shared(model)); - NODELET_DEBUG( + pub_indices_->publish(std::make_shared(inliers)); + pub_model_->publish(std::make_shared(model)); + RCLCPP_DEBUG( + this->get_logger(), "[%s::input_indices_callback] Published PointIndices with %zu values on topic %s, " "and ModelCoefficients with %zu values on topic %s", - getName().c_str(), inliers.indices.size(), pnh_->resolveName("inliers").c_str(), - model.values.size(), pnh_->resolveName("model").c_str()); + this->get_name(), inliers.indices.size(), "inliers", + model.values.size(), "model"); if (inliers.indices.empty()) { - NODELET_WARN("[%s::input_indices_callback] No inliers found!", getName().c_str()); + RCLCPP_WARN(this->get_logger(), "[%s::input_indices_callback] No inliers found!", this->get_name()); } } @@ -396,83 +411,94 @@ pcl_ros::SACSegmentationFromNormals::onInit() // Call the super onInit () PCLNodelet::onInit(); - // Enable the dynamic reconfigure service - srv_ = boost::make_shared>(*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind( - &SACSegmentationFromNormals::config_callback, this, _1, _2); - srv_->setCallback(f); + // Declare parameters + this->declare_parameter("model_type", -1); + this->declare_parameter("distance_threshold", 0.0); + this->declare_parameter("method_type", 0); + this->declare_parameter("axis", std::vector{0.0, 0.0, 0.0}); + this->declare_parameter("min_inliers", 0); + this->declare_parameter("max_iterations", 50); + this->declare_parameter("probability", 0.99); + this->declare_parameter("eps_angle", 0.0); + this->declare_parameter("optimize_coefficients", true); + this->declare_parameter("normal_distance_weight", 0.1); + this->declare_parameter("radius_min", 0.0); + this->declare_parameter("radius_max", std::numeric_limits::max()); + + // Setup parameter callback + param_callback_handle_ = this->add_on_set_parameters_callback( + std::bind(&SACSegmentationFromNormals::config_callback, this, std::placeholders::_1)); // Advertise the output topics - pub_indices_ = advertise(*pnh_, "inliers", max_queue_size_); - pub_model_ = advertise(*pnh_, "model", max_queue_size_); + pub_indices_ = advertise(*this, "inliers", max_queue_size_); + pub_model_ = advertise(*this, "model", max_queue_size_); // ---[ Mandatory parameters - int model_type; - if (!pnh_->getParam("model_type", model_type)) { - NODELET_ERROR( + int model_type = this->get_parameter("model_type").as_int(); + if (model_type == -1) { + RCLCPP_ERROR( + this->get_logger(), "[%s::onInit] Need a 'model_type' parameter to be set before continuing!", - getName().c_str()); + this->get_name()); return; } - double threshold; // unused - set via dynamic reconfigure in the callback - if (!pnh_->getParam("distance_threshold", threshold)) { - NODELET_ERROR( + + double threshold = this->get_parameter("distance_threshold").as_double(); + if (threshold == 0.0) { + RCLCPP_ERROR( + this->get_logger(), "[%s::onInit] Need a 'distance_threshold' parameter to be set before continuing!", - getName().c_str()); + this->get_name()); return; } // ---[ Optional parameters - int method_type = 0; - pnh_->getParam("method_type", method_type); - - XmlRpc::XmlRpcValue axis_param; - pnh_->getParam("axis", axis_param); + int method_type = this->get_parameter("method_type").as_int(); + + std::vector axis_param = this->get_parameter("axis").as_double_array(); Eigen::Vector3f axis = Eigen::Vector3f::Zero(); - - switch (axis_param.getType()) { - case XmlRpc::XmlRpcValue::TypeArray: - { - if (axis_param.size() != 3) { - NODELET_ERROR( - "[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than " - "required (3)!", - getName().c_str(), axis_param.size()); - return; - } - for (int i = 0; i < 3; ++i) { - if (axis_param[i].getType() != XmlRpc::XmlRpcValue::TypeDouble) { - NODELET_ERROR( - "[%s::onInit] Need floating point values for 'axis' parameter.", - getName().c_str()); - return; - } - double value = axis_param[i]; axis[i] = value; - } - break; - } - default: - { - break; - } + + if (axis_param.size() == 3) { + for (int i = 0; i < 3; ++i) { + axis[i] = axis_param[i]; + } + } else if (axis_param.size() != 0) { + RCLCPP_ERROR( + this->get_logger(), + "[%s::onInit] Parameter 'axis' given but with a different number of values (%zu) than required (3)!", + this->get_name(), axis_param.size()); + return; } + min_inliers_ = this->get_parameter("min_inliers").as_int(); + // Initialize the random number generator srand(time(0)); - NODELET_DEBUG( + RCLCPP_DEBUG( + this->get_logger(), "[%s::onInit] Nodelet successfully created with the following parameters:\n" " - model_type : %d\n" " - method_type : %d\n" " - model_threshold : %f\n" " - axis : [%f, %f, %f]\n", - getName().c_str(), model_type, method_type, threshold, + this->get_name(), model_type, method_type, threshold, axis[0], axis[1], axis[2]); // Set given parameters here impl_.setModelType(model_type); impl_.setMethodType(method_type); impl_.setAxis(axis); + impl_.setDistanceThreshold(threshold); + impl_.setMaxIterations(this->get_parameter("max_iterations").as_int()); + impl_.setProbability(this->get_parameter("probability").as_double()); + impl_.setEpsAngle(this->get_parameter("eps_angle").as_double()); + impl_.setOptimizeCoefficients(this->get_parameter("optimize_coefficients").as_bool()); + impl_.setNormalDistanceWeight(this->get_parameter("normal_distance_weight").as_double()); + + double radius_min = this->get_parameter("radius_min").as_double(); + double radius_max = this->get_parameter("radius_max").as_double(); + impl_.setRadiusLimits(radius_min, radius_max); onInitPostProcess(); } @@ -482,27 +508,28 @@ void pcl_ros::SACSegmentationFromNormals::subscribe() { // Subscribe to the input and normals using filters - sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_); - sub_normals_filter_.subscribe(*pnh_, "normals", max_queue_size_); + sub_input_filter_.subscribe(*this, "input", rmw_qos_profile_sensor_data); + sub_normals_filter_.subscribe(*this, "normals", rmw_qos_profile_sensor_data); // Subscribe to an axis direction along which the model search is to be constrained (the first // 3 model coefficients will be checked) - sub_axis_ = pnh_->subscribe("axis", 1, &SACSegmentationFromNormals::axis_callback, this); + sub_axis_ = this->create_subscription( + "axis", 1, std::bind(&SACSegmentationFromNormals::axis_callback, this, std::placeholders::_1)); if (approximate_sync_) { sync_input_normals_indices_a_ = - boost::make_shared>>(max_queue_size_); } else { sync_input_normals_indices_e_ = - boost::make_shared>>(max_queue_size_); } // If we're supposed to look for PointIndices (indices) if (use_indices_) { // Subscribe to the input using a filter - sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); + sub_indices_filter_.subscribe(*this, "indices", rmw_qos_profile_default); if (approximate_sync_) { sync_input_normals_indices_a_->connectInput( @@ -515,7 +542,7 @@ pcl_ros::SACSegmentationFromNormals::subscribe() } } else { // Create a different callback for copying over the timestamp to fake indices - sub_input_filter_.registerCallback(bind(&SACSegmentationFromNormals::input_callback, this, _1)); + sub_input_filter_.registerCallback(std::bind(&SACSegmentationFromNormals::input_callback, this, std::placeholders::_1)); if (approximate_sync_) { sync_input_normals_indices_a_->connectInput(sub_input_filter_, sub_normals_filter_, nf_); @@ -526,14 +553,14 @@ pcl_ros::SACSegmentationFromNormals::subscribe() if (approximate_sync_) { sync_input_normals_indices_a_->registerCallback( - bind( + std::bind( &SACSegmentationFromNormals:: - input_normals_indices_callback, this, _1, _2, _3)); + input_normals_indices_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } else { sync_input_normals_indices_e_->registerCallback( - bind( + std::bind( &SACSegmentationFromNormals:: - input_normals_indices_callback, this, _1, _2, _3)); + input_normals_indices_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } } @@ -544,7 +571,7 @@ pcl_ros::SACSegmentationFromNormals::unsubscribe() sub_input_filter_.unsubscribe(); sub_normals_filter_.unsubscribe(); - sub_axis_.shutdown(); + sub_axis_.reset(); if (use_indices_) { sub_indices_filter_.unsubscribe(); @@ -554,99 +581,106 @@ pcl_ros::SACSegmentationFromNormals::unsubscribe() ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::SACSegmentationFromNormals::axis_callback( - const pcl_msgs::ModelCoefficientsConstPtr & model) + const ModelCoefficientsConstPtr & model) { - boost::mutex::scoped_lock lock(mutex_); + std::lock_guard lock(mutex_); if (model->values.size() < 3) { - NODELET_ERROR( + RCLCPP_ERROR( + this->get_logger(), "[%s::axis_callback] Invalid axis direction / model coefficients with %zu values sent on %s!", - getName().c_str(), model->values.size(), pnh_->resolveName("axis").c_str()); + this->get_name(), model->values.size(), "axis"); return; } - NODELET_DEBUG( + RCLCPP_DEBUG( + this->get_logger(), "[%s::axis_callback] Received axis direction: %f %f %f", - getName().c_str(), model->values[0], model->values[1], model->values[2]); + this->get_name(), model->values[0], model->values[1], model->values[2]); Eigen::Vector3f axis(model->values[0], model->values[1], model->values[2]); impl_.setAxis(axis); } ////////////////////////////////////////////////////////////////////////////////////////////// -void +rcl_interfaces::msg::SetParametersResult pcl_ros::SACSegmentationFromNormals::config_callback( - SACSegmentationFromNormalsConfig & config, - uint32_t level) + const std::vector & parameters) { - boost::mutex::scoped_lock lock(mutex_); - - if (impl_.getDistanceThreshold() != config.distance_threshold) { - impl_.setDistanceThreshold(config.distance_threshold); - NODELET_DEBUG( - "[%s::config_callback] Setting distance to model threshold to: %f.", - getName().c_str(), config.distance_threshold); - } - // The maximum allowed difference between the model normal and the given axis _in radians_ - if (impl_.getEpsAngle() != config.eps_angle) { - impl_.setEpsAngle(config.eps_angle); - NODELET_DEBUG( - "[%s::config_callback] Setting new epsilon angle to model threshold to: %f (%f degrees).", - getName().c_str(), config.eps_angle, config.eps_angle * 180.0 / M_PI); - } - - if (impl_.getMaxIterations() != config.max_iterations) { - impl_.setMaxIterations(config.max_iterations); - NODELET_DEBUG( - "[%s::config_callback] Setting new maximum number of iterations to: %d.", - getName().c_str(), config.max_iterations); - } - - // Number of inliers - if (min_inliers_ != config.min_inliers) { - min_inliers_ = config.min_inliers; - NODELET_DEBUG( - "[%s::config_callback] Setting new minimum number of inliers to: %d.", - getName().c_str(), min_inliers_); - } - - - if (impl_.getProbability() != config.probability) { - impl_.setProbability(config.probability); - NODELET_DEBUG( - "[%s::config_callback] Setting new probability to: %f.", - getName().c_str(), config.probability); - } - - if (impl_.getOptimizeCoefficients() != config.optimize_coefficients) { - impl_.setOptimizeCoefficients(config.optimize_coefficients); - NODELET_DEBUG( - "[%s::config_callback] Setting coefficient optimization to: %s.", - getName().c_str(), (config.optimize_coefficients) ? "true" : "false"); - } - - if (impl_.getNormalDistanceWeight() != config.normal_distance_weight) { - impl_.setNormalDistanceWeight(config.normal_distance_weight); - NODELET_DEBUG( - "[%s::config_callback] Setting new distance weight to: %f.", - getName().c_str(), config.normal_distance_weight); + std::lock_guard lock(mutex_); + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + for (const auto & param : parameters) { + if (param.get_name() == "distance_threshold") { + double distance_threshold = param.as_double(); + impl_.setDistanceThreshold(distance_threshold); + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting distance to model threshold to: %f.", + this->get_name(), distance_threshold); + } else if (param.get_name() == "eps_angle") { + double eps_angle = param.as_double(); + impl_.setEpsAngle(eps_angle); + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting new epsilon angle to model threshold to: %f (%f degrees).", + this->get_name(), eps_angle, eps_angle * 180.0 / M_PI); + } else if (param.get_name() == "max_iterations") { + int max_iterations = param.as_int(); + impl_.setMaxIterations(max_iterations); + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting new maximum number of iterations to: %d.", + this->get_name(), max_iterations); + } else if (param.get_name() == "min_inliers") { + min_inliers_ = param.as_int(); + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting new minimum number of inliers to: %d.", + this->get_name(), min_inliers_); + } else if (param.get_name() == "probability") { + double probability = param.as_double(); + impl_.setProbability(probability); + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting new probability to: %f.", + this->get_name(), probability); + } else if (param.get_name() == "optimize_coefficients") { + bool optimize_coefficients = param.as_bool(); + impl_.setOptimizeCoefficients(optimize_coefficients); + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting coefficient optimization to: %s.", + this->get_name(), optimize_coefficients ? "true" : "false"); + } else if (param.get_name() == "normal_distance_weight") { + double normal_distance_weight = param.as_double(); + impl_.setNormalDistanceWeight(normal_distance_weight); + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting new distance weight to: %f.", + this->get_name(), normal_distance_weight); + } else if (param.get_name() == "radius_min") { + double radius_min = param.as_double(); + double radius_max; + impl_.getRadiusLimits(radius_min, radius_max); + impl_.setRadiusLimits(radius_min, radius_max); + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting minimum allowable model radius to: %f.", + this->get_name(), radius_min); + } else if (param.get_name() == "radius_max") { + double radius_min, radius_max; + radius_max = param.as_double(); + impl_.getRadiusLimits(radius_min, radius_max); + impl_.setRadiusLimits(radius_min, radius_max); + RCLCPP_DEBUG( + this->get_logger(), + "[%s::config_callback] Setting maximum allowable model radius to: %f.", + this->get_name(), radius_max); + } } - double radius_min, radius_max; - impl_.getRadiusLimits(radius_min, radius_max); - if (radius_min != config.radius_min) { - radius_min = config.radius_min; - NODELET_DEBUG( - "[%s::config_callback] Setting minimum allowable model radius to: %f.", - getName().c_str(), radius_min); - impl_.setRadiusLimits(radius_min, radius_max); - } - if (radius_max != config.radius_max) { - radius_max = config.radius_max; - NODELET_DEBUG( - "[%s::config_callback] Setting maximum allowable model radius to: %f.", - getName().c_str(), radius_max); - impl_.setRadiusLimits(radius_min, radius_max); - } + return result; } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -657,7 +691,7 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback( const PointIndicesConstPtr & indices ) { - boost::mutex::scoped_lock lock(mutex_); + std::lock_guard lock(mutex_); PointIndices inliers; ModelCoefficients model; @@ -665,29 +699,30 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback( inliers.header = model.header = fromPCL(cloud->header); if (impl_.getModelType() < 0) { - NODELET_ERROR("[%s::input_normals_indices_callback] Model type not set!", getName().c_str()); - pub_indices_.publish(boost::make_shared(inliers)); - pub_model_.publish(boost::make_shared(model)); + RCLCPP_ERROR(this->get_logger(), "[%s::input_normals_indices_callback] Model type not set!", this->get_name()); + pub_indices_->publish(std::make_shared(inliers)); + pub_model_->publish(std::make_shared(model)); return; } if (!isValid(cloud)) { // || !isValid (cloud_normals, "normals")) - NODELET_ERROR("[%s::input_normals_indices_callback] Invalid input!", getName().c_str()); - pub_indices_.publish(boost::make_shared(inliers)); - pub_model_.publish(boost::make_shared(model)); + RCLCPP_ERROR(this->get_logger(), "[%s::input_normals_indices_callback] Invalid input!", this->get_name()); + pub_indices_->publish(std::make_shared(inliers)); + pub_model_->publish(std::make_shared(model)); return; } // If indices are given, check if they are valid if (indices && !isValid(indices)) { - NODELET_ERROR("[%s::input_normals_indices_callback] Invalid indices!", getName().c_str()); - pub_indices_.publish(boost::make_shared(inliers)); - pub_model_.publish(boost::make_shared(model)); + RCLCPP_ERROR(this->get_logger(), "[%s::input_normals_indices_callback] Invalid indices!", this->get_name()); + pub_indices_->publish(std::make_shared(inliers)); + pub_model_->publish(std::make_shared(model)); return; } /// DEBUG if (indices && !indices->header.frame_id.empty()) { - NODELET_DEBUG( + RCLCPP_DEBUG( + this->get_logger(), "[%s::input_normals_indices_callback]\n" " - PointCloud with %d data points (%s), stamp %f, and " "frame %s on topic %s received.\n" @@ -695,45 +730,44 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback( "frame %s on topic %s received.\n" " - PointIndices with %zu values, stamp %f, and " "frame %s on topic %s received.", - getName().c_str(), - cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( - cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( - "input").c_str(), - cloud_normals->width * cloud_normals->height, pcl::getFieldsList( - *cloud_normals).c_str(), fromPCL( - cloud_normals->header).stamp.toSec(), - cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str(), - indices->indices.size(), indices->header.stamp.toSec(), - indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); + this->get_name(), + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), + rclcpp::Time(fromPCL(cloud->header).stamp).seconds(), + cloud->header.frame_id.c_str(), "input", + cloud_normals->width * cloud_normals->height, pcl::getFieldsList(*cloud_normals).c_str(), + rclcpp::Time(fromPCL(cloud_normals->header).stamp).seconds(), + cloud_normals->header.frame_id.c_str(), "normals", + indices->indices.size(), rclcpp::Time(indices->header.stamp).seconds(), + indices->header.frame_id.c_str(), "indices"); } else { - NODELET_DEBUG( + RCLCPP_DEBUG( + this->get_logger(), "[%s::input_normals_indices_callback]\n" " - PointCloud with %d data points (%s), stamp %f, and " "frame %s on topic %s received.\n" " - PointCloud with %d data points (%s), stamp %f, and " "frame %s on topic %s received.", - getName().c_str(), - cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( - cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( - "input").c_str(), - cloud_normals->width * cloud_normals->height, pcl::getFieldsList( - *cloud_normals).c_str(), fromPCL( - cloud_normals->header).stamp.toSec(), - cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str()); + this->get_name(), + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), + rclcpp::Time(fromPCL(cloud->header).stamp).seconds(), + cloud->header.frame_id.c_str(), "input", + cloud_normals->width * cloud_normals->height, pcl::getFieldsList(*cloud_normals).c_str(), + rclcpp::Time(fromPCL(cloud_normals->header).stamp).seconds(), + cloud_normals->header.frame_id.c_str(), "normals"); } /// - // Extra checks for safety int cloud_nr_points = cloud->width * cloud->height; int cloud_normals_nr_points = cloud_normals->width * cloud_normals->height; if (cloud_nr_points != cloud_normals_nr_points) { - NODELET_ERROR( + RCLCPP_ERROR( + this->get_logger(), "[%s::input_normals_indices_callback] Number of points in the input dataset (%d) differs " "from the number of points in the normals (%d)!", - getName().c_str(), cloud_nr_points, cloud_normals_nr_points); - pub_indices_.publish(boost::make_shared(inliers)); - pub_model_.publish(boost::make_shared(model)); + this->get_name(), cloud_nr_points, cloud_normals_nr_points); + pub_indices_->publish(std::make_shared(inliers)); + pub_model_->publish(std::make_shared(model)); return; } @@ -766,19 +800,20 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback( } // Publish - pub_indices_.publish(boost::make_shared(inliers)); - pub_model_.publish(boost::make_shared(model)); - NODELET_DEBUG( + pub_indices_->publish(std::make_shared(inliers)); + pub_model_->publish(std::make_shared(model)); + RCLCPP_DEBUG( + this->get_logger(), "[%s::input_normals_callback] Published PointIndices with %zu values on topic %s, and " "ModelCoefficients with %zu values on topic %s", - getName().c_str(), inliers.indices.size(), pnh_->resolveName("inliers").c_str(), - model.values.size(), pnh_->resolveName("model").c_str()); + this->get_name(), inliers.indices.size(), "inliers", + model.values.size(), "model"); if (inliers.indices.empty()) { - NODELET_WARN("[%s::input_indices_callback] No inliers found!", getName().c_str()); + RCLCPP_WARN(this->get_logger(), "[%s::input_indices_callback] No inliers found!", this->get_name()); } } typedef pcl_ros::SACSegmentation SACSegmentation; typedef pcl_ros::SACSegmentationFromNormals SACSegmentationFromNormals; -PLUGINLIB_EXPORT_CLASS(SACSegmentation, nodelet::Nodelet) -PLUGINLIB_EXPORT_CLASS(SACSegmentationFromNormals, nodelet::Nodelet) +RCLCPP_COMPONENTS_REGISTER_NODE(SACSegmentation) +RCLCPP_COMPONENTS_REGISTER_NODE(SACSegmentationFromNormals) \ No newline at end of file diff --git a/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp b/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp index b2a20b262..2b091f2a5 100644 --- a/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp @@ -35,10 +35,12 @@ * */ -#include -#include +#include +#include #include "pcl_ros/segmentation/segment_differences.hpp" +using pcl_conversions::fromPCL; + /////////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::SegmentDifferences::onInit() @@ -48,47 +50,69 @@ pcl_ros::SegmentDifferences::onInit() pub_output_ = advertise(*pnh_, "output", max_queue_size_); - NODELET_DEBUG( + // Declare and get parameters + distance_threshold_ = pnh_->declare_parameter("distance_threshold", 0.0); + + // Set up parameter callback + param_callback_handle_ = pnh_->add_on_set_parameters_callback( + std::bind(&SegmentDifferences::parametersCallback, this, std::placeholders::_1)); + + RCLCPP_DEBUG(pnh_->get_logger(), "[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - max_queue_size : %d", + " - max_queue_size : %d\n" + " - distance_threshold: %f", getName().c_str(), - max_queue_size_); + max_queue_size_, + distance_threshold_); onInitPostProcess(); } +/////////////////////////////////////////////////////////////////////////////////////////////////// +rcl_interfaces::msg::SetParametersResult +pcl_ros::SegmentDifferences::parametersCallback( + const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + for (const auto & parameter : parameters) { + if (parameter.get_name() == "distance_threshold") { + distance_threshold_ = parameter.as_double(); + impl_.setDistanceThreshold(distance_threshold_); + RCLCPP_DEBUG(pnh_->get_logger(), + "[%s::parametersCallback] Setting new distance threshold to: %f.", + getName().c_str(), distance_threshold_); + } + } + + return result; +} + /////////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::SegmentDifferences::subscribe() { // Subscribe to the input using a filter - sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_); - sub_target_filter_.subscribe(*pnh_, "target", max_queue_size_); - - // Enable the dynamic reconfigure service - srv_ = boost::make_shared>(*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind( - &SegmentDifferences::config_callback, this, _1, _2); - srv_->setCallback(f); + sub_input_filter_.subscribe(*pnh_, "input", rmw_qos_profile_sensor_data); + sub_target_filter_.subscribe(*pnh_, "target", rmw_qos_profile_sensor_data); if (approximate_sync_) { sync_input_target_a_ = - boost::make_shared>>(max_queue_size_); sync_input_target_a_->connectInput(sub_input_filter_, sub_target_filter_); sync_input_target_a_->registerCallback( - bind( - &SegmentDifferences::input_target_callback, this, - _1, _2)); + std::bind(&SegmentDifferences::input_target_callback, this, + std::placeholders::_1, std::placeholders::_2)); } else { sync_input_target_e_ = - boost::make_shared>>(max_queue_size_); sync_input_target_e_->connectInput(sub_input_filter_, sub_target_filter_); sync_input_target_e_->registerCallback( - bind( - &SegmentDifferences::input_target_callback, this, - _1, _2)); + std::bind(&SegmentDifferences::input_target_callback, this, + std::placeholders::_1, std::placeholders::_2)); } } @@ -100,19 +124,6 @@ pcl_ros::SegmentDifferences::unsubscribe() sub_target_filter_.unsubscribe(); } -/////////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::SegmentDifferences::config_callback(SegmentDifferencesConfig & config, uint32_t level) -{ - if (impl_.getDistanceThreshold() != config.distance_threshold) { - impl_.setDistanceThreshold(config.distance_threshold); - NODELET_DEBUG( - "[%s::config_callback] Setting new distance threshold to: %f.", - getName().c_str(), config.distance_threshold); - } -} - - ////////////////////////////////////////////////////////////////////////////////////////////// void pcl_ros::SegmentDifferences::input_target_callback( @@ -124,40 +135,42 @@ pcl_ros::SegmentDifferences::input_target_callback( } if (!isValid(cloud) || !isValid(cloud_target, "target")) { - NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str()); + RCLCPP_ERROR(pnh_->get_logger(), "[%s::input_target_callback] Invalid input!", getName().c_str()); PointCloud output; output.header = cloud->header; - pub_output_.publish(ros_ptr(output.makeShared())); + pub_output_.publish(output.makeShared()); return; } - NODELET_DEBUG( - "[%s::input_indices_callback]\n" + RCLCPP_DEBUG(pnh_->get_logger(), + "[%s::input_target_callback]\n" " - PointCloud with %d data points (%s), stamp %f, and " "frame %s on topic %s received.\n" " - PointCloud with %d data points (%s), stamp %f, and " "frame %s on topic %s received.", getName().c_str(), - cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( - cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( - "input").c_str(), + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), + rclcpp::Time(cloud->header.stamp).seconds(), + cloud->header.frame_id.c_str(), + pnh_->resolve_topic_name("input").c_str(), cloud_target->width * cloud_target->height, pcl::getFieldsList(*cloud_target).c_str(), - fromPCL(cloud_target->header).stamp.toSec(), - cloud_target->header.frame_id.c_str(), pnh_->resolveName("target").c_str()); + rclcpp::Time(cloud_target->header.stamp).seconds(), + cloud_target->header.frame_id.c_str(), + pnh_->resolve_topic_name("target").c_str()); - impl_.setInputCloud(pcl_ptr(cloud)); - impl_.setTargetCloud(pcl_ptr(cloud_target)); + impl_.setInputCloud(cloud); + impl_.setTargetCloud(cloud_target); PointCloud output; impl_.segment(output); - pub_output_.publish(ros_ptr(output.makeShared())); - NODELET_DEBUG( - "[%s::segmentAndPublish] Published PointCloud2 with %zu points and stamp %f on topic %s", + pub_output_.publish(output.makeShared()); + RCLCPP_DEBUG(pnh_->get_logger(), + "[%s::input_target_callback] Published PointCloud2 with %zu points and stamp %f on topic %s", getName().c_str(), - output.points.size(), fromPCL(output.header).stamp.toSec(), - pnh_->resolveName("output").c_str()); + output.points.size(), + rclcpp::Time(output.header.stamp).seconds(), + pnh_->resolve_topic_name("output").c_str()); } -typedef pcl_ros::SegmentDifferences SegmentDifferences; -PLUGINLIB_EXPORT_CLASS(SegmentDifferences, nodelet::Nodelet) +RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::SegmentDifferences) \ No newline at end of file