Skip to content

Commit

Permalink
Port the nodelet base class to a ROS2 component node
Browse files Browse the repository at this point in the history 
Most of work comes from PR ros-perception#255/ros-perception#229 by sloretz/klintan
  • Loading branch information
@theseankelly
theseankelly committed Aug 31, 2020
1 parent d7a79b9 commit f5d3212
Show file tree
Hide file tree
Showing 3 changed files with 138 additions and 111 deletions.
2 changes: 2 additions & 0 deletions pcl_ros/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@ find_package(geometry_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(message_filters REQUIRED)

set(dependencies
pcl_conversions
Expand All @@ -30,6 +31,7 @@ set(dependencies
tf2
tf2_geometry_msgs
tf2_ros
message_filters
EIGEN3
PCL
)
Expand Down
246 changes: 135 additions & 111 deletions pcl_ros/include/pcl_ros/pcl_nodelet.hpp → pcl_ros/include/pcl_ros/pcl_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 $
*
*/

Expand All @@ -41,65 +41,103 @@
**/

#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 <sensor_msgs/PointCloud2.h>
// PCL includes
#include <pcl_msgs/PointIndices.h>
#include <pcl_msgs/ModelCoefficients.h>
#include <pcl/pcl_base.h>
#include <rclcpp/rclcpp.hpp>
#include <pcl/point_types.h>
#include <pcl_msgs/msg/point_indices.hpp>
#include <pcl_msgs/msg/model_coefficients.hpp>
#include <pcl_conversions/pcl_conversions.h>
// ROS Nodelet includes
#include <nodelet_topic_tools/nodelet_lazy.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>

// Include TF
#include <tf/transform_listener.h>

// STL
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <memory>
#include <string>

#include "pcl_ros/point_cloud.hpp"
#include <vector>

using pcl_conversions::fromPCL;

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.
*/
class PCLNode : public rclcpp::Node
{
public:
typedef sensor_msgs::PointCloud2 PointCloud2;
typedef sensor_msgs::msg::PointCloud2 PointCloud2;

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef boost::shared_ptr<PointCloud> PointCloudPtr;
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
typedef PointCloud::Ptr PointCloudPtr;
typedef PointCloud::ConstPtr PointCloudConstPtr;

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;

/** \brief Constructor. */
PCLNode(std::string node_name, const rclcpp::NodeOptions & options)
: rclcpp::Node(node_name, options),
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);
}

typedef pcl_msgs::PointIndices PointIndices;
typedef PointIndices::Ptr PointIndicesPtr;
typedef PointIndices::ConstPtr PointIndicesConstPtr;
{
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);
}

typedef pcl_msgs::ModelCoefficients ModelCoefficients;
typedef ModelCoefficients::Ptr ModelCoefficientsPtr;
typedef ModelCoefficients::ConstPtr ModelCoefficientsConstPtr;
{
rcl_interfaces::msg::ParameterDescriptor desc;
desc.name = "latched_indices";
desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
desc.description = "Does indices topic use transient local documentation";
desc.read_only = true;
latched_indices_ = declare_parameter(desc.name, latched_indices_, desc);
}

typedef pcl::IndicesPtr IndicesPtr;
typedef pcl::IndicesConstPtr IndicesConstPtr;
{
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);
}

/** \brief Empty constructor. */
PCLNodelet()
: use_indices_(false), latched_indices_(false),
max_queue_size_(3), approximate_sync_(false) {}
RCLCPP_DEBUG(
this->get_logger(), "PCL Node successfully created with the following parameters:\n"
" - approximate_sync : %s\n"
" - use_indices : %s\n"
" - latched_indices : %s\n"
" - max_queue_size : %d",
(approximate_sync_) ? "true" : "false",
(use_indices_) ? "true" : "false",
(latched_indices_) ? "true" : "false",
max_queue_size_);
}

protected:
/** \brief Set to true if point indices are used.
Expand All @@ -112,71 +150,71 @@ class PCLNodelet : public nodelet_topic_tools::NodeletLazy
* ~indices topics must be synchronised in time, either exact or within a
* specified jitter. See also @ref latched_indices_ and approximate_sync.
**/
bool use_indices_;
/** \brief Set to true if the indices topic is latched.
bool use_indices_ = false;
/** \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 latched_indices_ = false;

/** \brief The message filter subscriber for PointCloud2. */
message_filters::Subscriber<PointCloud> sub_input_filter_;
message_filters::Subscriber<PointCloud2> sub_input_filter_;

/** \brief The message filter subscriber for PointIndices. */
message_filters::Subscriber<PointIndices> sub_indices_filter_;

/** \brief The output PointCloud publisher. */
ros::Publisher pub_output_;
rclcpp::Publisher<PointCloud2>::SharedPtr pub_output_;

/** \brief The maximum queue size (default: 3). */
int max_queue_size_;
int max_queue_size_ = 3;

/** \brief True if we use an approximate time synchronizer
* versus an exact one (false by default).
**/
bool approximate_sync_;
/** \brief True if we use an approximate time synchronizer versus an
* exact one (false by default).
*/
bool approximate_sync_ = false;

/** \brief TF listener object. */
tf::TransformListener tf_listener_;
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;

/** \brief Test whether a given PointCloud message is "valid" (i.e., has points, and width and height are non-zero).
/** \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) "
RCLCPP_WARN(
this->get_logger(), "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());
cloud->data.size(), cloud->width, cloud->height, cloud->point_step, cloud->header.stamp.sec,
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).
/** \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 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 %f, and frame %s on topic %s received!",
cloud->points.size(), cloud->width, cloud->height, fromPCL(
cloud->header).stamp.sec, cloud->header.frame_id.c_str(), topic_name.c_str());

return false;
}
Expand All @@ -190,14 +228,14 @@ class PCLNodelet : public nodelet_topic_tools::NodeletLazy
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->indices.empty()) {
RCLCPP_WARN(
this->get_logger(), "Empty indices (values = %zu) with stamp %f, and frame %s on "
"topic %s received!",
indices->indices.size(), indices->header.stamp.sec,
indices->header.frame_id.c_str(), topic_name.c_str());
return true;
}
return true;
}

Expand All @@ -208,53 +246,39 @@ class PCLNodelet : public nodelet_topic_tools::NodeletLazy
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->values.empty()) {
RCLCPP_WARN(
this->get_logger(), "Empty model (values = %zu) with stamp %f, and frame %s on "
"topic %s received!",
model->values.size(), model->header.stamp.sec,
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.
**/
virtual void subscribe() {}
virtual void unsubscribe() {}

/** \brief Nodelet initialization routine. Reads in global parameters used by all nodelets. */
virtual void
onInit()
/* \brief Return QoS settings for indices topic */
rclcpp::QoS
indicesQoS() const
{
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_);
rclcpp::QoS qos(max_queue_size_);
if (latched_indices_) {
qos.transient_local();
}
return qos;
}

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_);
/* \brief Return QoS settings for point cloud topic */
rclcpp::QoS
cloudQoS() const
{
rclcpp::QoS qos(max_queue_size_);
return qos;
}

public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl_ros

#endif // PCL_ROS__PCL_NODELET_HPP_
#endif // PCL_ROS__PCL_NODE_HPP_
1 change: 1 addition & 0 deletions pcl_ros/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@
<depend>tf2</depend>
<depend>tf2_geometry_msgs</depend>
<depend>tf2_ros</depend>
<depend>message_filters</depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
Expand Down

0 comments on commit f5d3212

Please sign in to comment.