migrate pcl_nodelet.hpp to pcl_node.hpp (#385)
* 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
GitHub
parent
ca69652eb8
commit
5c5382eb5c
@ -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 <sensor_msgs/PointCloud2.h>
|
||||
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||||
// PCL includes
|
||||
#include <pcl_msgs/PointIndices.h>
|
||||
#include <pcl_msgs/ModelCoefficients.h>
|
||||
#include <pcl_msgs/msg/point_indices.hpp>
|
||||
#include <pcl_msgs/msg/model_coefficients.hpp>
|
||||
#include <pcl/pcl_base.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
// 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/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>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
#include <tf2_ros/buffer.h>
|
||||
|
||||
// STL
|
||||
#include <memory>
|
||||
#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;
|
||||
|
||||
@ -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<typename T, typename PublisherT = rclcpp::Publisher<T>>
|
||||
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::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<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_;
|
||||
/** \brief The output PointCloud publisher. Allow each individual class that inherits from this
|
||||
* to declare the Publisher with their type.
|
||||
*/
|
||||
std::shared_ptr<PublisherT> 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_
|
||||
Loading…
x
Reference in New Issue
Block a user