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

migrate abstract filter node (#388)

Browse Source 
* migrate abstract filter node

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

* remove use_frame_params from constructor and make a function for the logic

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-19 14:13:06 -05:00 committed by GitHub
parent 5c5382eb5c
commit 387f5b158b
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 143 additions and 136 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
pcl_ros/CMakeLists.txt
View File

@ -80,19 +80,19 @@ ament_target_dependencies(pcl_ros_tf
# #
# #
### Declare the pcl_ros_filters library ### Declare the pcl_ros_filters library
#add_library(pcl_ros_filters add_library(pcl_ros_filters SHARED
# src/pcl_ros/filters/extract_indices.cpp # src/pcl_ros/filters/extract_indices.cpp
# src/pcl_ros/filters/filter.cpp src/pcl_ros/filters/filter.cpp
# src/pcl_ros/filters/passthrough.cpp # src/pcl_ros/filters/passthrough.cpp
# src/pcl_ros/filters/project_inliers.cpp # src/pcl_ros/filters/project_inliers.cpp
# src/pcl_ros/filters/radius_outlier_removal.cpp # src/pcl_ros/filters/radius_outlier_removal.cpp
# src/pcl_ros/filters/statistical_outlier_removal.cpp # src/pcl_ros/filters/statistical_outlier_removal.cpp
# src/pcl_ros/filters/voxel_grid.cpp # src/pcl_ros/filters/voxel_grid.cpp
# src/pcl_ros/filters/crop_box.cpp # src/pcl_ros/filters/crop_box.cpp
#) )
#target_link_libraries(pcl_ros_filters pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES}) target_link_libraries(pcl_ros_filters pcl_ros_tf ${PCL_LIBRARIES})
#add_dependencies(pcl_ros_filters ${PROJECT_NAME}_gencfg) ament_target_dependencies(pcl_ros_filters ${dependencies})
#class_loader_hide_library_symbols(pcl_ros_filters) class_loader_hide_library_symbols(pcl_ros_filters)
# #
### Declare the pcl_ros_segmentation library ### Declare the pcl_ros_segmentation library
#add_library (pcl_ros_segmentation #add_library (pcl_ros_segmentation
@ -184,7 +184,7 @@ install(
pcd_to_pointcloud_lib pcd_to_pointcloud_lib
# pcl_ros_io # pcl_ros_io
# pcl_ros_features # pcl_ros_features
# pcl_ros_filters pcl_ros_filters
# pcl_ros_surface # pcl_ros_surface
# pcl_ros_segmentation # pcl_ros_segmentation
# pointcloud_to_pcd # pointcloud_to_pcd

63
pcl_ros/include/pcl_ros/filters/filter.hpp
View File

@ -39,10 +39,10 @@
#define PCL_ROS__FILTERS__FILTER_HPP_ #define PCL_ROS__FILTERS__FILTER_HPP_
#include <pcl/filters/filter.h> #include <pcl/filters/filter.h>
#include <dynamic_reconfigure/server.h> #include <memory>
#include <string> #include <string>
#include "pcl_ros/pcl_nodelet.hpp" #include <vector>
#include "pcl_ros/FilterConfig.hpp" #include "pcl_ros/pcl_node.hpp"
namespace pcl_ros namespace pcl_ros
{ {
@ -52,19 +52,27 @@ namespace sync_policies = message_filters::sync_policies;
* applicable to all filters are defined here as static methods. * applicable to all filters are defined here as static methods.
* \author Radu Bogdan Rusu * \author Radu Bogdan Rusu
*/ */
class Filter : public PCLNodelet class Filter : public PCLNode<sensor_msgs::msg::PointCloud2>
{ {
public: public:
typedef sensor_msgs::PointCloud2 PointCloud2; typedef sensor_msgs::msg::PointCloud2 PointCloud2;
typedef pcl::IndicesPtr IndicesPtr; typedef pcl::IndicesPtr IndicesPtr;
typedef pcl::IndicesConstPtr IndicesConstPtr; typedef pcl::IndicesConstPtr IndicesConstPtr;
Filter() {} /** \brief Filter constructor
* \param node_name node name
* \param options node options
*/
Filter(std::string node_name, const rclcpp::NodeOptions & options);
protected: protected:
/** \brief declare and subscribe to param callback for input_frame and output_frame params */
void
use_frame_params();
/** \brief The input PointCloud subscriber. */ /** \brief The input PointCloud subscriber. */
ros::Subscriber sub_input_; rclcpp::Subscription<PointCloud2>::SharedPtr sub_input_;
message_filters::Subscriber<PointCloud2> sub_input_filter_; message_filters::Subscriber<PointCloud2> sub_input_filter_;
@ -96,18 +104,7 @@ protected:
std::string tf_output_frame_; std::string tf_output_frame_;
/** \brief Internal mutex. */ /** \brief Internal mutex. */
boost::mutex mutex_; std::mutex mutex_;
/** \brief Child initialization routine.
* \param nh ROS node handle
* \param has_service set to true if the child has a Dynamic Reconfigure service
*/
virtual bool
child_init(ros::NodeHandle & nh, bool & has_service)
{
has_service = false;
return true;
}
/** \brief Virtual abstract filter method. To be implemented by every child. /** \brief Virtual abstract filter method. To be implemented by every child.
* \param input the input point cloud dataset. * \param input the input point cloud dataset.
@ -116,7 +113,7 @@ protected:
*/ */
virtual void virtual void
filter( filter(
const PointCloud2::ConstPtr & input, const IndicesPtr & indices, const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
PointCloud2 & output) = 0; PointCloud2 & output) = 0;
/** \brief Lazy transport subscribe routine. */ /** \brief Lazy transport subscribe routine. */
@ -127,36 +124,34 @@ protected:
virtual void virtual void
unsubscribe(); unsubscribe();
/** \brief Nodelet initialization routine. */
virtual void
onInit();
/** \brief Call the child filter () method, optionally transform the result, and publish it. /** \brief Call the child filter () method, optionally transform the result, and publish it.
* \param input the input point cloud dataset. * \param input the input point cloud dataset.
* \param indices a pointer to the vector of point indices to use. * \param indices a pointer to the vector of point indices to use.
*/ */
void void
computePublish(const PointCloud2::ConstPtr & input, const IndicesPtr & indices); computePublish(const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices);
private: private:
/** \brief Pointer to a dynamic reconfigure service. */ /** \brief Pointer to parameters callback handle. */
boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::FilterConfig>> srv_; OnSetParametersCallbackHandle::SharedPtr callback_handle_;
/** \brief Synchronized input, and indices.*/ /** \brief Synchronized input, and indices.*/
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, std::shared_ptr<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
PointIndices>>> sync_input_indices_e_; PointIndices>>> sync_input_indices_e_;
boost::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, std::shared_ptr<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
PointIndices>>> sync_input_indices_a_; PointIndices>>> sync_input_indices_a_;
/** \brief Dynamic reconfigure service callback. */ /** \brief Parameter callback
virtual void * \param params parameter values to set
config_callback(pcl_ros::FilterConfig & config, uint32_t level); */
rcl_interfaces::msg::SetParametersResult
config_callback(const std::vector<rclcpp::Parameter> & params);
/** \brief PointCloud2 + Indices data callback. */ /** \brief PointCloud2 + Indices data callback. */
void void
input_indices_callback( input_indices_callback(
const PointCloud2::ConstPtr & cloud, const PointCloud2::ConstSharedPtr & cloud,
const PointIndicesConstPtr & indices); const PointIndices::ConstSharedPtr & indices);
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW

202
pcl_ros/src/pcl_ros/filters/filter.cpp
View File

@ -37,7 +37,6 @@
#include "pcl_ros/filters/filter.hpp" #include "pcl_ros/filters/filter.hpp"
#include <pcl/common/io.h> #include <pcl/common/io.h>
#include <vector>
#include "pcl_ros/transforms.hpp" #include "pcl_ros/transforms.hpp"
/*//#include <pcl/filters/pixel_grid.h> /*//#include <pcl/filters/pixel_grid.h>
@ -62,42 +61,42 @@
/////////////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::Filter::computePublish(const PointCloud2::ConstPtr & input, const IndicesPtr & indices) pcl_ros::Filter::computePublish(const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices)
{ {
PointCloud2 output; PointCloud2 output;
// Call the virtual method in the child // Call the virtual method in the child
filter(input, indices, output); filter(input, indices, output);
PointCloud2::Ptr cloud_tf(new PointCloud2(output)); // set the output by default PointCloud2::UniquePtr cloud_tf(new PointCloud2(output)); // set the output by default
// Check whether the user has given a different output TF frame // Check whether the user has given a different output TF frame
if (!tf_output_frame_.empty() && output.header.frame_id != tf_output_frame_) { if (!tf_output_frame_.empty() && output.header.frame_id != tf_output_frame_) {
NODELET_DEBUG( RCLCPP_DEBUG(
"[%s::computePublish] Transforming output dataset from %s to %s.", this->get_logger(), "Transforming output dataset from %s to %s.",
getName().c_str(), output.header.frame_id.c_str(), tf_output_frame_.c_str()); output.header.frame_id.c_str(), tf_output_frame_.c_str());
// Convert the cloud into the different frame // Convert the cloud into the different frame
PointCloud2 cloud_transformed; PointCloud2 cloud_transformed;
if (!pcl_ros::transformPointCloud(tf_output_frame_, output, cloud_transformed, tf_listener_)) { if (!pcl_ros::transformPointCloud(tf_output_frame_, output, cloud_transformed, tf_buffer_)) {
NODELET_ERROR( RCLCPP_ERROR(
"[%s::computePublish] Error converting output dataset from %s to %s.", this->get_logger(), "Error converting output dataset from %s to %s.",
getName().c_str(), output.header.frame_id.c_str(), tf_output_frame_.c_str()); output.header.frame_id.c_str(), tf_output_frame_.c_str());
return; return;
} }
cloud_tf.reset(new PointCloud2(cloud_transformed)); cloud_tf.reset(new PointCloud2(cloud_transformed));
} }
if (tf_output_frame_.empty() && output.header.frame_id != tf_input_orig_frame_) { if (tf_output_frame_.empty() && output.header.frame_id != tf_input_orig_frame_) {
// no tf_output_frame given, transform the dataset to its original frame // no tf_output_frame given, transform the dataset to its original frame
NODELET_DEBUG( RCLCPP_DEBUG(
"[%s::computePublish] Transforming output dataset from %s back to %s.", this->get_logger(), "Transforming output dataset from %s back to %s.",
getName().c_str(), output.header.frame_id.c_str(), tf_input_orig_frame_.c_str()); output.header.frame_id.c_str(), tf_input_orig_frame_.c_str());
// Convert the cloud into the different frame // Convert the cloud into the different frame
PointCloud2 cloud_transformed; PointCloud2 cloud_transformed;
if (!pcl_ros::transformPointCloud( if (!pcl_ros::transformPointCloud(
tf_input_orig_frame_, output, cloud_transformed, tf_input_orig_frame_, output, cloud_transformed,
tf_listener_)) tf_buffer_))
{ {
NODELET_ERROR( RCLCPP_ERROR(
"[%s::computePublish] Error converting output dataset from %s back to %s.", this->get_logger(), "Error converting output dataset from %s back to %s.",
getName().c_str(), output.header.frame_id.c_str(), tf_input_orig_frame_.c_str()); output.header.frame_id.c_str(), tf_input_orig_frame_.c_str());
return; return;
} }
cloud_tf.reset(new PointCloud2(cloud_transformed)); cloud_tf.reset(new PointCloud2(cloud_transformed));
@ -106,8 +105,8 @@ pcl_ros::Filter::computePublish(const PointCloud2::ConstPtr & input, const Indic
// Copy timestamp to keep it // Copy timestamp to keep it
cloud_tf->header.stamp = input->header.stamp; cloud_tf->header.stamp = input->header.stamp;
// Publish a boost shared ptr // Publish the unique ptr
pub_output_.publish(cloud_tf); pub_output_->publish(move(cloud_tf));
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
@ -117,28 +116,32 @@ pcl_ros::Filter::subscribe()
// If we're supposed to look for PointIndices (indices) // If we're supposed to look for PointIndices (indices)
if (use_indices_) { if (use_indices_) {
// Subscribe to the input using a filter // Subscribe to the input using a filter
sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_); sub_input_filter_.subscribe(this, "input", rclcpp::QoS(max_queue_size_).get_rmw_qos_profile());
sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_); sub_indices_filter_.subscribe(this, "indices", rclcpp::QoS(max_queue_size_).get_rmw_qos_profile());
if (approximate_sync_) { if (approximate_sync_) {
sync_input_indices_a_ = sync_input_indices_a_ =
boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2, std::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
pcl_msgs::PointIndices>>>(max_queue_size_); pcl_msgs::msg::PointIndices>>>(max_queue_size_);
sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_); sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback(bind(&Filter::input_indices_callback, this, _1, _2)); sync_input_indices_a_->registerCallback(std::bind(&Filter::input_indices_callback, this, std::placeholders::_1, std::placeholders::_2));
} else { } else {
sync_input_indices_e_ = sync_input_indices_e_ =
boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2, std::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
pcl_msgs::PointIndices>>>(max_queue_size_); pcl_msgs::msg::PointIndices>>>(max_queue_size_);
sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_); sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_e_->registerCallback(bind(&Filter::input_indices_callback, this, _1, _2)); sync_input_indices_e_->registerCallback(std::bind(&Filter::input_indices_callback, this, std::placeholders::_1, std::placeholders::_2));
} }
} else { } else {
// Workaround for a callback with custom arguments ros2/rclcpp#766
std::function<void(PointCloud2::ConstSharedPtr)> callback =
std::bind(&Filter::input_indices_callback, this, std::placeholders::_1, nullptr);
// Subscribe in an old fashion to input only (no filters) // Subscribe in an old fashion to input only (no filters)
sub_input_ = sub_input_ =
pnh_->subscribe<sensor_msgs::PointCloud2>( this->create_subscription<PointCloud2>(
"input", max_queue_size_, "input", max_queue_size_,
bind(&Filter::input_indices_callback, this, _1, pcl_msgs::PointIndicesConstPtr())); callback);
} }
} }
@ -150,114 +153,123 @@ pcl_ros::Filter::unsubscribe()
sub_input_filter_.unsubscribe(); sub_input_filter_.unsubscribe();
sub_indices_filter_.unsubscribe(); sub_indices_filter_.unsubscribe();
} else { } else {
sub_input_.shutdown(); sub_input_.reset();
} }
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void pcl_ros::Filter::Filter(std::string node_name, const rclcpp::NodeOptions & options)
pcl_ros::Filter::onInit() : PCLNode(node_name, options)
{ {
// Call the super onInit () pub_output_ = create_publisher<PointCloud2>("output", max_queue_size_);
PCLNodelet::onInit(); // TODO(daisukes): lazy subscription after rclcpp#2060
subscribe();
// Call the child's local init RCLCPP_DEBUG(this->get_logger(), "Node successfully created.");
bool has_service = false;
if (!child_init(*pnh_, has_service)) {
NODELET_ERROR("[%s::onInit] Initialization failed.", getName().c_str());
return;
}
pub_output_ = advertise<PointCloud2>(*pnh_, "output", max_queue_size_);
// Enable the dynamic reconfigure service
if (!has_service) {
srv_ = boost::make_shared<dynamic_reconfigure::Server<pcl_ros::FilterConfig>>(*pnh_);
dynamic_reconfigure::Server<pcl_ros::FilterConfig>::CallbackType f = boost::bind(
&Filter::config_callback, this, _1, _2);
srv_->setCallback(f);
}
NODELET_DEBUG("[%s::onInit] Nodelet successfully created.", getName().c_str());
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::Filter::config_callback(pcl_ros::FilterConfig & config, uint32_t level) pcl_ros::Filter::use_frame_params()
{ {
// The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are rcl_interfaces::msg::ParameterDescriptor input_frame_desc;
// inexistent in PCL input_frame_desc.name = "input_frame";
if (tf_input_frame_ != config.input_frame) { input_frame_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
tf_input_frame_ = config.input_frame; input_frame_desc.description = "The input TF frame the data should be transformed into before processing, if input.header.frame_id is different.";
NODELET_DEBUG( declare_parameter(input_frame_desc.name, rclcpp::ParameterValue(""), input_frame_desc);
"[%s::config_callback] Setting the input TF frame to: %s.",
getName().c_str(), tf_input_frame_.c_str()); 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(&Filter::config_callback, this, std::placeholders::_1));
std::vector<std::string> 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);
} }
if (tf_output_frame_ != config.output_frame) { }
tf_output_frame_ = config.output_frame;
NODELET_DEBUG( //////////////////////////////////////////////////////////////////////////////////////////////
"[%s::config_callback] Setting the output TF frame to: %s.", rcl_interfaces::msg::SetParametersResult
getName().c_str(), tf_output_frame_.c_str()); pcl_ros::Filter::config_callback(const std::vector<rclcpp::Parameter> & params)
{
std::lock_guard<std::mutex> 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());
}
}
} }
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
return result;
} }
////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////
void void
pcl_ros::Filter::input_indices_callback( pcl_ros::Filter::input_indices_callback(
const PointCloud2::ConstPtr & cloud, const PointCloud2::ConstSharedPtr & cloud,
const PointIndicesConstPtr & indices) const PointIndices::ConstSharedPtr & indices)
{ {
// If cloud is given, check if it's valid // If cloud is given, check if it's valid
if (!isValid(cloud)) { if (!isValid(cloud)) {
NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str()); RCLCPP_ERROR(this->get_logger(), "Invalid input!");
return; return;
} }
// If indices are given, check if they are valid // If indices are given, check if they are valid
if (indices && !isValid(indices)) { if (indices && !isValid(indices)) {
NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str()); RCLCPP_ERROR(this->get_logger(), "Invalid indices!");
return; return;
} }
/// DEBUG /// DEBUG
if (indices) { if (indices) {
NODELET_DEBUG( RCLCPP_DEBUG(
"[%s::input_indices_callback]\n" this->get_logger(), "[input_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and " " - PointCloud with %d data points (%s), stamp %d.%09d, and frame %s on topic %s received.\n"
"frame %s on topic %s received.\n" " - PointIndices with %zu values, stamp %d.%09d, and frame %s on topic %s received.",
" - 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(), cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName( cloud->header.stamp.sec, cloud->header.stamp.nanosec, cloud->header.frame_id.c_str(), "input",
"input").c_str(), indices->indices.size(), indices->header.stamp.sec, indices->header.stamp.nanosec,
indices->indices.size(), indices->header.stamp.toSec(), indices->header.frame_id.c_str(), "indices");
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
} else { } else {
NODELET_DEBUG( RCLCPP_DEBUG(
"[%s::input_indices_callback] PointCloud with %d data points and frame %s on " this->get_logger(), "PointCloud with %d data points and frame %s on topic %s received.",
"topic %s received.", cloud->width * cloud->height, cloud->header.frame_id.c_str(), "input");
getName().c_str(), cloud->width * cloud->height,
cloud->header.frame_id.c_str(), pnh_->resolveName("input").c_str());
} }
/// ///
// Check whether the user has given a different input TF frame // Check whether the user has given a different input TF frame
tf_input_orig_frame_ = cloud->header.frame_id; tf_input_orig_frame_ = cloud->header.frame_id;
PointCloud2::ConstPtr cloud_tf; PointCloud2::ConstSharedPtr cloud_tf;
if (!tf_input_frame_.empty() && cloud->header.frame_id != tf_input_frame_) { if (!tf_input_frame_.empty() && cloud->header.frame_id != tf_input_frame_) {
NODELET_DEBUG( RCLCPP_DEBUG(
"[%s::input_indices_callback] Transforming input dataset from %s to %s.", this->get_logger(), "Transforming input dataset from %s to %s.",
getName().c_str(), cloud->header.frame_id.c_str(), tf_input_frame_.c_str()); cloud->header.frame_id.c_str(), tf_input_frame_.c_str());
// Save the original frame ID // Save the original frame ID
// Convert the cloud into the different frame // Convert the cloud into the different frame
PointCloud2 cloud_transformed; PointCloud2 cloud_transformed;
if (!pcl_ros::transformPointCloud(tf_input_frame_, *cloud, cloud_transformed, tf_listener_)) { if (!pcl_ros::transformPointCloud(tf_input_frame_, *cloud, cloud_transformed, tf_buffer_)) {
NODELET_ERROR( RCLCPP_ERROR(
"[%s::input_indices_callback] Error converting input dataset from %s to %s.", this->get_logger(), "Error converting input dataset from %s to %s.",
getName().c_str(), cloud->header.frame_id.c_str(), tf_input_frame_.c_str()); cloud->header.frame_id.c_str(), tf_input_frame_.c_str());
return; return;
} }
cloud_tf = boost::make_shared<PointCloud2>(cloud_transformed); cloud_tf = std::make_shared<PointCloud2>(cloud_transformed);
} else { } else {
cloud_tf = cloud; cloud_tf = cloud;
} }