diff --git a/pcl_ros/include/pcl_ros/pcl_nodelet.hpp b/pcl_ros/include/pcl_ros/pcl_node.hpp similarity index 53% rename from pcl_ros/include/pcl_ros/pcl_nodelet.hpp rename to pcl_ros/include/pcl_ros/pcl_node.hpp index 4806338f..d80cb6f4 100644 --- a/pcl_ros/include/pcl_ros/pcl_nodelet.hpp +++ b/pcl_ros/include/pcl_ros/pcl_node.hpp @@ -31,7 +31,7 @@ * 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 $ + * $Id: pcl_node.h 33238 2010-03-11 00:46:58Z rusu $ * */ @@ -41,30 +41,35 @@ **/ -#ifndef PCL_ROS__PCL_NODELET_HPP_ -#define PCL_ROS__PCL_NODELET_HPP_ +#ifndef PCL_ROS__PCL_NODE_HPP_ +#define PCL_ROS__PCL_NODE_HPP_ -#include +#include // PCL includes -#include -#include +#include +#include #include #include #include // ROS Nodelet includes -#include +// #include // TODO(daisukes): lazy subscription #include #include #include #include // Include TF -#include +#include +#include // STL +#include #include +#include +// ROS2 includes +#include -#include "pcl_ros/point_cloud.hpp" +// #include "pcl_ros/point_cloud.hpp" using pcl_conversions::fromPCL; @@ -73,33 +78,85 @@ 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 PCLNode represents the base PCL Node class. All PCL node should inherit from + * this class. */ +template> +class PCLNode : public rclcpp::Node { public: - typedef sensor_msgs::PointCloud2 PointCloud2; + typedef sensor_msgs::msg::PointCloud2 PointCloud2; typedef pcl::PointCloud PointCloud; - typedef boost::shared_ptr PointCloudPtr; - typedef boost::shared_ptr PointCloudConstPtr; + typedef PointCloud::Ptr PointCloudPtr; + typedef PointCloud::ConstPtr PointCloudConstPtr; - typedef pcl_msgs::PointIndices PointIndices; - typedef PointIndices::Ptr PointIndicesPtr; - typedef PointIndices::ConstPtr PointIndicesConstPtr; + typedef pcl_msgs::msg::PointIndices PointIndices; + typedef PointIndices::SharedPtr PointIndicesPtr; + typedef PointIndices::ConstSharedPtr PointIndicesConstPtr; - typedef pcl_msgs::ModelCoefficients ModelCoefficients; - typedef ModelCoefficients::Ptr ModelCoefficientsPtr; - typedef ModelCoefficients::ConstPtr ModelCoefficientsConstPtr; + typedef pcl_msgs::msg::ModelCoefficients ModelCoefficients; + typedef ModelCoefficients::SharedPtr ModelCoefficientsPtr; + typedef ModelCoefficients::ConstSharedPtr ModelCoefficientsConstPtr; typedef pcl::IndicesPtr IndicesPtr; typedef pcl::IndicesConstPtr IndicesConstPtr; /** \brief Empty constructor. */ - PCLNodelet() - : use_indices_(false), latched_indices_(false), - max_queue_size_(3), approximate_sync_(false) {} + PCLNode(std::string node_name, const rclcpp::NodeOptions & options) + : rclcpp::Node(node_name, options), + use_indices_(false), transient_local_indices_(false), + max_queue_size_(3), approximate_sync_(false), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_) + { + { + rcl_interfaces::msg::ParameterDescriptor desc; + desc.name = "max_queue_size"; + desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + desc.description = "QoS History depth"; + desc.read_only = true; + max_queue_size_ = declare_parameter(desc.name, max_queue_size_, desc); + } + + { + rcl_interfaces::msg::ParameterDescriptor desc; + desc.name = "use_indices"; + desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + desc.description = "Only process a subset of the point cloud from an indices topic"; + desc.read_only = true; + use_indices_ = declare_parameter(desc.name, use_indices_, desc); + } + + { + rcl_interfaces::msg::ParameterDescriptor desc; + desc.name = "transient_local_indices"; + desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + desc.description = "Does indices topic use transient local documentation"; + desc.read_only = true; + transient_local_indices_ = declare_parameter(desc.name, transient_local_indices_, desc); + } + + { + rcl_interfaces::msg::ParameterDescriptor desc; + desc.name = "approximate_sync"; + desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + desc.description = + "Match indices and point cloud messages if time stamps are approximatly the same."; + desc.read_only = true; + approximate_sync_ = declare_parameter(desc.name, approximate_sync_, desc); + } + + RCLCPP_DEBUG( + this->get_logger(), "PCL Node successfully created with the following parameters:\n" + " - approximate_sync : %s\n" + " - use_indices : %s\n" + " - transient_local_indices_ : %s\n" + " - max_queue_size : %d", + (approximate_sync_) ? "true" : "false", + (use_indices_) ? "true" : "false", + (transient_local_indices_) ? "true" : "false", + max_queue_size_); + } protected: /** \brief Set to true if point indices are used. @@ -110,26 +167,28 @@ protected: * 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. + * specified jitter. See also @ref transient_local_indices_ and approximate_sync. **/ bool use_indices_; - /** \brief Set to true if the indices topic is latched. + /** \brief Set to true if the indices topic has transient_local durability. * * If use_indices_ is true, the ~input and ~indices topics generally must * be synchronised in time. By setting this flag to true, the most recent * value from ~indices can be used instead of requiring a synchronised * message. **/ - bool latched_indices_; + bool transient_local_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. Allow each individual class that inherits from this + * to declare the Publisher with their type. + */ + std::shared_ptr pub_output_; /** \brief The maximum queue size (default: 3). */ int max_queue_size_; @@ -140,23 +199,24 @@ protected: bool approximate_sync_; /** \brief TF listener object. */ - tf::TransformListener tf_listener_; - + tf2_ros::TransformListener tf_listener_; + 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") */ inline bool - isValid(const PointCloud2::ConstPtr & cloud, const std::string & topic_name = "input") + isValid(const PointCloud2::ConstSharedPtr & 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(), + RCLCPP_WARN( + this->get_logger(), + "Invalid PointCloud (data = %zu, width = %d, height = %d, step = %d) " + "with stamp %d.%09d, and frame %s on topic %s received!", cloud->data.size(), cloud->width, cloud->height, cloud->point_step, - cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( - topic_name).c_str()); + cloud->header.stamp.sec, cloud->header.stamp.nanosec, + cloud->header.frame_id.c_str(), topic_name.c_str()); return false; } @@ -171,12 +231,13 @@ protected: 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()); + RCLCPP_WARN( + this->get_logger(), + "Invalid PointCloud (points = %zu, width = %d, height = %d) " + "with stamp %d.%09d, and frame %s on topic %s received!", + cloud->points.size(), cloud->width, cloud->height, + fromPCL(cloud->header).stamp.sec, fromPCL(cloud->header).stamp.nanosec, + cloud->header.frame_id.c_str(), topic_name.c_str()); return false; } @@ -192,11 +253,13 @@ protected: { /*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); + RCLCPP_WARN( + this->get_logger(), "Empty indices (values = %zu) with stamp %d.%09d, " + "and frame %s on topic %s received!", + indices->indices.size(), indices->header.stamp.sec, indices->header.stamp.nanosec, + indices->header.frame_id.c_str(), topic_name.c_str()); + //return (true); // looks like it should be false + return false; }*/ return true; } @@ -210,10 +273,11 @@ protected: { /*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 ()); + RCLCPP_WARN( + this->get_logger(), "Empty model (values = %zu) with stamp %d.%09d, " + "and frame %s on topic %s received!", + model->values.size(), model->header.stamp.sec, model->header.stamp.nanosec, + model->header.frame_id.c_str(), topic_name.c_str()); return (false); }*/ return true; @@ -225,36 +289,9 @@ protected: virtual void subscribe() {} virtual void unsubscribe() {} - /** \brief Nodelet initialization routine. Reads in global parameters used by all nodelets. */ - 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_); - } - public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace pcl_ros -#endif // PCL_ROS__PCL_NODELET_HPP_ +#endif // PCL_ROS__PCL_NODE_HPP_