diff --git a/pcl_ros/CMakeLists.txt b/pcl_ros/CMakeLists.txt index 18da2b1e..e162cae0 100644 --- a/pcl_ros/CMakeLists.txt +++ b/pcl_ros/CMakeLists.txt @@ -137,16 +137,23 @@ rclcpp_components_register_node(pcl_ros_filters class_loader_hide_library_symbols(pcl_ros_filters) # ### Declare the pcl_ros_segmentation library -#add_library (pcl_ros_segmentation +add_library (pcl_ros_segmentation SHARED # src/pcl_ros/segmentation/extract_clusters.cpp # src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp -# src/pcl_ros/segmentation/sac_segmentation.cpp + src/pcl_ros/segmentation/sac_segmentation.cpp # src/pcl_ros/segmentation/segment_differences.cpp # src/pcl_ros/segmentation/segmentation.cpp -#) -#target_link_libraries(pcl_ros_segmentation pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) -#add_dependencies(pcl_ros_segmentation ${PROJECT_NAME}_gencfg) -#class_loader_hide_library_symbols(pcl_ros_segmentation) +) +target_link_libraries(pcl_ros_segmentation PUBLIC + ${PCL_LIBRARIES} + pcl_ros_tf + rclcpp_components::component +) +rclcpp_components_register_node(pcl_ros_segmentation + PLUGIN "pcl_ros::SACSegmentation" + EXECUTABLE segmentation_sac_node +) +class_loader_hide_library_symbols(pcl_ros_segmentation) # ### Declare the pcl_ros_surface library #add_library (pcl_ros_surface @@ -293,7 +300,7 @@ install( pcl_ros_filter pcl_ros_filters # pcl_ros_surface -# pcl_ros_segmentation + pcl_ros_segmentation pointcloud_to_pcd_lib combined_pointcloud_to_pcd_lib bag_to_pcd_lib diff --git a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp index ca4ca34f..88021c85 100644 --- a/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp +++ b/pcl_ros/include/pcl_ros/segmentation/sac_segmentation.hpp @@ -38,68 +38,39 @@ #ifndef PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_ #define PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_ -#include -#include #include -#include -#include "pcl_ros/pcl_nodelet.hpp" -#include "pcl_ros/SACSegmentationConfig.hpp" -#include "pcl_ros/SACSegmentationFromNormalsConfig.hpp" +#include + +#include "pcl_ros/pcl_node.hpp" +#include + +// PCL includes +#include namespace pcl_ros { namespace sync_policies = message_filters::sync_policies; //////////////////////////////////////////////////////////////////////////////////////////// -/** \brief @b SACSegmentation represents the Nodelet segmentation class for Sample Consensus - * methods and models, in the sense that it just creates a Nodelet wrapper for generic-purpose +/** \brief @b SACSegmentation represents a segmentation class for Sample Consensus + * methods and models, in the sense that it just creates a wrapper for generic-purpose * SAC-based segmentation. * \author Radu Bogdan Rusu */ -class SACSegmentation : public PCLNodelet +class SACSegmentation : public PCLNode { - typedef pcl::PointCloud PointCloud; - typedef boost::shared_ptr PointCloudPtr; - typedef boost::shared_ptr PointCloudConstPtr; - public: - /** \brief Constructor. */ - SACSegmentation() - : min_inliers_(0) {} - - /** \brief Set the input TF frame the data should be transformed into before processing, - * if input.header.frame_id is different. - * \param tf_frame the TF frame the input PointCloud should be transformed into before processing - */ - inline void setInputTFframe(std::string tf_frame) {tf_input_frame_ = tf_frame;} - - /** \brief Get the TF frame the input PointCloud should be transformed into before processing. */ - inline std::string getInputTFframe() {return tf_input_frame_;} - - /** \brief Set the output TF frame the data should be transformed into after processing. - * \param tf_frame the TF frame the PointCloud should be transformed into after processing - */ - inline void setOutputTFframe(std::string tf_frame) {tf_output_frame_ = tf_frame;} + EIGEN_MAKE_ALIGNED_OPERATOR_NEW - /** \brief Get the TF frame the PointCloud should be transformed into after processing. */ - inline std::string getOutputTFframe() {return tf_output_frame_;} + explicit SACSegmentation(const rclcpp::NodeOptions & options); protected: - // The minimum number of inliers a model must have in order to be considered valid. - int min_inliers_; - - // ROS nodelet attributes - /** \brief The output PointIndices publisher. */ - ros::Publisher pub_indices_; - - /** \brief The output ModelCoefficients publisher. */ - ros::Publisher pub_model_; + /** \brief declare and subscribe to param callback for input_frame and output_frame params */ + void + use_frame_params(); /** \brief The input PointCloud subscriber. */ - ros::Subscriber sub_input_; - - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr> srv_; + rclcpp::Subscription::SharedPtr sub_input_; /** \brief The input TF frame the data should be transformed into, * if input.header.frame_id is different. @@ -114,188 +85,182 @@ class SACSegmentation : public PCLNodelet */ std::string tf_output_frame_; - /** \brief Null passthrough filter, used for pushing empty elements in the - * synchronizer */ - message_filters::PassThrough nf_pi_; - - /** \brief Nodelet initialization routine. */ - virtual void onInit(); - - /** \brief LazyNodelet connection routine. */ - virtual void subscribe(); - virtual void unsubscribe(); + /** \brief Internal mutex. */ + std::mutex mutex_; - /** \brief Dynamic reconfigure callback - * \param config the config object - * \param level the dynamic reconfigure level + /** \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 + * \param model the coefficients of the segmented model */ - void config_callback(SACSegmentationConfig & config, uint32_t level); + inline void + segment( + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointIndices & output, ModelCoefficients & model); - /** \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 Lazy transport subscribe routine. */ + virtual void + subscribe(); - /** \brief Pointer to a set of indices stored internally. - * (used when \a latched_indices_ is set). - */ - PointIndices indices_; + /** \brief Lazy transport unsubscribe routine. */ + virtual void + unsubscribe(); - /** \brief Indices callback. Used when \a latched_indices_ is set. - * \param indices the pointer to the input point cloud indices - */ - inline void - indices_callback(const PointIndicesConstPtr & indices) - { - indices_ = *indices; - } + /** \brief Create publishers for output PointCloud2 data. */ + virtual void + createPublishers(); - /** \brief Input callback. Used when \a latched_indices_ is set. - * \param input the pointer to the input point cloud + /** \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. */ - inline void - input_callback(const PointCloudConstPtr & input) - { - indices_.header = fromPCL(input->header); - PointIndicesConstPtr indices; - indices.reset(new PointIndices(indices_)); - nf_pi_.add(indices); - } + void + computePublish(const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices); private: - /** \brief Internal mutex. */ - boost::mutex mutex_; - - /** \brief The PCL implementation used. */ - pcl::SACSegmentation impl_; + /** \brief Pointer to parameters callback handle. */ + OnSetParametersCallbackHandle::SharedPtr callback_handle_; /** \brief Synchronized input, and indices.*/ - boost::shared_ptr>> sync_input_indices_e_; - boost::shared_ptr>> sync_input_indices_a_; -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;} - - /** \brief Get the TF frame the input PointCloud should be transformed into before processing. */ - inline std::string getInputTFframe() {return tf_input_frame_;} - - /** \brief Set the output TF frame the data should be transformed into after processing. - * \param tf_frame the TF frame the PointCloud should be transformed into after processing - */ - inline void setOutputTFframe(std::string tf_frame) {tf_output_frame_ = tf_frame;} - - /** \brief Get the TF frame the PointCloud should be transformed into after processing. */ - inline std::string getOutputTFframe() {return tf_output_frame_;} - -protected: - // ROS nodelet attributes - /** \brief The normals PointCloud subscriber filter. */ - message_filters::Subscriber sub_normals_filter_; - - /** \brief The input PointCloud subscriber. */ - ros::Subscriber sub_axis_; - - /** \brief Pointer to a dynamic reconfigure service. */ - boost::shared_ptr> srv_; - - /** \brief Input point cloud callback. - * Because we want to use the same synchronizer object, we push back - * empty elements with the same timestamp. - */ - inline void - input_callback(const PointCloudConstPtr & cloud) - { - PointIndices indices; - indices.header.stamp = fromPCL(cloud->header).stamp; - nf_.add(boost::make_shared(indices)); - } - - /** \brief Null passthrough filter, used for pushing empty elements in the - * synchronizer */ - message_filters::PassThrough nf_; - - /** \brief The input TF frame the data should be transformed into, - * if input.header.frame_id is different. - */ - std::string tf_input_frame_; - /** \brief The original data input TF frame. */ - std::string tf_input_orig_frame_; - /** \brief The output TF frame the data should be transformed into, - * if input.header.frame_id is different. - */ - std::string tf_output_frame_; - - /** \brief Nodelet initialization routine. */ - virtual void onInit(); - - /** \brief LazyNodelet connection routine. */ - virtual void subscribe(); - virtual void unsubscribe(); - - /** \brief Model callback - * \param model the sample consensus model found - */ - void axis_callback(const pcl_msgs::ModelCoefficientsConstPtr & model); - - /** \brief Dynamic reconfigure callback - * \param config the config object - * \param level the dynamic reconfigure level - */ - void config_callback(SACSegmentationFromNormalsConfig & config, uint32_t level); + /** \brief The output ModelCoefficients publisher. */ + rclcpp::Publisher::SharedPtr pub_model_; - /** \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 + /** \brief Parameter callback + * \param params parameter values to set */ - void input_normals_indices_callback( - const PointCloudConstPtr & cloud, - const PointCloudNConstPtr & cloud_normals, - const PointIndicesConstPtr & indices); + rcl_interfaces::msg::SetParametersResult + config_callback(const std::vector & params); -private: - /** \brief Internal mutex. */ - boost::mutex mutex_; + /** \brief PointCloud2 + Indices data callback. */ + void + input_indices_callback( + const PointCloud2::ConstSharedPtr & cloud, + const PointIndices::ConstSharedPtr & indices); /** \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 + pcl::SACSegmentation impl_; }; + +//////////////////////////////////////////////////////////////////////////////////////////// +/** \brief @b SACSegmentationFromNormals represents the PCL nodelet segmentation class for + * Sample Consensus methods and models that require the use of surface normals for estimation. + */ +// class SACSegmentationFromNormals : public SACSegmentation +// { +// typedef pcl::PointCloud PointCloud; +// typedef boost::shared_ptr PointCloudPtr; +// typedef boost::shared_ptr PointCloudConstPtr; + +// typedef pcl::PointCloud PointCloudN; +// typedef boost::shared_ptr PointCloudNPtr; +// typedef boost::shared_ptr PointCloudNConstPtr; + +// public: +// /** \brief Set the input TF frame the data should be transformed into before processing, +// * if input.header.frame_id is different. +// * \param tf_frame the TF frame the input PointCloud should be transformed into before processing +// */ +// inline void setInputTFframe(std::string tf_frame) {tf_input_frame_ = tf_frame;} + +// /** \brief Get the TF frame the input PointCloud should be transformed into before processing. */ +// inline std::string getInputTFframe() {return tf_input_frame_;} + +// /** \brief Set the output TF frame the data should be transformed into after processing. +// * \param tf_frame the TF frame the PointCloud should be transformed into after processing +// */ +// inline void setOutputTFframe(std::string tf_frame) {tf_output_frame_ = tf_frame;} + +// /** \brief Get the TF frame the PointCloud should be transformed into after processing. */ +// inline std::string getOutputTFframe() {return tf_output_frame_;} + +// protected: +// // ROS nodelet attributes +// /** \brief The normals PointCloud subscriber filter. */ +// message_filters::Subscriber sub_normals_filter_; + +// /** \brief The input PointCloud subscriber. */ +// ros::Subscriber sub_axis_; + +// /** \brief Pointer to a dynamic reconfigure service. */ +// boost::shared_ptr> srv_; + +// /** \brief Input point cloud callback. +// * Because we want to use the same synchronizer object, we push back +// * empty elements with the same timestamp. +// */ +// inline void +// input_callback(const PointCloudConstPtr & cloud) +// { +// PointIndices indices; +// indices.header.stamp = fromPCL(cloud->header).stamp; +// nf_.add(boost::make_shared(indices)); +// } + +// /** \brief Null passthrough filter, used for pushing empty elements in the +// * synchronizer */ +// message_filters::PassThrough nf_; + +// /** \brief The input TF frame the data should be transformed into, +// * if input.header.frame_id is different. +// */ +// std::string tf_input_frame_; +// /** \brief The original data input TF frame. */ +// std::string tf_input_orig_frame_; +// /** \brief The output TF frame the data should be transformed into, +// * if input.header.frame_id is different. +// */ +// std::string tf_output_frame_; + +// /** \brief Nodelet initialization routine. */ +// virtual void onInit(); + +// /** \brief LazyNodelet connection routine. */ +// virtual void subscribe(); +// virtual void unsubscribe(); + +// /** \brief Model callback +// * \param model the sample consensus model found +// */ +// void axis_callback(const pcl_msgs::ModelCoefficientsConstPtr & model); + +// /** \brief Dynamic reconfigure callback +// * \param config the config object +// * \param level the dynamic reconfigure level +// */ +// void config_callback(SACSegmentationFromNormalsConfig & config, uint32_t level); + +// /** \brief Input point cloud callback. +// * \param cloud the pointer to the input point cloud +// * \param cloud_normals the pointer to the input point cloud normals +// * \param indices the pointer to the input point cloud indices +// */ +// void input_normals_indices_callback( +// const PointCloudConstPtr & cloud, +// const PointCloudNConstPtr & cloud_normals, +// const PointIndicesConstPtr & indices); + +// private: +// /** \brief Internal mutex. */ +// boost::mutex mutex_; + +// /** \brief The PCL implementation used. */ +// pcl::SACSegmentationFromNormals impl_; + +// /** \brief Synchronized input, normals, and indices.*/ +// boost::shared_ptr>> sync_input_normals_indices_a_; +// boost::shared_ptr>> sync_input_normals_indices_e_; + +// public: +// EIGEN_MAKE_ALIGNED_OPERATOR_NEW +// }; } // namespace pcl_ros #endif // PCL_ROS__SEGMENTATION__SAC_SEGMENTATION_HPP_ diff --git a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp index bcc128c4..aeeb0570 100644 --- a/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp +++ b/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp @@ -35,97 +35,211 @@ * */ -#include -#include -#include -#include +#include + #include "pcl_ros/segmentation/sac_segmentation.hpp" +#include +#include "pcl_ros/transforms.hpp" -using pcl_conversions::fromPCL; ////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::SACSegmentation::onInit() +pcl_ros::SACSegmentation::SACSegmentation(const rclcpp::NodeOptions & options) +: PCLNode("SACSegmentationNode", options) { - // 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_); + rcl_interfaces::msg::ParameterDescriptor model_type_desc; + model_type_desc.name = "model_type"; + model_type_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + model_type_desc.description = + "The type of model to use for segmentation."; + { + rcl_interfaces::msg::IntegerRange int_range; + int_range.from_value = 0; + int_range.to_value = 17; + model_type_desc.integer_range.push_back(int_range); + } + declare_parameter( + model_type_desc.name, rclcpp::ParameterValue( + pcl::SACMODEL_PLANE), model_type_desc); + + rcl_interfaces::msg::ParameterDescriptor distance_threshold_desc; + distance_threshold_desc.name = "distance_threshold"; + distance_threshold_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + distance_threshold_desc.description = + "The minimum distance (in meters) for a point to be considered an inlier."; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = 0.0; + float_range.to_value = 100000.0; + distance_threshold_desc.floating_point_range.push_back(float_range); + } + declare_parameter( + distance_threshold_desc.name, rclcpp::ParameterValue(0.01), distance_threshold_desc); + + rcl_interfaces::msg::ParameterDescriptor eps_angle_desc; + eps_angle_desc.name = "eps_angle"; + eps_angle_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + eps_angle_desc.description = + "The maximum allowed difference (radians) between the model normal and the given axis."; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = 0.0; + float_range.to_value = M_PI_2; + eps_angle_desc.floating_point_range.push_back(float_range); + } + declare_parameter( + eps_angle_desc.name, rclcpp::ParameterValue(0.0), eps_angle_desc); + + rcl_interfaces::msg::ParameterDescriptor method_type_desc; + method_type_desc.name = "method_type"; + method_type_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + method_type_desc.description = + "The type of method to use for segmentation."; + { + rcl_interfaces::msg::IntegerRange int_range; + int_range.from_value = 0; + int_range.to_value = 6; + method_type_desc.integer_range.push_back(int_range); + } + declare_parameter( + method_type_desc.name, rclcpp::ParameterValue(pcl::SAC_RANSAC), + method_type_desc); + + rcl_interfaces::msg::ParameterDescriptor axis_desc; + axis_desc.name = "axis"; + axis_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY; + axis_desc.description = + "The axis along which the method need to search for a model perpendicular to."; + declare_parameter( + axis_desc.name, rclcpp::ParameterValue(std::vector({0.0, 0.0, 0.0})), + axis_desc); + + rcl_interfaces::msg::ParameterDescriptor max_iterations_desc; + max_iterations_desc.name = "max_iterations"; + max_iterations_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + max_iterations_desc.description = + "The maximum number of iterations the sample consensus method will run."; + declare_parameter( + max_iterations_desc.name, rclcpp::ParameterValue(pcl::SAC_RANSAC), + max_iterations_desc); + + rcl_interfaces::msg::ParameterDescriptor probability_desc; + probability_desc.name = "probability"; + probability_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + probability_desc.description = + "The desired probability of choosing at least one sample free from outliers."; + { + rcl_interfaces::msg::FloatingPointRange float_range; + float_range.from_value = 0.0; + float_range.to_value = 1.0; + probability_desc.floating_point_range.push_back(float_range); + } + declare_parameter( + probability_desc.name, rclcpp::ParameterValue(0.99), probability_desc); + + rcl_interfaces::msg::ParameterDescriptor optimize_coefficients_desc; + optimize_coefficients_desc.name = "optimize_coefficients"; + optimize_coefficients_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + optimize_coefficients_desc.description = + "Model coefficient refinement. true for enabling model coefficient refinement, false otherwise."; + declare_parameter( + optimize_coefficients_desc.name, rclcpp::ParameterValue(true), optimize_coefficients_desc); + + rcl_interfaces::msg::ParameterDescriptor radius_min_desc; + radius_min_desc.name = "radius_min"; + radius_min_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + radius_min_desc.description = + "The minimum allowable radius for the model (applicable to models that estimate a radius)"; + declare_parameter( + radius_min_desc.name, rclcpp::ParameterValue( + -std::numeric_limits::max()), radius_min_desc); + + rcl_interfaces::msg::ParameterDescriptor radius_max_desc; + radius_max_desc.name = "radius_max"; + radius_max_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + radius_max_desc.description = + "The maximum allowable radius for the model (applicable to models that estimate a radius)"; + declare_parameter( + radius_max_desc.name, rclcpp::ParameterValue( + std::numeric_limits::max()), radius_max_desc); + + std::vector param_names { + model_type_desc.name, + distance_threshold_desc.name, + eps_angle_desc.name, + method_type_desc.name, + axis_desc.name, + max_iterations_desc.name, + probability_desc.name, + optimize_coefficients_desc.name, + radius_min_desc.name, + radius_max_desc.name + }; + + callback_handle_ = + add_on_set_parameters_callback( + std::bind( + &SACSegmentation::config_callback, this, + std::placeholders::_1)); + + config_callback(get_parameters(param_names)); + + createPublishers(); +} - // ---[ Mandatory parameters - int model_type; - if (!pnh_->getParam("model_type", model_type)) { - NODELET_ERROR("[onInit] Need a 'model_type' parameter to be set before continuing!"); - return; +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl_ros::SACSegmentation::computePublish( + const PointCloud2::ConstSharedPtr & input, + const IndicesPtr & indices) +{ + PointCloud2::UniquePtr cloud_tf(new PointCloud2(*input)); // set the output by default + // Check whether the user has given a different output TF frame + if (!tf_output_frame_.empty() && input->header.frame_id != tf_output_frame_) { + RCLCPP_DEBUG( + this->get_logger(), "Transforming input dataset from %s to %s.", + input->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_, *input, cloud_transformed, tf_buffer_)) { + RCLCPP_ERROR( + this->get_logger(), "Error converting input dataset from %s to %s.", + input->header.frame_id.c_str(), tf_output_frame_.c_str()); + return; + } + cloud_tf.reset(new PointCloud2(cloud_transformed)); } - double threshold; // unused - set via dynamic reconfigure in the callback - if (!pnh_->getParam("distance_threshold", threshold)) { - NODELET_ERROR("[onInit] Need a 'distance_threshold' parameter to be set before continuing!"); - return; + if (tf_output_frame_.empty() && input->header.frame_id != tf_input_orig_frame_) { + // no tf_output_frame given, transform the dataset to its original frame + RCLCPP_DEBUG( + this->get_logger(), "Transforming input dataset from %s back to %s.", + input->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_, *input, cloud_transformed, + tf_buffer_)) + { + RCLCPP_ERROR( + this->get_logger(), "Error converting input dataset from %s back to %s.", + input->header.frame_id.c_str(), tf_input_orig_frame_.c_str()); + return; + } + cloud_tf.reset(new PointCloud2(cloud_transformed)); } - // ---[ Optional parameters - int method_type = 0; - pnh_->getParam("method_type", method_type); - - XmlRpc::XmlRpcValue axis_param; - pnh_->getParam("axis", axis_param); - Eigen::Vector3f axis = Eigen::Vector3f::Zero(); - - switch (axis_param.getType()) { - case XmlRpc::XmlRpcValue::TypeArray: - { - if (axis_param.size() != 3) { - NODELET_ERROR( - "[%s::onInit] Parameter 'axis' given but with a different number of values (%d) " - "than required (3)!", - getName().c_str(), axis_param.size()); - return; - } - for (int i = 0; i < 3; ++i) { - if (axis_param[i].getType() != XmlRpc::XmlRpcValue::TypeDouble) { - NODELET_ERROR( - "[%s::onInit] Need floating point values for 'axis' parameter.", - getName().c_str()); - return; - } - double value = axis_param[i]; axis[i] = value; - } - break; - } - default: - { - break; - } + PointIndices output; + ModelCoefficients model; + output.header = model.header = input->header; + // Call the virtual method in the child + if (!input->data.empty()) { + segment(input, indices, output, model); + } else { + RCLCPP_DEBUG(this->get_logger(), "Received empty input point cloud"); } - // Initialize the random number generator - srand(time(0)); - - // Enable the dynamic reconfigure service - srv_ = boost::make_shared>(*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind( - &SACSegmentation::config_callback, this, _1, _2); - srv_->setCallback(f); - - NODELET_DEBUG( - "[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - model_type : %d\n" - " - method_type : %d\n" - " - model_threshold : %f\n" - " - axis : [%f, %f, %f]\n", - getName().c_str(), model_type, method_type, threshold, - axis[0], axis[1], axis[2]); - - // Set given parameters here - impl_.setModelType(model_type); - impl_.setMethodType(method_type); - impl_.setAxis(axis); - - onInitPostProcess(); + // Publish the unique ptr + pub_output_->publish(move(output)); + pub_model_->publish(move(model)); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -135,56 +249,39 @@ pcl_ros::SACSegmentation::subscribe() // If we're supposed to look for PointIndices (indices) if (use_indices_) { // Subscribe to the input using a filter - sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_); - sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); - - // when "use_indices" is set to true, and "latched_indices" is set to true, - // we'll subscribe and get a separate callback for PointIndices that will - // save the indices internally, and a PointCloud + PointIndices callback - // will take care of meshing the new PointClouds with the old saved indices. - if (latched_indices_) { - // Subscribe to a callback that saves the indices - sub_indices_filter_.registerCallback(bind(&SACSegmentation::indices_callback, this, _1)); - // Subscribe to a callback that sets the header of the saved indices to the cloud header - sub_input_filter_.registerCallback(bind(&SACSegmentation::input_callback, this, _1)); - - // Synchronize the two topics. No need for an approximate synchronizer here, as we'll - // match the timestamps exactly + auto sensor_qos_profile = rclcpp::SensorDataQoS().keep_last(max_queue_size_); + sub_input_filter_.subscribe(this, "input", sensor_qos_profile); + sub_indices_filter_.subscribe(this, "indices", sensor_qos_profile); + + if (approximate_sync_) { + sync_input_indices_a_ = + std::make_shared>>(max_queue_size_); + sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); + sync_input_indices_a_->registerCallback( + std::bind( + &SACSegmentation::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_, nf_pi_); + std::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)); - } else { // "latched_indices" not set, proceed with regular pairs - if (approximate_sync_) { - sync_input_indices_a_ = - boost::make_shared>>(max_queue_size_); - sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); - sync_input_indices_a_->registerCallback( - bind( - &SACSegmentation::input_indices_callback, this, - _1, _2)); - } else { - sync_input_indices_e_ = - boost::make_shared>>(max_queue_size_); - sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); - sync_input_indices_e_->registerCallback( - bind( - &SACSegmentation::input_indices_callback, this, - _1, _2)); - } + std::placeholders::_1, std::placeholders::_2)); } } else { + // Workaround for a callback with custom arguments ros2/rclcpp#766 + std::function callback = + std::bind(&SACSegmentation::input_indices_callback, this, std::placeholders::_1, nullptr); + // Subscribe in an old fashion to input only (no filters) sub_input_ = - pnh_->subscribe( - "input", max_queue_size_, - bind(&SACSegmentation::input_indices_callback, this, _1, PointIndicesConstPtr())); + this->create_subscription( + "input", rclcpp::SensorDataQoS().keep_last(max_queue_size_), + callback); } } @@ -196,589 +293,671 @@ pcl_ros::SACSegmentation::unsubscribe() sub_input_filter_.unsubscribe(); sub_indices_filter_.unsubscribe(); } else { - sub_input_.shutdown(); - } -} - -////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::SACSegmentation::config_callback(SACSegmentationConfig & config, uint32_t level) -{ - 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!"); - } -} - -////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::SACSegmentation::input_indices_callback( - const PointCloudConstPtr & cloud, - const PointIndicesConstPtr & indices) -{ - boost::mutex::scoped_lock lock(mutex_); - - pcl_msgs::PointIndices inliers; - pcl_msgs::ModelCoefficients model; - // Enforce that the TF frame and the timestamp are copied - inliers.header = model.header = fromPCL(cloud->header); - - // If cloud is given, check if it's valid - if (!isValid(cloud)) { - NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str()); - pub_indices_.publish(inliers); - pub_model_.publish(model); - return; - } - // If indices are given, check if they are valid - if (indices && !isValid(indices)) { - NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str()); - pub_indices_.publish(inliers); - pub_model_.publish(model); - return; - } - - /// DEBUG - if (indices && !indices->header.frame_id.empty()) { - NODELET_DEBUG( - "[%s::input_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and " - "frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and " - "frame %s on topic %s received.", - getName().c_str(), - cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( - cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( - "input").c_str(), - indices->indices.size(), indices->header.stamp.toSec(), - indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); - } else { - NODELET_DEBUG( - "[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and " - "frame %s on topic %s received.", - getName().c_str(), cloud->width * cloud->height, fromPCL( - cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( - "input").c_str()); - } - /// - - // Check whether the user has given a different input TF frame - tf_input_orig_frame_ = cloud->header.frame_id; - PointCloudConstPtr cloud_tf; -/* if (!tf_input_frame_.empty () && cloud->header.frame_id != tf_input_frame_) - { - NODELET_DEBUG ("[input_callback] Transforming input dataset from %s to %s.", - // cloud->header.frame_id.c_str (), tf_input_frame_.c_str ()); - // Save the original frame ID - // Convert the cloud into the different frame - PointCloud cloud_transformed; - if (!pcl::transformPointCloud (tf_input_frame_, cloud->header.stamp, *cloud, - cloud_transformed, tf_listener_)) - return; - cloud_tf.reset (new PointCloud (cloud_transformed)); - } - else*/ - cloud_tf = cloud; - - IndicesPtr indices_ptr; - if (indices && !indices->header.frame_id.empty()) { - indices_ptr.reset(new std::vector(indices->indices)); - } - - impl_.setInputCloud(pcl_ptr(cloud_tf)); - impl_.setIndices(indices_ptr); - - // Final check if the data is empty - // (remember that indices are set to the size of the data -- if indices* = NULL) - if (!cloud->points.empty()) { - pcl::PointIndices pcl_inliers; - pcl::ModelCoefficients pcl_model; - pcl_conversions::moveToPCL(inliers, pcl_inliers); - pcl_conversions::moveToPCL(model, pcl_model); - impl_.segment(pcl_inliers, pcl_model); - pcl_conversions::moveFromPCL(pcl_inliers, inliers); - pcl_conversions::moveFromPCL(pcl_model, model); - } - - // Probably need to transform the model of the plane here - - // Check if we have enough inliers, clear inliers + model if not - if (static_cast(inliers.indices.size()) <= min_inliers_) { - inliers.indices.clear(); - model.values.clear(); - } - - // Publish - pub_indices_.publish(boost::make_shared(inliers)); - pub_model_.publish(boost::make_shared(model)); - NODELET_DEBUG( - "[%s::input_indices_callback] Published PointIndices with %zu values on topic %s, " - "and ModelCoefficients with %zu values on topic %s", - getName().c_str(), inliers.indices.size(), pnh_->resolveName("inliers").c_str(), - model.values.size(), pnh_->resolveName("model").c_str()); - - if (inliers.indices.empty()) { - NODELET_WARN("[%s::input_indices_callback] No inliers found!", getName().c_str()); - } -} - -////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::SACSegmentationFromNormals::onInit() -{ - // Call the super onInit () - PCLNodelet::onInit(); - - // Enable the dynamic reconfigure service - srv_ = boost::make_shared>(*pnh_); - dynamic_reconfigure::Server::CallbackType f = boost::bind( - &SACSegmentationFromNormals::config_callback, this, _1, _2); - srv_->setCallback(f); - - // Advertise the output topics - pub_indices_ = advertise(*pnh_, "inliers", max_queue_size_); - pub_model_ = advertise(*pnh_, "model", max_queue_size_); - - // ---[ Mandatory parameters - int model_type; - if (!pnh_->getParam("model_type", model_type)) { - NODELET_ERROR( - "[%s::onInit] Need a 'model_type' parameter to be set before continuing!", - getName().c_str()); - return; + sub_input_.reset(); } - double threshold; // unused - set via dynamic reconfigure in the callback - if (!pnh_->getParam("distance_threshold", threshold)) { - NODELET_ERROR( - "[%s::onInit] Need a 'distance_threshold' parameter to be set before continuing!", - getName().c_str()); - return; - } - - // ---[ Optional parameters - int method_type = 0; - pnh_->getParam("method_type", method_type); - - XmlRpc::XmlRpcValue axis_param; - pnh_->getParam("axis", axis_param); - Eigen::Vector3f axis = Eigen::Vector3f::Zero(); - - switch (axis_param.getType()) { - case XmlRpc::XmlRpcValue::TypeArray: - { - if (axis_param.size() != 3) { - NODELET_ERROR( - "[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than " - "required (3)!", - getName().c_str(), axis_param.size()); - return; - } - for (int i = 0; i < 3; ++i) { - if (axis_param[i].getType() != XmlRpc::XmlRpcValue::TypeDouble) { - NODELET_ERROR( - "[%s::onInit] Need floating point values for 'axis' parameter.", - getName().c_str()); - return; - } - double value = axis_param[i]; axis[i] = value; - } - break; - } - default: - { - break; - } - } - - // Initialize the random number generator - srand(time(0)); - - NODELET_DEBUG( - "[%s::onInit] Nodelet successfully created with the following parameters:\n" - " - model_type : %d\n" - " - method_type : %d\n" - " - model_threshold : %f\n" - " - axis : [%f, %f, %f]\n", - getName().c_str(), model_type, method_type, threshold, - axis[0], axis[1], axis[2]); - - // Set given parameters here - impl_.setModelType(model_type); - impl_.setMethodType(method_type); - impl_.setAxis(axis); - - onInitPostProcess(); } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SACSegmentationFromNormals::subscribe() +pcl_ros::SACSegmentation::createPublishers() { - // Subscribe to the input and normals using filters - sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_); - sub_normals_filter_.subscribe(*pnh_, "normals", max_queue_size_); - - // Subscribe to an axis direction along which the model search is to be constrained (the first - // 3 model coefficients will be checked) - sub_axis_ = pnh_->subscribe("axis", 1, &SACSegmentationFromNormals::axis_callback, this); - - if (approximate_sync_) { - sync_input_normals_indices_a_ = - boost::make_shared>>(max_queue_size_); - } else { - sync_input_normals_indices_e_ = - boost::make_shared>>(max_queue_size_); - } - - // If we're supposed to look for PointIndices (indices) if (use_indices_) { - // Subscribe to the input using a filter - sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); - - if (approximate_sync_) { - sync_input_normals_indices_a_->connectInput( - sub_input_filter_, sub_normals_filter_, - sub_indices_filter_); - } else { - sync_input_normals_indices_e_->connectInput( - sub_input_filter_, sub_normals_filter_, - sub_indices_filter_); + if (!sub_input_filter_.getSubscriber() || !sub_indices_filter_.getSubscriber()) { + subscribe(); } } else { - // Create a different callback for copying over the timestamp to fake indices - sub_input_filter_.registerCallback(bind(&SACSegmentationFromNormals::input_callback, this, _1)); - - if (approximate_sync_) { - sync_input_normals_indices_a_->connectInput(sub_input_filter_, sub_normals_filter_, nf_); - } else { - sync_input_normals_indices_e_->connectInput(sub_input_filter_, sub_normals_filter_, nf_); + if (!sub_input_) { + subscribe(); } } - - if (approximate_sync_) { - sync_input_normals_indices_a_->registerCallback( - bind( - &SACSegmentationFromNormals:: - input_normals_indices_callback, this, _1, _2, _3)); - } else { - sync_input_normals_indices_e_->registerCallback( - bind( - &SACSegmentationFromNormals:: - input_normals_indices_callback, this, _1, _2, _3)); - } + pub_output_ = create_publisher("output", max_queue_size_); + pub_model_ = create_publisher("model", max_queue_size_); } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SACSegmentationFromNormals::unsubscribe() +pcl_ros::SACSegmentation::use_frame_params() { - sub_input_filter_.unsubscribe(); - sub_normals_filter_.unsubscribe(); - - sub_axis_.shutdown(); - - if (use_indices_) { - sub_indices_filter_.unsubscribe(); + rcl_interfaces::msg::ParameterDescriptor input_frame_desc; + input_frame_desc.name = "input_frame"; + input_frame_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + input_frame_desc.description = + "The input TF frame the data should be transformed into before processing, " + "if input.header.frame_id is different."; + declare_parameter(input_frame_desc.name, rclcpp::ParameterValue(""), input_frame_desc); + + rcl_interfaces::msg::ParameterDescriptor output_frame_desc; + output_frame_desc.name = "output_frame"; + output_frame_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + output_frame_desc.description = + "The output TF frame the data should be transformed into after processing, " + "if input.header.frame_id is different."; + declare_parameter(output_frame_desc.name, rclcpp::ParameterValue(""), output_frame_desc); + + // Validate initial values using same callback + callback_handle_ = + add_on_set_parameters_callback( + std::bind( + &SACSegmentation::config_callback, this, + std::placeholders::_1)); + + std::vector param_names{input_frame_desc.name, output_frame_desc.name}; + auto result = config_callback(get_parameters(param_names)); + if (!result.successful) { + throw std::runtime_error(result.reason); } } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SACSegmentationFromNormals::axis_callback( - const pcl_msgs::ModelCoefficientsConstPtr & model) +pcl_ros::SACSegmentation::segment( + const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices, + PointIndices & output, ModelCoefficients & model) { - boost::mutex::scoped_lock lock(mutex_); - - if (model->values.size() < 3) { - NODELET_ERROR( - "[%s::axis_callback] Invalid axis direction / model coefficients with %zu values sent on %s!", - getName().c_str(), model->values.size(), pnh_->resolveName("axis").c_str()); - return; - } - NODELET_DEBUG( - "[%s::axis_callback] Received axis direction: %f %f %f", - getName().c_str(), model->values[0], model->values[1], model->values[2]); - - Eigen::Vector3f axis(model->values[0], model->values[1], model->values[2]); - impl_.setAxis(axis); + std::lock_guard lock(mutex_); + pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); + pcl::fromROSMsg(*input, *pcl_input); + impl_.setInputCloud(pcl_input); + impl_.setIndices(indices); + pcl::PointIndices::Ptr pcl_inliers(new pcl::PointIndices()); + pcl::ModelCoefficients::Ptr pcl_model(new pcl::ModelCoefficients); + impl_.segment(*pcl_inliers, *pcl_model); + pcl_conversions::moveFromPCL(*pcl_inliers, output); + pcl_conversions::moveFromPCL(*pcl_model, model); } ////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl_ros::SACSegmentationFromNormals::config_callback( - SACSegmentationFromNormalsConfig & config, - uint32_t level) +rcl_interfaces::msg::SetParametersResult +pcl_ros::SACSegmentation::config_callback(const std::vector & params) { - 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_); + + for (const rclcpp::Parameter & param : params) { + if (param.get_name() == "input_frame") { + if (tf_input_frame_ != param.as_string()) { + tf_input_frame_ = param.as_string(); + RCLCPP_DEBUG( + get_logger(), + "Setting the input frame to: %s.", + tf_input_frame_.c_str()); + } + } + if (param.get_name() == "output_frame") { + if (tf_output_frame_ != param.as_string()) { + tf_output_frame_ = param.as_string(); + RCLCPP_DEBUG( + get_logger(), + "Setting the output frame to: %s.", + tf_output_frame_.c_str()); + } + } + if (param.get_name() == "model_type") { + int model_type = impl_.getModelType(); + if (model_type != param.as_int()) { + RCLCPP_DEBUG( + get_logger(), + "Setting the model type to: %u.", + model_type); + impl_.setModelType(param.as_int()); + } + } + if (param.get_name() == "distance_threshold") { + double distance_threshold = impl_.getDistanceThreshold(); + if (distance_threshold != param.as_double()) { + distance_threshold = param.as_double(); + RCLCPP_DEBUG( + get_logger(), + "Setting the distance threshold to: %f.", + distance_threshold); + impl_.setDistanceThreshold(distance_threshold); + } + } + if (param.get_name() == "eps_angle") { + double eps_angle = impl_.getEpsAngle(); + if (eps_angle != param.as_double()) { + eps_angle = param.as_double(); + RCLCPP_DEBUG( + get_logger(), + "Setting the eps angle to: %f.", + eps_angle); + impl_.setEpsAngle(eps_angle); + } + } + if (param.get_name() == "method_type") { + int method_type = impl_.getMethodType(); + if (method_type != param.as_int()) { + RCLCPP_DEBUG( + get_logger(), + "Setting the method type to: %u.", + method_type); + impl_.setMethodType(param.as_int()); + } + } + if (param.get_name() == "axis") { + std::vector axis_param = param.as_double_array(); + Eigen::Vector3f axis(axis_param[0], axis_param[1], axis_param[2]); + if (impl_.getAxis() != axis) { + RCLCPP_DEBUG( + get_logger(), "Setting the axis to: %f %f %f.", + axis[0], axis[1], axis[2]); + impl_.setAxis(axis); + } + } + if (param.get_name() == "max_iterations") { + int max_iterations = impl_.getMaxIterations(); + if (max_iterations != param.as_int()) { + RCLCPP_DEBUG( + get_logger(), + "Setting the max iterations to: %u.", + max_iterations); + impl_.setMaxIterations(param.as_int()); + } + } + if (param.get_name() == "probability") { + double probability = impl_.getProbability(); + if (probability != param.as_double()) { + probability = param.as_double(); + RCLCPP_DEBUG( + get_logger(), + "Setting the probability to: %f.", + probability); + impl_.setProbability(probability); + } + } + if (param.get_name() == "optimize_coefficients") { + bool optimize_coefficients = param.as_bool(); + if (impl_.getOptimizeCoefficients() != optimize_coefficients) { + RCLCPP_DEBUG( + get_logger(), + "Setting optimize coefficients to: %s.", + (optimize_coefficients ? "true" : "false")); + impl_.setOptimizeCoefficients(optimize_coefficients); + } + } + if (param.get_name() == "radius_min") { + double radius_min, radius_max; + impl_.getRadiusLimits(radius_min, radius_max); + if (radius_min != param.as_double()) { + radius_min = param.as_double(); + RCLCPP_DEBUG( + get_logger(), + "Setting the minimum radius to: %f.", + radius_min); + impl_.setRadiusLimits(radius_min, radius_max); + } + } + if (param.get_name() == "radius_max") { + double radius_min, radius_max; + impl_.getRadiusLimits(radius_min, radius_max); + if (radius_max != param.as_double()) { + radius_max = param.as_double(); + RCLCPP_DEBUG( + get_logger(), + "Setting the maximum radius to: %f.", + radius_max); + impl_.setRadiusLimits(radius_min, radius_max); + } + } } - double radius_min, radius_max; - impl_.getRadiusLimits(radius_min, radius_max); - if (radius_min != config.radius_min) { - radius_min = config.radius_min; - NODELET_DEBUG( - "[%s::config_callback] Setting minimum allowable model radius to: %f.", - getName().c_str(), radius_min); - impl_.setRadiusLimits(radius_min, radius_max); - } - if (radius_max != config.radius_max) { - radius_max = config.radius_max; - NODELET_DEBUG( - "[%s::config_callback] Setting maximum allowable model radius to: %f.", - getName().c_str(), radius_max); - impl_.setRadiusLimits(radius_min, radius_max); - } + // Range constraints are enforced by rclcpp::Parameter. + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; } ////////////////////////////////////////////////////////////////////////////////////////////// void -pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback( - const PointCloudConstPtr & cloud, - const PointCloudNConstPtr & cloud_normals, - const PointIndicesConstPtr & indices -) +pcl_ros::SACSegmentation::input_indices_callback( + const PointCloud2::ConstSharedPtr & cloud, + const PointIndices::ConstSharedPtr & indices) { - boost::mutex::scoped_lock lock(mutex_); - - PointIndices inliers; - ModelCoefficients model; - // Enforce that the TF frame and the timestamp are copied - inliers.header = model.header = fromPCL(cloud->header); - - if (impl_.getModelType() < 0) { - NODELET_ERROR("[%s::input_normals_indices_callback] Model type not set!", getName().c_str()); - pub_indices_.publish(boost::make_shared(inliers)); - pub_model_.publish(boost::make_shared(model)); - return; - } - - if (!isValid(cloud)) { // || !isValid (cloud_normals, "normals")) - NODELET_ERROR("[%s::input_normals_indices_callback] Invalid input!", getName().c_str()); - pub_indices_.publish(boost::make_shared(inliers)); - pub_model_.publish(boost::make_shared(model)); + // If cloud is given, check if it's valid + if (!isValid(cloud)) { + RCLCPP_ERROR(this->get_logger(), "Invalid input!"); return; } // If indices are given, check if they are valid if (indices && !isValid(indices)) { - 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(), "Invalid indices!"); return; } /// DEBUG - if (indices && !indices->header.frame_id.empty()) { - NODELET_DEBUG( - "[%s::input_normals_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and " - "frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and " - "frame %s on topic %s received.\n" - " - PointIndices with %zu values, stamp %f, and " - "frame %s on topic %s received.", - getName().c_str(), - cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( - cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( - "input").c_str(), - cloud_normals->width * cloud_normals->height, pcl::getFieldsList( - *cloud_normals).c_str(), fromPCL( - cloud_normals->header).stamp.toSec(), - cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str(), - indices->indices.size(), indices->header.stamp.toSec(), - indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); + if (indices) { + RCLCPP_DEBUG( + this->get_logger(), "[input_indices_callback]\n" + " - PointCloud with %d data points (%s), stamp %d.%09d, and frame %s on topic %s received.\n" + " - PointIndices with %zu values, stamp %d.%09d, and frame %s on topic %s received.", + cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), + cloud->header.stamp.sec, cloud->header.stamp.nanosec, cloud->header.frame_id.c_str(), "input", + indices->indices.size(), indices->header.stamp.sec, indices->header.stamp.nanosec, + indices->header.frame_id.c_str(), "indices"); } else { - NODELET_DEBUG( - "[%s::input_normals_indices_callback]\n" - " - PointCloud with %d data points (%s), stamp %f, and " - "frame %s on topic %s received.\n" - " - PointCloud with %d data points (%s), stamp %f, and " - "frame %s on topic %s received.", - getName().c_str(), - cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( - cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( - "input").c_str(), - cloud_normals->width * cloud_normals->height, pcl::getFieldsList( - *cloud_normals).c_str(), fromPCL( - cloud_normals->header).stamp.toSec(), - cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str()); + RCLCPP_DEBUG( + this->get_logger(), "PointCloud with %d data points and frame %s on topic %s received.", + cloud->width * cloud->height, cloud->header.frame_id.c_str(), "input"); } /// - - // Extra checks for safety - int cloud_nr_points = cloud->width * cloud->height; - int cloud_normals_nr_points = cloud_normals->width * cloud_normals->height; - if (cloud_nr_points != cloud_normals_nr_points) { - NODELET_ERROR( - "[%s::input_normals_indices_callback] Number of points in the input dataset (%d) differs " - "from the number of points in the normals (%d)!", - getName().c_str(), cloud_nr_points, cloud_normals_nr_points); - pub_indices_.publish(boost::make_shared(inliers)); - pub_model_.publish(boost::make_shared(model)); - return; - } - - impl_.setInputCloud(pcl_ptr(cloud)); - impl_.setInputNormals(pcl_ptr(cloud_normals)); - - IndicesPtr indices_ptr; - if (indices && !indices->header.frame_id.empty()) { - indices_ptr.reset(new std::vector(indices->indices)); - } - - impl_.setIndices(indices_ptr); - - // Final check if the data is empty - // (remember that indices are set to the size of the data -- if indices* = NULL) - if (!cloud->points.empty()) { - pcl::PointIndices pcl_inliers; - pcl::ModelCoefficients pcl_model; - pcl_conversions::moveToPCL(inliers, pcl_inliers); - pcl_conversions::moveToPCL(model, pcl_model); - impl_.segment(pcl_inliers, pcl_model); - pcl_conversions::moveFromPCL(pcl_inliers, inliers); - pcl_conversions::moveFromPCL(pcl_model, model); + // Check whether the user has given a different input TF frame + tf_input_orig_frame_ = cloud->header.frame_id; + PointCloud2::ConstSharedPtr cloud_tf; + if (!tf_input_frame_.empty() && cloud->header.frame_id != tf_input_frame_) { + RCLCPP_DEBUG( + this->get_logger(), "Transforming input dataset from %s to %s.", + cloud->header.frame_id.c_str(), tf_input_frame_.c_str()); + // Save the original frame ID + // Convert the cloud into the different frame + PointCloud2 cloud_transformed; + if (!pcl_ros::transformPointCloud(tf_input_frame_, *cloud, cloud_transformed, tf_buffer_)) { + RCLCPP_ERROR( + this->get_logger(), "Error converting input dataset from %s to %s.", + cloud->header.frame_id.c_str(), tf_input_frame_.c_str()); + return; + } + cloud_tf = std::make_shared(cloud_transformed); + } else { + cloud_tf = cloud; } - // Check if we have enough inliers, clear inliers + model if not - if (static_cast(inliers.indices.size()) <= min_inliers_) { - inliers.indices.clear(); - model.values.clear(); + // Need setInputCloud () here because we have to extract x/y/z + IndicesPtr vindices; + if (indices) { + vindices.reset(new std::vector(indices->indices)); } - // Publish - pub_indices_.publish(boost::make_shared(inliers)); - pub_model_.publish(boost::make_shared(model)); - NODELET_DEBUG( - "[%s::input_normals_callback] Published PointIndices with %zu values on topic %s, and " - "ModelCoefficients with %zu values on topic %s", - getName().c_str(), inliers.indices.size(), pnh_->resolveName("inliers").c_str(), - model.values.size(), pnh_->resolveName("model").c_str()); - if (inliers.indices.empty()) { - NODELET_WARN("[%s::input_indices_callback] No inliers found!", getName().c_str()); - } + computePublish(cloud_tf, vindices); } -typedef pcl_ros::SACSegmentation SACSegmentation; -typedef pcl_ros::SACSegmentationFromNormals SACSegmentationFromNormals; -PLUGINLIB_EXPORT_CLASS(SACSegmentation, nodelet::Nodelet) -PLUGINLIB_EXPORT_CLASS(SACSegmentationFromNormals, nodelet::Nodelet) +////////////////////////////////////////////////////////////////////////////////////////////// +// void +// pcl_ros::SACSegmentationFromNormals::onInit() +// { +// // Call the super onInit () +// PCLNodelet::onInit(); + +// // Enable the dynamic reconfigure service +// srv_ = boost::make_shared>(*pnh_); +// dynamic_reconfigure::Server::CallbackType f = boost::bind( +// &SACSegmentationFromNormals::config_callback, this, _1, _2); +// srv_->setCallback(f); + +// // Advertise the output topics +// pub_indices_ = advertise(*pnh_, "inliers", max_queue_size_); +// pub_model_ = advertise(*pnh_, "model", max_queue_size_); + +// // ---[ Mandatory parameters +// int model_type; +// if (!pnh_->getParam("model_type", model_type)) { +// NODELET_ERROR( +// "[%s::onInit] Need a 'model_type' parameter to be set before continuing!", +// getName().c_str()); +// return; +// } +// double threshold; // unused - set via dynamic reconfigure in the callback +// if (!pnh_->getParam("distance_threshold", threshold)) { +// NODELET_ERROR( +// "[%s::onInit] Need a 'distance_threshold' parameter to be set before continuing!", +// getName().c_str()); +// return; +// } + +// // ---[ Optional parameters +// int method_type = 0; +// pnh_->getParam("method_type", method_type); + +// XmlRpc::XmlRpcValue axis_param; +// pnh_->getParam("axis", axis_param); +// Eigen::Vector3f axis = Eigen::Vector3f::Zero(); + +// switch (axis_param.getType()) { +// case XmlRpc::XmlRpcValue::TypeArray: +// { +// if (axis_param.size() != 3) { +// NODELET_ERROR( +// "[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than " +// "required (3)!", +// getName().c_str(), axis_param.size()); +// return; +// } +// for (int i = 0; i < 3; ++i) { +// if (axis_param[i].getType() != XmlRpc::XmlRpcValue::TypeDouble) { +// NODELET_ERROR( +// "[%s::onInit] Need floating point values for 'axis' parameter.", +// getName().c_str()); +// return; +// } +// double value = axis_param[i]; axis[i] = value; +// } +// break; +// } +// default: +// { +// break; +// } +// } + +// // Initialize the random number generator +// srand(time(0)); + +// NODELET_DEBUG( +// "[%s::onInit] Nodelet successfully created with the following parameters:\n" +// " - model_type : %d\n" +// " - method_type : %d\n" +// " - model_threshold : %f\n" +// " - axis : [%f, %f, %f]\n", +// getName().c_str(), model_type, method_type, threshold, +// axis[0], axis[1], axis[2]); + +// // Set given parameters here +// impl_.setModelType(model_type); +// impl_.setMethodType(method_type); +// impl_.setAxis(axis); + +// onInitPostProcess(); +// } + +// ////////////////////////////////////////////////////////////////////////////////////////////// +// void +// pcl_ros::SACSegmentationFromNormals::subscribe() +// { +// // Subscribe to the input and normals using filters +// sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_); +// sub_normals_filter_.subscribe(*pnh_, "normals", max_queue_size_); + +// // Subscribe to an axis direction along which the model search is to be constrained (the first +// // 3 model coefficients will be checked) +// sub_axis_ = pnh_->subscribe("axis", 1, &SACSegmentationFromNormals::axis_callback, this); + +// if (approximate_sync_) { +// sync_input_normals_indices_a_ = +// boost::make_shared>>(max_queue_size_); +// } else { +// sync_input_normals_indices_e_ = +// boost::make_shared>>(max_queue_size_); +// } + +// // If we're supposed to look for PointIndices (indices) +// if (use_indices_) { +// // Subscribe to the input using a filter +// sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); + +// if (approximate_sync_) { +// sync_input_normals_indices_a_->connectInput( +// sub_input_filter_, sub_normals_filter_, +// sub_indices_filter_); +// } else { +// sync_input_normals_indices_e_->connectInput( +// sub_input_filter_, sub_normals_filter_, +// sub_indices_filter_); +// } +// } else { +// // Create a different callback for copying over the timestamp to fake indices +// sub_input_filter_.registerCallback(bind(&SACSegmentationFromNormals::input_callback, this, _1)); + +// if (approximate_sync_) { +// sync_input_normals_indices_a_->connectInput(sub_input_filter_, sub_normals_filter_, nf_); +// } else { +// sync_input_normals_indices_e_->connectInput(sub_input_filter_, sub_normals_filter_, nf_); +// } +// } + +// if (approximate_sync_) { +// sync_input_normals_indices_a_->registerCallback( +// bind( +// &SACSegmentationFromNormals:: +// input_normals_indices_callback, this, _1, _2, _3)); +// } else { +// sync_input_normals_indices_e_->registerCallback( +// bind( +// &SACSegmentationFromNormals:: +// input_normals_indices_callback, this, _1, _2, _3)); +// } +// } + +// ////////////////////////////////////////////////////////////////////////////////////////////// +// void +// pcl_ros::SACSegmentationFromNormals::unsubscribe() +// { +// sub_input_filter_.unsubscribe(); +// sub_normals_filter_.unsubscribe(); + +// sub_axis_.shutdown(); + +// if (use_indices_) { +// sub_indices_filter_.unsubscribe(); +// } +// } + +// ////////////////////////////////////////////////////////////////////////////////////////////// +// void +// pcl_ros::SACSegmentationFromNormals::axis_callback( +// const pcl_msgs::ModelCoefficientsConstPtr & model) +// { +// boost::mutex::scoped_lock lock(mutex_); + +// if (model->values.size() < 3) { +// NODELET_ERROR( +// "[%s::axis_callback] Invalid axis direction / model coefficients with %zu values sent on %s!", +// getName().c_str(), model->values.size(), pnh_->resolveName("axis").c_str()); +// return; +// } +// NODELET_DEBUG( +// "[%s::axis_callback] Received axis direction: %f %f %f", +// getName().c_str(), model->values[0], model->values[1], model->values[2]); + +// Eigen::Vector3f axis(model->values[0], model->values[1], model->values[2]); +// impl_.setAxis(axis); +// } + +// ////////////////////////////////////////////////////////////////////////////////////////////// +// void +// pcl_ros::SACSegmentationFromNormals::config_callback( +// SACSegmentationFromNormalsConfig & config, +// uint32_t level) +// { +// boost::mutex::scoped_lock lock(mutex_); + +// if (impl_.getDistanceThreshold() != config.distance_threshold) { +// impl_.setDistanceThreshold(config.distance_threshold); +// NODELET_DEBUG( +// "[%s::config_callback] Setting distance to model threshold to: %f.", +// getName().c_str(), config.distance_threshold); +// } +// // The maximum allowed difference between the model normal and the given axis _in radians_ +// if (impl_.getEpsAngle() != config.eps_angle) { +// impl_.setEpsAngle(config.eps_angle); +// NODELET_DEBUG( +// "[%s::config_callback] Setting new epsilon angle to model threshold to: %f (%f degrees).", +// getName().c_str(), config.eps_angle, config.eps_angle * 180.0 / M_PI); +// } + +// if (impl_.getMaxIterations() != config.max_iterations) { +// impl_.setMaxIterations(config.max_iterations); +// NODELET_DEBUG( +// "[%s::config_callback] Setting new maximum number of iterations to: %d.", +// getName().c_str(), config.max_iterations); +// } + +// // Number of inliers +// if (min_inliers_ != config.min_inliers) { +// min_inliers_ = config.min_inliers; +// NODELET_DEBUG( +// "[%s::config_callback] Setting new minimum number of inliers to: %d.", +// getName().c_str(), min_inliers_); +// } + + +// if (impl_.getProbability() != config.probability) { +// impl_.setProbability(config.probability); +// NODELET_DEBUG( +// "[%s::config_callback] Setting new probability to: %f.", +// getName().c_str(), config.probability); +// } + +// if (impl_.getOptimizeCoefficients() != config.optimize_coefficients) { +// impl_.setOptimizeCoefficients(config.optimize_coefficients); +// NODELET_DEBUG( +// "[%s::config_callback] Setting coefficient optimization to: %s.", +// getName().c_str(), (config.optimize_coefficients) ? "true" : "false"); +// } + +// if (impl_.getNormalDistanceWeight() != config.normal_distance_weight) { +// impl_.setNormalDistanceWeight(config.normal_distance_weight); +// NODELET_DEBUG( +// "[%s::config_callback] Setting new distance weight to: %f.", +// getName().c_str(), config.normal_distance_weight); +// } + +// double radius_min, radius_max; +// impl_.getRadiusLimits(radius_min, radius_max); +// if (radius_min != config.radius_min) { +// radius_min = config.radius_min; +// NODELET_DEBUG( +// "[%s::config_callback] Setting minimum allowable model radius to: %f.", +// getName().c_str(), radius_min); +// impl_.setRadiusLimits(radius_min, radius_max); +// } +// if (radius_max != config.radius_max) { +// radius_max = config.radius_max; +// NODELET_DEBUG( +// "[%s::config_callback] Setting maximum allowable model radius to: %f.", +// getName().c_str(), radius_max); +// impl_.setRadiusLimits(radius_min, radius_max); +// } +// } + +// ////////////////////////////////////////////////////////////////////////////////////////////// +// void +// pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback( +// const PointCloudConstPtr & cloud, +// const PointCloudNConstPtr & cloud_normals, +// const PointIndicesConstPtr & indices +// ) +// { +// boost::mutex::scoped_lock lock(mutex_); + +// PointIndices inliers; +// ModelCoefficients model; +// // Enforce that the TF frame and the timestamp are copied +// inliers.header = model.header = fromPCL(cloud->header); + +// if (impl_.getModelType() < 0) { +// NODELET_ERROR("[%s::input_normals_indices_callback] Model type not set!", getName().c_str()); +// pub_indices_.publish(boost::make_shared(inliers)); +// pub_model_.publish(boost::make_shared(model)); +// return; +// } + +// if (!isValid(cloud)) { // || !isValid (cloud_normals, "normals")) +// NODELET_ERROR("[%s::input_normals_indices_callback] Invalid input!", getName().c_str()); +// pub_indices_.publish(boost::make_shared(inliers)); +// pub_model_.publish(boost::make_shared(model)); +// return; +// } +// // If indices are given, check if they are valid +// if (indices && !isValid(indices)) { +// NODELET_ERROR("[%s::input_normals_indices_callback] Invalid indices!", getName().c_str()); +// pub_indices_.publish(boost::make_shared(inliers)); +// pub_model_.publish(boost::make_shared(model)); +// return; +// } + +// /// DEBUG +// if (indices && !indices->header.frame_id.empty()) { +// NODELET_DEBUG( +// "[%s::input_normals_indices_callback]\n" +// " - PointCloud with %d data points (%s), stamp %f, and " +// "frame %s on topic %s received.\n" +// " - PointCloud with %d data points (%s), stamp %f, and " +// "frame %s on topic %s received.\n" +// " - PointIndices with %zu values, stamp %f, and " +// "frame %s on topic %s received.", +// getName().c_str(), +// cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( +// cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( +// "input").c_str(), +// cloud_normals->width * cloud_normals->height, pcl::getFieldsList( +// *cloud_normals).c_str(), fromPCL( +// cloud_normals->header).stamp.toSec(), +// cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str(), +// indices->indices.size(), indices->header.stamp.toSec(), +// indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str()); +// } else { +// NODELET_DEBUG( +// "[%s::input_normals_indices_callback]\n" +// " - PointCloud with %d data points (%s), stamp %f, and " +// "frame %s on topic %s received.\n" +// " - PointCloud with %d data points (%s), stamp %f, and " +// "frame %s on topic %s received.", +// getName().c_str(), +// cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL( +// cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( +// "input").c_str(), +// cloud_normals->width * cloud_normals->height, pcl::getFieldsList( +// *cloud_normals).c_str(), fromPCL( +// cloud_normals->header).stamp.toSec(), +// cloud_normals->header.frame_id.c_str(), pnh_->resolveName("normals").c_str()); +// } +// /// + + +// // Extra checks for safety +// int cloud_nr_points = cloud->width * cloud->height; +// int cloud_normals_nr_points = cloud_normals->width * cloud_normals->height; +// if (cloud_nr_points != cloud_normals_nr_points) { +// NODELET_ERROR( +// "[%s::input_normals_indices_callback] Number of points in the input dataset (%d) differs " +// "from the number of points in the normals (%d)!", +// getName().c_str(), cloud_nr_points, cloud_normals_nr_points); +// pub_indices_.publish(boost::make_shared(inliers)); +// pub_model_.publish(boost::make_shared(model)); +// return; +// } + +// impl_.setInputCloud(pcl_ptr(cloud)); +// impl_.setInputNormals(pcl_ptr(cloud_normals)); + +// IndicesPtr indices_ptr; +// if (indices && !indices->header.frame_id.empty()) { +// indices_ptr.reset(new std::vector(indices->indices)); +// } + +// impl_.setIndices(indices_ptr); + +// // Final check if the data is empty +// // (remember that indices are set to the size of the data -- if indices* = NULL) +// if (!cloud->points.empty()) { +// pcl::PointIndices pcl_inliers; +// pcl::ModelCoefficients pcl_model; +// pcl_conversions::moveToPCL(inliers, pcl_inliers); +// pcl_conversions::moveToPCL(model, pcl_model); +// impl_.segment(pcl_inliers, pcl_model); +// pcl_conversions::moveFromPCL(pcl_inliers, inliers); +// pcl_conversions::moveFromPCL(pcl_model, model); +// } + +// // Check if we have enough inliers, clear inliers + model if not +// if (static_cast(inliers.indices.size()) <= min_inliers_) { +// inliers.indices.clear(); +// model.values.clear(); +// } + +// // Publish +// pub_indices_.publish(boost::make_shared(inliers)); +// pub_model_.publish(boost::make_shared(model)); +// NODELET_DEBUG( +// "[%s::input_normals_callback] Published PointIndices with %zu values on topic %s, and " +// "ModelCoefficients with %zu values on topic %s", +// getName().c_str(), inliers.indices.size(), pnh_->resolveName("inliers").c_str(), +// model.values.size(), pnh_->resolveName("model").c_str()); +// if (inliers.indices.empty()) { +// NODELET_WARN("[%s::input_indices_callback] No inliers found!", getName().c_str()); +// } +// } + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros::SACSegmentation)