apoorva/greenhouse
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

migrate pcl_nodelet.hpp to pcl_node.hpp (#385)

Browse Source 
* migrate pcl_nodelet.hpp to pcl_node.hpp
TODOs
- lazy subscription
- cpplint, uncrustify
- type adaptation

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

* rename latched_indices to transient_local_indices

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

* - remove some TODOs
- change pub_output_ type

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

* remove TODOs

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

* use template instead of PublisherBase

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>
This commit is contained in:
Daisuke Sato 2022-12-09 14:12:01 -05:00 committed by GitHub
parent ca69652eb8
commit 5c5382eb5c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 120 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

201
pcl_ros/include/pcl_ros/pcl_nodelet.hpp → pcl_ros/include/pcl_ros/pcl_node.hpp
View File

@ -31,7 +31,7 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE. * 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_ #ifndef PCL_ROS__PCL_NODE_HPP_
#define PCL_ROS__PCL_NODELET_HPP_ #define PCL_ROS__PCL_NODE_HPP_
#include <sensor_msgs/PointCloud2.h> #include <sensor_msgs/msg/point_cloud2.hpp>
// PCL includes // PCL includes
#include <pcl_msgs/PointIndices.h> #include <pcl_msgs/msg/point_indices.hpp>
#include <pcl_msgs/ModelCoefficients.h> #include <pcl_msgs/msg/model_coefficients.hpp>
#include <pcl/pcl_base.h> #include <pcl/pcl_base.h>
#include <pcl/point_types.h> #include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h> #include <pcl_conversions/pcl_conversions.h>
// ROS Nodelet includes // ROS Nodelet includes
#include <nodelet_topic_tools/nodelet_lazy.h> // #include <nodelet_topic_tools/nodelet_lazy.h> // TODO(daisukes): lazy subscription
#include <message_filters/subscriber.h> #include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h> #include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h> #include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h> #include <message_filters/sync_policies/approximate_time.h>
// Include TF // Include TF
#include <tf/transform_listener.h> #include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
// STL // STL
#include <memory>
#include <string> #include <string>
#include <vector>
// ROS2 includes
#include <rclcpp/rclcpp.hpp>
#include "pcl_ros/point_cloud.hpp" // #include "pcl_ros/point_cloud.hpp"
using pcl_conversions::fromPCL; using pcl_conversions::fromPCL;
@ -73,33 +78,85 @@ namespace pcl_ros
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b PCLNodelet represents the base PCL Nodelet class. All PCL nodelets should /** \brief @b PCLNode represents the base PCL Node class. All PCL node should inherit from
* inherit from this class. * this class. */
**/ template<typename T, typename PublisherT = rclcpp::Publisher<T>>
class PCLNodelet : public nodelet_topic_tools::NodeletLazy class PCLNode : public rclcpp::Node
{ {
public: public:
typedef sensor_msgs::PointCloud2 PointCloud2; typedef sensor_msgs::msg::PointCloud2 PointCloud2;
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud; typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef boost::shared_ptr<PointCloud> PointCloudPtr; typedef PointCloud::Ptr PointCloudPtr;
typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr; typedef PointCloud::ConstPtr PointCloudConstPtr;
typedef pcl_msgs::PointIndices PointIndices; typedef pcl_msgs::msg::PointIndices PointIndices;
typedef PointIndices::Ptr PointIndicesPtr; typedef PointIndices::SharedPtr PointIndicesPtr;
typedef PointIndices::ConstPtr PointIndicesConstPtr; typedef PointIndices::ConstSharedPtr PointIndicesConstPtr;
typedef pcl_msgs::ModelCoefficients ModelCoefficients; typedef pcl_msgs::msg::ModelCoefficients ModelCoefficients;
typedef ModelCoefficients::Ptr ModelCoefficientsPtr; typedef ModelCoefficients::SharedPtr ModelCoefficientsPtr;
typedef ModelCoefficients::ConstPtr ModelCoefficientsConstPtr; typedef ModelCoefficients::ConstSharedPtr ModelCoefficientsConstPtr;
typedef pcl::IndicesPtr IndicesPtr; typedef pcl::IndicesPtr IndicesPtr;
typedef pcl::IndicesConstPtr IndicesConstPtr; typedef pcl::IndicesConstPtr IndicesConstPtr;
/** \brief Empty constructor. */ /** \brief Empty constructor. */
PCLNodelet() PCLNode(std::string node_name, const rclcpp::NodeOptions & options)
: use_indices_(false), latched_indices_(false), : rclcpp::Node(node_name, options),
max_queue_size_(3), approximate_sync_(false) {} 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: protected:
/** \brief Set to true if point indices are used. /** \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 * 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 * 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 * ~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_; 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 * 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 * be synchronised in time. By setting this flag to true, the most recent
* value from ~indices can be used instead of requiring a synchronised * value from ~indices can be used instead of requiring a synchronised
* message. * message.
**/ **/
bool latched_indices_; bool transient_local_indices_;
/** \brief The message filter subscriber for PointCloud2. */ /** \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. */ /** \brief The message filter subscriber for PointIndices. */
message_filters::Subscriber<PointIndices> sub_indices_filter_; message_filters::Subscriber<PointIndices> sub_indices_filter_;
/** \brief The output PointCloud publisher. */ /** \brief The output PointCloud publisher. Allow each individual class that inherits from this
ros::Publisher pub_output_; * to declare the Publisher with their type.
*/
std::shared_ptr<PublisherT> pub_output_;
/** \brief The maximum queue size (default: 3). */ /** \brief The maximum queue size (default: 3). */
int max_queue_size_; int max_queue_size_;
@ -140,23 +199,24 @@ protected:
bool approximate_sync_; bool approximate_sync_;
/** \brief TF listener object. */ /** \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). /** \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 cloud the point cloud to test
* \param topic_name an optional topic name (only used for printing, defaults to "input") * \param topic_name an optional topic name (only used for printing, defaults to "input")
*/ */
inline bool 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()) { if (cloud->width * cloud->height * cloud->point_step != cloud->data.size()) {
NODELET_WARN( RCLCPP_WARN(
"[%s] Invalid PointCloud (data = %zu, width = %d, height = %d, step = %d) " this->get_logger(),
"with stamp %f, and frame %s on topic %s received!", "Invalid PointCloud (data = %zu, width = %d, height = %d, step = %d) "
getName().c_str(), "with stamp %d.%09d, and frame %s on topic %s received!",
cloud->data.size(), cloud->width, cloud->height, cloud->point_step, cloud->data.size(), cloud->width, cloud->height, cloud->point_step,
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( cloud->header.stamp.sec, cloud->header.stamp.nanosec,
topic_name).c_str()); cloud->header.frame_id.c_str(), topic_name.c_str());
return false; return false;
} }
@ -171,12 +231,13 @@ protected:
isValid(const PointCloudConstPtr & cloud, const std::string & topic_name = "input") isValid(const PointCloudConstPtr & cloud, const std::string & topic_name = "input")
{ {
if (cloud->width * cloud->height != cloud->points.size()) { if (cloud->width * cloud->height != cloud->points.size()) {
NODELET_WARN( RCLCPP_WARN(
"[%s] Invalid PointCloud (points = %zu, width = %d, height = %d) " this->get_logger(),
"with stamp %f, and frame %s on topic %s received!", "Invalid PointCloud (points = %zu, width = %d, height = %d) "
getName().c_str(), cloud->points.size(), cloud->width, cloud->height, "with stamp %d.%09d, and frame %s on topic %s received!",
fromPCL(cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), cloud->points.size(), cloud->width, cloud->height,
pnh_->resolveName(topic_name).c_str()); fromPCL(cloud->header).stamp.sec, fromPCL(cloud->header).stamp.nanosec,
cloud->header.frame_id.c_str(), topic_name.c_str());
return false; return false;
} }
@ -192,11 +253,13 @@ protected:
{ {
/*if (indices->indices.empty ()) /*if (indices->indices.empty ())
{ {
NODELET_WARN ("[%s] Empty indices (values = %zu) " RCLCPP_WARN(
"with stamp %f, and frame %s on topic %s received!", this->get_logger(), "Empty indices (values = %zu) with stamp %d.%09d, "
getName ().c_str (), indices->indices.size (), indices->header.stamp.toSec (), "and frame %s on topic %s received!",
indices->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ()); indices->indices.size(), indices->header.stamp.sec, indices->header.stamp.nanosec,
return (true); indices->header.frame_id.c_str(), topic_name.c_str());
//return (true); // looks like it should be false
return false;
}*/ }*/
return true; return true;
} }
@ -210,10 +273,11 @@ protected:
{ {
/*if (model->values.empty ()) /*if (model->values.empty ())
{ {
NODELET_WARN ("[%s] Empty model (values = %zu) with stamp %f, " RCLCPP_WARN(
"and frame %s on topic %s received!", this->get_logger(), "Empty model (values = %zu) with stamp %d.%09d, "
getName ().c_str (), model->values.size (), model->header.stamp.toSec (), "and frame %s on topic %s received!",
model->header.frame_id.c_str (), pnh_->resolveName (topic_name).c_str ()); model->values.size(), model->header.stamp.sec, model->header.stamp.nanosec,
model->header.frame_id.c_str(), topic_name.c_str());
return (false); return (false);
}*/ }*/
return true; return true;
@ -225,36 +289,9 @@ protected:
virtual void subscribe() {} virtual void subscribe() {}
virtual void unsubscribe() {} 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: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}; };
} // namespace pcl_ros } // namespace pcl_ros
#endif // PCL_ROS__PCL_NODELET_HPP_ #endif // PCL_ROS__PCL_NODE_HPP_