migrate abstract filter node (#388)
* 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
GitHub
parent
5c5382eb5c
commit
387f5b158b
@ -80,19 +80,19 @@ ament_target_dependencies(pcl_ros_tf
|
||||
#
|
||||
#
|
||||
### 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/filter.cpp
|
||||
src/pcl_ros/filters/filter.cpp
|
||||
# src/pcl_ros/filters/passthrough.cpp
|
||||
# src/pcl_ros/filters/project_inliers.cpp
|
||||
# src/pcl_ros/filters/radius_outlier_removal.cpp
|
||||
# src/pcl_ros/filters/statistical_outlier_removal.cpp
|
||||
# src/pcl_ros/filters/voxel_grid.cpp
|
||||
# src/pcl_ros/filters/crop_box.cpp
|
||||
#)
|
||||
#target_link_libraries(pcl_ros_filters pcl_ros_tf ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES})
|
||||
#add_dependencies(pcl_ros_filters ${PROJECT_NAME}_gencfg)
|
||||
#class_loader_hide_library_symbols(pcl_ros_filters)
|
||||
)
|
||||
target_link_libraries(pcl_ros_filters pcl_ros_tf ${PCL_LIBRARIES})
|
||||
ament_target_dependencies(pcl_ros_filters ${dependencies})
|
||||
class_loader_hide_library_symbols(pcl_ros_filters)
|
||||
#
|
||||
### Declare the pcl_ros_segmentation library
|
||||
#add_library (pcl_ros_segmentation
|
||||
@ -184,7 +184,7 @@ install(
|
||||
pcd_to_pointcloud_lib
|
||||
# pcl_ros_io
|
||||
# pcl_ros_features
|
||||
# pcl_ros_filters
|
||||
pcl_ros_filters
|
||||
# pcl_ros_surface
|
||||
# pcl_ros_segmentation
|
||||
# pointcloud_to_pcd
|
||||
|
||||
@ -39,10 +39,10 @@
|
||||
#define PCL_ROS__FILTERS__FILTER_HPP_
|
||||
|
||||
#include <pcl/filters/filter.h>
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include "pcl_ros/pcl_nodelet.hpp"
|
||||
#include "pcl_ros/FilterConfig.hpp"
|
||||
#include <vector>
|
||||
#include "pcl_ros/pcl_node.hpp"
|
||||
|
||||
namespace pcl_ros
|
||||
{
|
||||
@ -52,19 +52,27 @@ namespace sync_policies = message_filters::sync_policies;
|
||||
* applicable to all filters are defined here as static methods.
|
||||
* \author Radu Bogdan Rusu
|
||||
*/
|
||||
class Filter : public PCLNodelet
|
||||
class Filter : public PCLNode<sensor_msgs::msg::PointCloud2>
|
||||
{
|
||||
public:
|
||||
typedef sensor_msgs::PointCloud2 PointCloud2;
|
||||
typedef sensor_msgs::msg::PointCloud2 PointCloud2;
|
||||
|
||||
typedef pcl::IndicesPtr IndicesPtr;
|
||||
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:
|
||||
/** \brief declare and subscribe to param callback for input_frame and output_frame params */
|
||||
void
|
||||
use_frame_params();
|
||||
|
||||
/** \brief The input PointCloud subscriber. */
|
||||
ros::Subscriber sub_input_;
|
||||
rclcpp::Subscription<PointCloud2>::SharedPtr sub_input_;
|
||||
|
||||
message_filters::Subscriber<PointCloud2> sub_input_filter_;
|
||||
|
||||
@ -96,18 +104,7 @@ protected:
|
||||
std::string tf_output_frame_;
|
||||
|
||||
/** \brief Internal mutex. */
|
||||
boost::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;
|
||||
}
|
||||
std::mutex mutex_;
|
||||
|
||||
/** \brief Virtual abstract filter method. To be implemented by every child.
|
||||
* \param input the input point cloud dataset.
|
||||
@ -116,7 +113,7 @@ protected:
|
||||
*/
|
||||
virtual void
|
||||
filter(
|
||||
const PointCloud2::ConstPtr & input, const IndicesPtr & indices,
|
||||
const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices,
|
||||
PointCloud2 & output) = 0;
|
||||
|
||||
/** \brief Lazy transport subscribe routine. */
|
||||
@ -127,36 +124,34 @@ protected:
|
||||
virtual void
|
||||
unsubscribe();
|
||||
|
||||
/** \brief Nodelet initialization routine. */
|
||||
virtual void
|
||||
onInit();
|
||||
|
||||
/** \brief Call the child filter () method, optionally transform the result, and publish it.
|
||||
* \param input the input point cloud dataset.
|
||||
* \param indices a pointer to the vector of point indices to use.
|
||||
*/
|
||||
void
|
||||
computePublish(const PointCloud2::ConstPtr & input, const IndicesPtr & indices);
|
||||
computePublish(const PointCloud2::ConstSharedPtr & input, const IndicesPtr & indices);
|
||||
|
||||
private:
|
||||
/** \brief Pointer to a dynamic reconfigure service. */
|
||||
boost::shared_ptr<dynamic_reconfigure::Server<pcl_ros::FilterConfig>> srv_;
|
||||
/** \brief Pointer to parameters callback handle. */
|
||||
OnSetParametersCallbackHandle::SharedPtr callback_handle_;
|
||||
|
||||
/** \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_;
|
||||
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_;
|
||||
|
||||
/** \brief Dynamic reconfigure service callback. */
|
||||
virtual void
|
||||
config_callback(pcl_ros::FilterConfig & config, uint32_t level);
|
||||
/** \brief Parameter callback
|
||||
* \param params parameter values to set
|
||||
*/
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
config_callback(const std::vector<rclcpp::Parameter> & params);
|
||||
|
||||
/** \brief PointCloud2 + Indices data callback. */
|
||||
void
|
||||
input_indices_callback(
|
||||
const PointCloud2::ConstPtr & cloud,
|
||||
const PointIndicesConstPtr & indices);
|
||||
const PointCloud2::ConstSharedPtr & cloud,
|
||||
const PointIndices::ConstSharedPtr & indices);
|
||||
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
|
||||
@ -37,7 +37,6 @@
|
||||
|
||||
#include "pcl_ros/filters/filter.hpp"
|
||||
#include <pcl/common/io.h>
|
||||
#include <vector>
|
||||
#include "pcl_ros/transforms.hpp"
|
||||
|
||||
/*//#include <pcl/filters/pixel_grid.h>
|
||||
@ -62,42 +61,42 @@
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
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;
|
||||
// Call the virtual method in the child
|
||||
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
|
||||
if (!tf_output_frame_.empty() && output.header.frame_id != tf_output_frame_) {
|
||||
NODELET_DEBUG(
|
||||
"[%s::computePublish] Transforming output dataset from %s to %s.",
|
||||
getName().c_str(), output.header.frame_id.c_str(), tf_output_frame_.c_str());
|
||||
RCLCPP_DEBUG(
|
||||
this->get_logger(), "Transforming output dataset from %s to %s.",
|
||||
output.header.frame_id.c_str(), tf_output_frame_.c_str());
|
||||
// Convert the cloud into the different frame
|
||||
PointCloud2 cloud_transformed;
|
||||
if (!pcl_ros::transformPointCloud(tf_output_frame_, output, cloud_transformed, tf_listener_)) {
|
||||
NODELET_ERROR(
|
||||
"[%s::computePublish] Error converting output dataset from %s to %s.",
|
||||
getName().c_str(), output.header.frame_id.c_str(), tf_output_frame_.c_str());
|
||||
if (!pcl_ros::transformPointCloud(tf_output_frame_, output, cloud_transformed, tf_buffer_)) {
|
||||
RCLCPP_ERROR(
|
||||
this->get_logger(), "Error converting output dataset from %s to %s.",
|
||||
output.header.frame_id.c_str(), tf_output_frame_.c_str());
|
||||
return;
|
||||
}
|
||||
cloud_tf.reset(new PointCloud2(cloud_transformed));
|
||||
}
|
||||
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
|
||||
NODELET_DEBUG(
|
||||
"[%s::computePublish] Transforming output dataset from %s back to %s.",
|
||||
getName().c_str(), output.header.frame_id.c_str(), tf_input_orig_frame_.c_str());
|
||||
RCLCPP_DEBUG(
|
||||
this->get_logger(), "Transforming output dataset from %s back to %s.",
|
||||
output.header.frame_id.c_str(), tf_input_orig_frame_.c_str());
|
||||
// Convert the cloud into the different frame
|
||||
PointCloud2 cloud_transformed;
|
||||
if (!pcl_ros::transformPointCloud(
|
||||
tf_input_orig_frame_, output, cloud_transformed,
|
||||
tf_listener_))
|
||||
tf_buffer_))
|
||||
{
|
||||
NODELET_ERROR(
|
||||
"[%s::computePublish] Error converting output dataset from %s back to %s.",
|
||||
getName().c_str(), output.header.frame_id.c_str(), tf_input_orig_frame_.c_str());
|
||||
RCLCPP_ERROR(
|
||||
this->get_logger(), "Error converting output dataset from %s back to %s.",
|
||||
output.header.frame_id.c_str(), tf_input_orig_frame_.c_str());
|
||||
return;
|
||||
}
|
||||
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
|
||||
cloud_tf->header.stamp = input->header.stamp;
|
||||
|
||||
// Publish a boost shared ptr
|
||||
pub_output_.publish(cloud_tf);
|
||||
// Publish the unique ptr
|
||||
pub_output_->publish(move(cloud_tf));
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
@ -117,28 +116,32 @@ pcl_ros::Filter::subscribe()
|
||||
// If we're supposed to look for PointIndices (indices)
|
||||
if (use_indices_) {
|
||||
// Subscribe to the input using a filter
|
||||
sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
|
||||
sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
|
||||
sub_input_filter_.subscribe(this, "input", rclcpp::QoS(max_queue_size_).get_rmw_qos_profile());
|
||||
sub_indices_filter_.subscribe(this, "indices", rclcpp::QoS(max_queue_size_).get_rmw_qos_profile());
|
||||
|
||||
if (approximate_sync_) {
|
||||
sync_input_indices_a_ =
|
||||
boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
|
||||
pcl_msgs::PointIndices>>>(max_queue_size_);
|
||||
std::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
|
||||
pcl_msgs::msg::PointIndices>>>(max_queue_size_);
|
||||
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 {
|
||||
sync_input_indices_e_ =
|
||||
boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
|
||||
pcl_msgs::PointIndices>>>(max_queue_size_);
|
||||
std::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
|
||||
pcl_msgs::msg::PointIndices>>>(max_queue_size_);
|
||||
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 {
|
||||
// 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)
|
||||
sub_input_ =
|
||||
pnh_->subscribe<sensor_msgs::PointCloud2>(
|
||||
this->create_subscription<PointCloud2>(
|
||||
"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_indices_filter_.unsubscribe();
|
||||
} else {
|
||||
sub_input_.shutdown();
|
||||
sub_input_.reset();
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
void
|
||||
pcl_ros::Filter::onInit()
|
||||
pcl_ros::Filter::Filter(std::string node_name, const rclcpp::NodeOptions & options)
|
||||
: PCLNode(node_name, options)
|
||||
{
|
||||
// Call the super onInit ()
|
||||
PCLNodelet::onInit();
|
||||
|
||||
// Call the child's local init
|
||||
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());
|
||||
pub_output_ = create_publisher<PointCloud2>("output", max_queue_size_);
|
||||
// TODO(daisukes): lazy subscription after rclcpp#2060
|
||||
subscribe();
|
||||
RCLCPP_DEBUG(this->get_logger(), "Node successfully created.");
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
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
|
||||
// inexistent in PCL
|
||||
if (tf_input_frame_ != config.input_frame) {
|
||||
tf_input_frame_ = config.input_frame;
|
||||
NODELET_DEBUG(
|
||||
"[%s::config_callback] Setting the input TF frame to: %s.",
|
||||
getName().c_str(), tf_input_frame_.c_str());
|
||||
rcl_interfaces::msg::ParameterDescriptor input_frame_desc;
|
||||
input_frame_desc.name = "input_frame";
|
||||
input_frame_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
|
||||
input_frame_desc.description = "The input TF frame the data should be transformed into before processing, if input.header.frame_id is different.";
|
||||
declare_parameter(input_frame_desc.name, rclcpp::ParameterValue(""), input_frame_desc);
|
||||
|
||||
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.",
|
||||
getName().c_str(), tf_output_frame_.c_str());
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
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
|
||||
pcl_ros::Filter::input_indices_callback(
|
||||
const PointCloud2::ConstPtr & cloud,
|
||||
const PointIndicesConstPtr & indices)
|
||||
const PointCloud2::ConstSharedPtr & cloud,
|
||||
const PointIndices::ConstSharedPtr & indices)
|
||||
{
|
||||
// If cloud is given, check if it's valid
|
||||
if (!isValid(cloud)) {
|
||||
NODELET_ERROR("[%s::input_indices_callback] Invalid input!", getName().c_str());
|
||||
RCLCPP_ERROR(this->get_logger(), "Invalid input!");
|
||||
return;
|
||||
}
|
||||
// If indices are given, check if they are valid
|
||||
if (indices && !isValid(indices)) {
|
||||
NODELET_ERROR("[%s::input_indices_callback] Invalid indices!", getName().c_str());
|
||||
RCLCPP_ERROR(this->get_logger(), "Invalid indices!");
|
||||
return;
|
||||
}
|
||||
|
||||
/// DEBUG
|
||||
if (indices) {
|
||||
NODELET_DEBUG(
|
||||
"[%s::input_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %f, and "
|
||||
"frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %f, and "
|
||||
"frame %s on topic %s received.",
|
||||
getName().c_str(),
|
||||
RCLCPP_DEBUG(
|
||||
this->get_logger(), "[input_indices_callback]\n"
|
||||
" - PointCloud with %d data points (%s), stamp %d.%09d, and frame %s on topic %s received.\n"
|
||||
" - PointIndices with %zu values, stamp %d.%09d, and frame %s on topic %s received.",
|
||||
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
|
||||
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
|
||||
"input").c_str(),
|
||||
indices->indices.size(), indices->header.stamp.toSec(),
|
||||
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
|
||||
cloud->header.stamp.sec, cloud->header.stamp.nanosec, cloud->header.frame_id.c_str(), "input",
|
||||
indices->indices.size(), indices->header.stamp.sec, indices->header.stamp.nanosec,
|
||||
indices->header.frame_id.c_str(), "indices");
|
||||
} else {
|
||||
NODELET_DEBUG(
|
||||
"[%s::input_indices_callback] PointCloud with %d data points and frame %s on "
|
||||
"topic %s received.",
|
||||
getName().c_str(), cloud->width * cloud->height,
|
||||
cloud->header.frame_id.c_str(), pnh_->resolveName("input").c_str());
|
||||
RCLCPP_DEBUG(
|
||||
this->get_logger(), "PointCloud with %d data points and frame %s on topic %s received.",
|
||||
cloud->width * cloud->height, cloud->header.frame_id.c_str(), "input");
|
||||
}
|
||||
///
|
||||
|
||||
// Check whether the user has given a different input TF frame
|
||||
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_) {
|
||||
NODELET_DEBUG(
|
||||
"[%s::input_indices_callback] Transforming input dataset from %s to %s.",
|
||||
getName().c_str(), cloud->header.frame_id.c_str(), tf_input_frame_.c_str());
|
||||
RCLCPP_DEBUG(
|
||||
this->get_logger(), "Transforming input dataset from %s to %s.",
|
||||
cloud->header.frame_id.c_str(), tf_input_frame_.c_str());
|
||||
// Save the original frame ID
|
||||
// Convert the cloud into the different frame
|
||||
PointCloud2 cloud_transformed;
|
||||
if (!pcl_ros::transformPointCloud(tf_input_frame_, *cloud, cloud_transformed, tf_listener_)) {
|
||||
NODELET_ERROR(
|
||||
"[%s::input_indices_callback] Error converting input dataset from %s to %s.",
|
||||
getName().c_str(), cloud->header.frame_id.c_str(), tf_input_frame_.c_str());
|
||||
if (!pcl_ros::transformPointCloud(tf_input_frame_, *cloud, cloud_transformed, tf_buffer_)) {
|
||||
RCLCPP_ERROR(
|
||||
this->get_logger(), "Error converting input dataset from %s to %s.",
|
||||
cloud->header.frame_id.c_str(), tf_input_frame_.c_str());
|
||||
return;
|
||||
}
|
||||
cloud_tf = boost::make_shared<PointCloud2>(cloud_transformed);
|
||||
cloud_tf = std::make_shared<PointCloud2>(cloud_transformed);
|
||||
} else {
|
||||
cloud_tf = cloud;
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user