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

Make ament_cpplint pass (#298)

Browse Source 
Collection of hand-made changes to make ament_cpplint pass consisting of:
- whitespace between comments
- line length
- header ordering
- include guard formats
- remove a couple `using namespace std;`
- using c++ casts instead of c-style casts

Signed-off-by: Sean Kelly <sean@seankelly.dev>
This commit is contained in:
Sean Kelly
2020-08-09 19:47:21 -04:00
committed by GitHub
parent 9689971aee
commit 420f5b032b
67 changed files with 831 additions and 593 deletions
Download Patch File Download Diff File Expand all files Collapse all files
pcl_ros/src/pcl_ros/features/feature.cpp
+87 -52
View File
@@ -35,21 +35,22 @@
*
*/
//#include <pluginlib/class_list_macros.h>
// #include <pluginlib/class_list_macros.h>
// Include the implementations here instead of compiling them separately to speed up compile time
//#include "normal_3d.cpp"
//#include "boundary.cpp"
//#include "fpfh.cpp"
//#include "fpfh_omp.cpp"
//#include "moment_invariants.cpp"
//#include "normal_3d_omp.cpp"
//#include "normal_3d_tbb.cpp"
//#include "pfh.cpp"
//#include "principal_curvatures.cpp"
//#include "vfh.cpp"
// #include "normal_3d.cpp"
// #include "boundary.cpp"
// #include "fpfh.cpp"
// #include "fpfh_omp.cpp"
// #include "moment_invariants.cpp"
// #include "normal_3d_omp.cpp"
// #include "normal_3d_tbb.cpp"
// #include "pfh.cpp"
// #include "principal_curvatures.cpp"
// #include "vfh.cpp"
#include <pcl/io/io.h>
#include "pcl_ros/features/feature.hpp"
#include <message_filters/null_types.h>
#include <vector>
#include "pcl_ros/features/feature.hpp"
////////////////////////////////////////////////////////////////////////////////////////////
void
@@ -62,13 +63,15 @@ pcl_ros::Feature::onInit()
childInit(*pnh_);
// Allow each individual class that inherits from us to declare their own Publisher
// This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc
//pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
// This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish
// PointCloud<Normal>, etc
// pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
// ---[ Mandatory parameters
if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) {
NODELET_ERROR(
"[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.",
"[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these "
"parameters before continuing.",
getName().c_str());
return;
}
@@ -108,11 +111,14 @@ pcl_ros::Feature::subscribe()
if (use_indices_ || use_surface_) {
// Create the objects here
if (approximate_sync_) {
sync_input_surface_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn,
PointCloudIn, PointIndices>>>(max_queue_size_);
sync_input_surface_indices_a_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ApproximateTime<
PointCloudIn, PointCloudIn, PointIndices>>>(max_queue_size_);
} else {
sync_input_surface_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
PointCloudIn, PointIndices>>>(max_queue_size_);
sync_input_surface_indices_e_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ExactTime<PointCloudIn, PointCloudIn, PointIndices>>>(max_queue_size_);
}
// Subscribe to the input using a filter
@@ -121,7 +127,8 @@ pcl_ros::Feature::subscribe()
// If indices are enabled, subscribe to the indices
sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
if (use_surface_) { // Use both indices and surface
// If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register
// If surface is enabled, subscribe to the surface,
// connect the input-indices-surface trio and register
sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
if (approximate_sync_) {
sync_input_surface_indices_a_->connectInput(
@@ -251,9 +258,12 @@ pcl_ros::Feature::input_surface_indices_callback(
if (indices) {
NODELET_DEBUG(
"[input_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\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.",
" - PointCloud with %d data points (%s), stamp %f, "
"and frame %s on topic %s received.\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.",
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(),
@@ -266,8 +276,10 @@ pcl_ros::Feature::input_surface_indices_callback(
} else {
NODELET_DEBUG(
"[input_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.",
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(),
@@ -279,8 +291,10 @@ pcl_ros::Feature::input_surface_indices_callback(
} else if (indices) {
NODELET_DEBUG(
"[input_surface_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.",
" - 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.",
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(),
@@ -288,16 +302,18 @@ pcl_ros::Feature::input_surface_indices_callback(
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
} else {
NODELET_DEBUG(
"[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", cloud->width * cloud->height, fromPCL(
"[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on "
"topic %s received.", cloud->width * cloud->height, fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str());
}
///
if ((int)(cloud->width * cloud->height) < k_) {
if (static_cast<int>(cloud->width * cloud->height) < k_) {
NODELET_ERROR(
"[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", k_,
"[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger "
"than the PointCloud size (%d)!", k_,
(int)(cloud->width * cloud->height));
emptyPublish(cloud);
return;
@@ -327,13 +343,15 @@ pcl_ros::FeatureFromNormals::onInit()
childInit(*pnh_);
// Allow each individual class that inherits from us to declare their own Publisher
// This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc
//pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
// This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish
// PointCloud<Normal>, etc
// pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
// ---[ Mandatory parameters
if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) {
NODELET_ERROR(
"[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.",
"[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these "
"parameters before continuing.",
getName().c_str());
return;
}
@@ -373,10 +391,13 @@ pcl_ros::FeatureFromNormals::subscribe()
// Create the objects here
if (approximate_sync_) {
sync_input_normals_surface_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn,
PointCloudN, PointCloudIn, PointIndices>>>(max_queue_size_);
sync_input_normals_surface_indices_a_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ApproximateTime<PointCloudIn, PointCloudN,
PointCloudIn, PointIndices>>>(max_queue_size_);
} else {
sync_input_normals_surface_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
sync_input_normals_surface_indices_e_ =
boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
PointCloudN, PointCloudIn, PointIndices>>>(max_queue_size_);
}
@@ -386,7 +407,8 @@ pcl_ros::FeatureFromNormals::subscribe()
// If indices are enabled, subscribe to the indices
sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
if (use_surface_) { // Use both indices and surface
// If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register
// If surface is enabled, subscribe to the surface, connect the input-indices-surface trio
// and register
sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
if (approximate_sync_) {
sync_input_normals_surface_indices_a_->connectInput(
@@ -490,7 +512,7 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback(
}
// If cloud+normals is given, check if it's valid
if (!isValid(cloud)) {// || !isValid (cloud_normals, "normals"))
if (!isValid(cloud)) { // || !isValid (cloud_normals, "normals"))
NODELET_ERROR("[%s::input_normals_surface_indices_callback] Invalid input!", getName().c_str());
emptyPublish(cloud);
return;
@@ -519,10 +541,14 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback(
if (indices) {
NODELET_DEBUG(
"[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\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.",
" - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.\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(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@@ -540,9 +566,12 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback(
} else {
NODELET_DEBUG(
"[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
" - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@@ -559,9 +588,12 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback(
} else if (indices) {
NODELET_DEBUG(
"[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\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.",
" - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.\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(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@@ -575,8 +607,10 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback(
} else {
NODELET_DEBUG(
"[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@@ -588,9 +622,10 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback(
}
///
if ((int)(cloud->width * cloud->height) < k_) {
if (static_cast<int>(cloud->width * cloud->height) < k_) {
NODELET_ERROR(
"[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!",
"[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) "
"is larger than the PointCloud size (%d)!",
getName().c_str(), k_, (int)(cloud->width * cloud->height));
emptyPublish(cloud);
return;
pcl_ros/src/pcl_ros/features/normal_3d_tbb.cpp
+1 -1
View File
@@ -78,4 +78,4 @@ pcl_ros::NormalEstimationTBB::computePublish(
typedef pcl_ros::NormalEstimationTBB NormalEstimationTBB;
PLUGINLIB_EXPORT_CLASS(NormalEstimationTBB, nodelet::Nodelet)
#endif // HAVE_TBB
#endif // HAVE_TBB
pcl_ros/src/pcl_ros/filters/crop_box.cpp
+3 -2
View File
@@ -36,8 +36,8 @@
*
*/
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/filters/crop_box.hpp"
#include <pluginlib/class_list_macros.h>
//////////////////////////////////////////////////////////////////////////////////////////////
bool
@@ -102,7 +102,8 @@ pcl_ros::CropBox::config_callback(pcl_ros::CropBoxConfig & config, uint32_t leve
impl_.setNegative(config.negative);
}
// The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL
// 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(
pcl_ros/src/pcl_ros/filters/features/feature.cpp
+94 -57
View File
@@ -35,21 +35,22 @@
*
*/
//#include <pluginlib/class_list_macros.h>
// #include <pluginlib/class_list_macros.h>
// Include the implementations here instead of compiling them separately to speed up compile time
//#include "normal_3d.cpp"
//#include "boundary.cpp"
//#include "fpfh.cpp"
//#include "fpfh_omp.cpp"
//#include "moment_invariants.cpp"
//#include "normal_3d_omp.cpp"
//#include "normal_3d_tbb.cpp"
//#include "pfh.cpp"
//#include "principal_curvatures.cpp"
//#include "vfh.cpp"
// #include "normal_3d.cpp"
// #include "boundary.cpp"
// #include "fpfh.cpp"
// #include "fpfh_omp.cpp"
// #include "moment_invariants.cpp"
// #include "normal_3d_omp.cpp"
// #include "normal_3d_tbb.cpp"
// #include "pfh.cpp"
// #include "principal_curvatures.cpp"
// #include "vfh.cpp"
#include <pcl/io/io.h>
#include "pcl_ros/features/feature.hpp"
#include <message_filters/null_types.h>
#include <vector>
#include "pcl_ros/features/feature.hpp"
////////////////////////////////////////////////////////////////////////////////////////////
void
@@ -62,13 +63,15 @@ pcl_ros::Feature::onInit()
childInit(*pnh_);
// Allow each individual class that inherits from us to declare their own Publisher
// This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc
//pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
// This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish
// PointCloud<Normal>, etc
// pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
// ---[ Mandatory parameters
if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) {
NODELET_ERROR(
"[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.",
"[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these "
"parameters before continuing.",
getName().c_str());
return;
}
@@ -92,11 +95,15 @@ pcl_ros::Feature::onInit()
if (use_indices_ || use_surface_) {
// Create the objects here
if (approximate_sync_) {
sync_input_surface_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn,
PointCloudIn, PointIndices>>>(max_queue_size_);
sync_input_surface_indices_a_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ApproximateTime<
PointCloudIn, PointCloudIn, PointIndices>>>(max_queue_size_);
} else {
sync_input_surface_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
PointCloudIn, PointIndices>>>(max_queue_size_);
sync_input_surface_indices_e_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ExactTime<
PointCloudIn, PointCloudIn, PointIndices>>>(max_queue_size_);
}
// Subscribe to the input using a filter
@@ -105,7 +112,8 @@ pcl_ros::Feature::onInit()
// If indices are enabled, subscribe to the indices
sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
if (use_surface_) { // Use both indices and surface
// If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register
// If surface is enabled, subscribe to the surface, connect the input-indices-surface trio
// and register
sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
if (approximate_sync_) {
sync_input_surface_indices_a_->connectInput(
@@ -225,9 +233,12 @@ pcl_ros::Feature::input_surface_indices_callback(
if (indices) {
NODELET_DEBUG(
"[input_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\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.",
" - PointCloud with %d data points (%s), stamp %f, "
"and frame %s on topic %s received.\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.",
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(),
@@ -240,8 +251,10 @@ pcl_ros::Feature::input_surface_indices_callback(
} else {
NODELET_DEBUG(
"[input_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, 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(),
@@ -253,8 +266,10 @@ pcl_ros::Feature::input_surface_indices_callback(
} else if (indices) {
NODELET_DEBUG(
"[input_surface_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.",
" - 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.",
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(),
@@ -262,16 +277,18 @@ pcl_ros::Feature::input_surface_indices_callback(
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
} else {
NODELET_DEBUG(
"[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", cloud->width * cloud->height,
"[input_surface_indices_callback] PointCloud with %d data points, stamp %f, and "
"frame %s on topic %s received.", cloud->width * cloud->height,
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str());
}
///
if ((int)(cloud->width * cloud->height) < k_) {
if (static_cast<int>(cloud->width * cloud->height) < k_) {
NODELET_ERROR(
"[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!", k_,
"[input_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger "
"than the PointCloud size (%d)!", k_,
(int)(cloud->width * cloud->height));
emptyPublish(cloud);
return;
@@ -301,13 +318,15 @@ pcl_ros::FeatureFromNormals::onInit()
childInit(*pnh_);
// Allow each individual class that inherits from us to declare their own Publisher
// This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish PointCloud<Normal>, etc
//pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
// This is useful for Publisher<pcl::PointCloud<T> >, as NormalEstimation can publish
// PointCloud<Normal>, etc
// pub_output_ = pnh_->template advertise<PointCloud2> ("output", max_queue_size_);
// ---[ Mandatory parameters
if (!pnh_->getParam("k_search", k_) && !pnh_->getParam("radius_search", search_radius_)) {
NODELET_ERROR(
"[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these parameters before continuing.",
"[%s::onInit] Neither 'k_search' nor 'radius_search' set! Need to set at least one of these "
"parameters before continuing.",
getName().c_str());
return;
}
@@ -331,11 +350,15 @@ pcl_ros::FeatureFromNormals::onInit()
// Create the objects here
if (approximate_sync_) {
sync_input_normals_surface_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloudIn,
PointCloudN, PointCloudIn, PointIndices>>>(max_queue_size_);
sync_input_normals_surface_indices_a_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ApproximateTime<
PointCloudIn, PointCloudN, PointCloudIn, PointIndices>>>(max_queue_size_);
} else {
sync_input_normals_surface_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloudIn,
PointCloudN, PointCloudIn, PointIndices>>>(max_queue_size_);
sync_input_normals_surface_indices_e_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ExactTime<
PointCloudIn, PointCloudN, PointCloudIn, PointIndices>>>(max_queue_size_);
}
// If we're supposed to look for PointIndices (indices) or PointCloud (surface) messages
@@ -344,7 +367,8 @@ pcl_ros::FeatureFromNormals::onInit()
// If indices are enabled, subscribe to the indices
sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
if (use_surface_) { // Use both indices and surface
// If surface is enabled, subscribe to the surface, connect the input-indices-surface trio and register
// If surface is enabled, subscribe to the surface, connect the input-indices-surface trio
// and register
sub_surface_filter_.subscribe(*pnh_, "surface", max_queue_size_);
if (approximate_sync_) {
sync_input_normals_surface_indices_a_->connectInput(
@@ -439,7 +463,7 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback(
}
// If cloud+normals is given, check if it's valid
if (!isValid(cloud)) {// || !isValid (cloud_normals, "normals"))
if (!isValid(cloud)) { // || !isValid (cloud_normals, "normals"))
NODELET_ERROR("[%s::input_normals_surface_indices_callback] Invalid input!", getName().c_str());
emptyPublish(cloud);
return;
@@ -468,10 +492,14 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback(
if (indices) {
NODELET_DEBUG(
"[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\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.",
" - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.\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(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@@ -489,9 +517,12 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback(
} else {
NODELET_DEBUG(
"[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
" - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@@ -508,38 +539,44 @@ pcl_ros::FeatureFromNormals::input_normals_surface_indices_callback(
} else if (indices) {
NODELET_DEBUG(
"[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\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.",
" - PointCloud with %d data points (%s), "
"stamp %f, and frame %s on topic %s received.\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(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(),
cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
*cloud_normals).c_str(),
cloud_normals->header.stamp.toSec(), cloud_normals->header.frame_id.c_str(), pnh_->resolveName(
"normals").c_str(),
cloud_normals->header.stamp.toSec(), cloud_normals->header.frame_id.c_str(),
pnh_->resolveName("normals").c_str(),
indices->indices.size(), indices->header.stamp.toSec(),
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
} else {
NODELET_DEBUG(
"[%s::input_normals_surface_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str(),
cloud_normals->width * cloud_normals->height, pcl::getFieldsList(
*cloud_normals).c_str(),
cloud_normals->header.stamp.toSec(), cloud_normals->header.frame_id.c_str(), pnh_->resolveName(
"normals").c_str());
cloud_normals->header.stamp.toSec(), cloud_normals->header.frame_id.c_str(),
pnh_->resolveName("normals").c_str());
}
///
if ((int)(cloud->width * cloud->height) < k_) {
if (static_cast<int>(cloud->width * cloud->height) < k_) {
NODELET_ERROR(
"[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) is larger than the PointCloud size (%d)!",
"[%s::input_normals_surface_indices_callback] Requested number of k-nearest neighbors (%d) "
"is larger than the PointCloud size (%d)!",
getName().c_str(), k_, (int)(cloud->width * cloud->height));
emptyPublish(cloud);
return;
pcl_ros/src/pcl_ros/filters/features/normal_3d_tbb.cpp
+1 -1
View File
@@ -78,4 +78,4 @@ pcl_ros::NormalEstimationTBB::computePublish(
typedef pcl_ros::NormalEstimationTBB NormalEstimationTBB;
PLUGINLIB_EXPORT_CLASS(NormalEstimationTBB, nodelet::Nodelet);
#endif // HAVE_TBB
#endif // HAVE_TBB
pcl_ros/src/pcl_ros/filters/filter.cpp
+22 -15
View File
@@ -35,9 +35,10 @@
*
*/
#include <pcl/io/io.h>
#include "pcl_ros/transforms.hpp"
#include "pcl_ros/filters/filter.hpp"
#include <pcl/io/io.h>
#include <vector>
#include "pcl_ros/transforms.hpp"
/*//#include <pcl/filters/pixel_grid.h>
//#include <pcl/filters/filter_dimension.h>
@@ -48,18 +49,18 @@
*/
// Include the implementations instead of compiling them separately to speed up compile time
//#include "extract_indices.cpp"
//#include "passthrough.cpp"
//#include "project_inliers.cpp"
//#include "radius_outlier_removal.cpp"
//#include "statistical_outlier_removal.cpp"
//#include "voxel_grid.cpp"
// #include "extract_indices.cpp"
// #include "passthrough.cpp"
// #include "project_inliers.cpp"
// #include "radius_outlier_removal.cpp"
// #include "statistical_outlier_removal.cpp"
// #include "voxel_grid.cpp"
/*//PLUGINLIB_EXPORT_CLASS(PixelGrid,nodelet::Nodelet);
//PLUGINLIB_EXPORT_CLASS(FilterDimension,nodelet::Nodelet);
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::Filter::computePublish(const PointCloud2::ConstPtr & input, const IndicesPtr & indices)
{
@@ -120,12 +121,14 @@ pcl_ros::Filter::subscribe()
sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
if (approximate_sync_) {
sync_input_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
sync_input_indices_a_ =
boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud2,
pcl_msgs::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));
} else {
sync_input_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
sync_input_indices_e_ =
boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud2,
pcl_msgs::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));
@@ -182,7 +185,8 @@ pcl_ros::Filter::onInit()
void
pcl_ros::Filter::config_callback(pcl_ros::FilterConfig & config, uint32_t level)
{
// The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL
// 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(
@@ -218,8 +222,10 @@ pcl_ros::Filter::input_indices_callback(
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.",
" - 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(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@@ -228,7 +234,8 @@ pcl_ros::Filter::input_indices_callback(
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
} else {
NODELET_DEBUG(
"[%s::input_indices_callback] PointCloud with %d data points and frame %s on topic %s received.",
"[%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());
}
pcl_ros/src/pcl_ros/filters/passthrough.cpp
+8 -5
View File
@@ -35,8 +35,8 @@
*
*/
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/filters/passthrough.hpp"
#include <pluginlib/class_list_macros.h>
//////////////////////////////////////////////////////////////////////////////////////////////
bool
@@ -65,7 +65,8 @@ pcl_ros::PassThrough::config_callback(pcl_ros::FilterConfig & config, uint32_t l
if (filter_min != config.filter_limit_min) {
filter_min = config.filter_limit_min;
NODELET_DEBUG(
"[%s::config_callback] Setting the minimum filtering value a point will be considered from to: %f.",
"[%s::config_callback] Setting the minimum filtering value a point will be "
"considered from to: %f.",
getName().c_str(), filter_min);
// Set the filter min-max if different
impl_.setFilterLimits(filter_min, filter_max);
@@ -74,14 +75,15 @@ pcl_ros::PassThrough::config_callback(pcl_ros::FilterConfig & config, uint32_t l
if (filter_max != config.filter_limit_max) {
filter_max = config.filter_limit_max;
NODELET_DEBUG(
"[%s::config_callback] Setting the maximum filtering value a point will be considered from to: %f.",
"[%s::config_callback] Setting the maximum filtering value a point will be "
"considered from to: %f.",
getName().c_str(), filter_max);
// Set the filter min-max if different
impl_.setFilterLimits(filter_min, filter_max);
}
// Check the current value for the filter field
//std::string filter_field = impl_.getFilterFieldName ();
// std::string filter_field = impl_.getFilterFieldName ();
if (impl_.getFilterFieldName() != config.filter_field_name) {
// Set the filter field if different
impl_.setFilterFieldName(config.filter_field_name);
@@ -108,7 +110,8 @@ pcl_ros::PassThrough::config_callback(pcl_ros::FilterConfig & config, uint32_t l
impl_.setFilterLimitsNegative(config.filter_limit_negative);
}
// The following parameters are updated automatically for all PCL_ROS Nodelet Filters as they are inexistent in PCL
// 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(
pcl_ros/src/pcl_ros/filters/project_inliers.cpp
+16 -8
View File
@@ -35,9 +35,10 @@
*
*/
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/filters/project_inliers.hpp"
#include <pluginlib/class_list_macros.h>
#include <pcl/io/io.h>
#include <vector>
//////////////////////////////////////////////////////////////////////////////////////////////
void
@@ -99,16 +100,20 @@ pcl_ros::ProjectInliers::subscribe()
sub_model_.subscribe(*pnh_, "model", max_queue_size_);
if (approximate_sync_) {
sync_input_indices_model_a_ = boost::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloud2,
PointIndices, ModelCoefficients>>>(max_queue_size_);
sync_input_indices_model_a_ =
boost::make_shared<message_filters::Synchronizer<
message_filters::sync_policies::ApproximateTime<
PointCloud2, PointIndices, ModelCoefficients>>>(max_queue_size_);
sync_input_indices_model_a_->connectInput(sub_input_filter_, sub_indices_filter_, sub_model_);
sync_input_indices_model_a_->registerCallback(
bind(
&ProjectInliers::input_indices_model_callback,
this, _1, _2, _3));
} else {
sync_input_indices_model_e_ = boost::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloud2,
PointIndices, ModelCoefficients>>>(max_queue_size_);
sync_input_indices_model_e_ =
boost::make_shared<message_filters::Synchronizer<
message_filters::sync_policies::ExactTime<
PointCloud2, PointIndices, ModelCoefficients>>>(max_queue_size_);
sync_input_indices_model_e_->connectInput(sub_input_filter_, sub_indices_filter_, sub_model_);
sync_input_indices_model_e_->registerCallback(
bind(
@@ -148,9 +153,12 @@ pcl_ros::ProjectInliers::input_indices_model_callback(
NODELET_DEBUG(
"[%s::input_indices_model_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.\n"
" - ModelCoefficients with %zu values, stamp %f, and frame %s on topic %s received.",
" - 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.\n"
" - ModelCoefficients 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->header.stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName("input").c_str(),
pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp
-1
View File
@@ -73,7 +73,6 @@ pcl_ros::RadiusOutlierRemoval::config_callback(
"[%s::config_callback] Setting the radius to search neighbors: %f.",
getName().c_str(), config.radius_search);
}
}
pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp
+2 -1
View File
@@ -63,7 +63,8 @@ pcl_ros::StatisticalOutlierRemoval::config_callback(
if (impl_.getMeanK() != config.mean_k) {
impl_.setMeanK(config.mean_k);
NODELET_DEBUG(
"[%s::config_callback] Setting the number of points (k) to use for mean distance estimation to: %d.",
"[%s::config_callback] Setting the number of points (k) to use for mean "
"distance estimation to: %d.",
getName().c_str(), config.mean_k);
}
pcl_ros/src/pcl_ros/filters/voxel_grid.cpp
+6 -3
View File
@@ -88,13 +88,15 @@ pcl_ros::VoxelGrid::config_callback(pcl_ros::VoxelGridConfig & config, uint32_t
if (filter_min != config.filter_limit_min) {
filter_min = config.filter_limit_min;
NODELET_DEBUG(
"[config_callback] Setting the minimum filtering value a point will be considered from to: %f.",
"[config_callback] Setting the minimum filtering value a point will be considered "
"from to: %f.",
filter_min);
}
if (filter_max != config.filter_limit_max) {
filter_max = config.filter_limit_max;
NODELET_DEBUG(
"[config_callback] Setting the maximum filtering value a point will be considered from to: %f.",
"[config_callback] Setting the maximum filtering value a point will be considered "
"from to: %f.",
filter_max);
}
impl_.setFilterLimits(filter_min, filter_max);
@@ -113,7 +115,8 @@ pcl_ros::VoxelGrid::config_callback(pcl_ros::VoxelGridConfig & config, uint32_t
config.filter_field_name.c_str());
}
// ---[ These really shouldn't be here, and as soon as dynamic_reconfigure improves, we'll remove them and inherit from Filter
// ---[ These really shouldn't be here, and as soon as dynamic_reconfigure improves,
// we'll remove them and inherit from Filter
if (tf_input_frame_ != config.input_frame) {
tf_input_frame_ = config.input_frame;
NODELET_DEBUG("[config_callback] Setting the input TF frame to: %s.", tf_input_frame_.c_str());
pcl_ros/src/pcl_ros/io/bag_io.cpp
+1
View File
@@ -36,6 +36,7 @@
*/
#include <pluginlib/class_list_macros.h>
#include <string>
#include "pcl_ros/io/bag_io.hpp"
//////////////////////////////////////////////////////////////////////////////////////////////
pcl_ros/src/pcl_ros/io/concatenate_data.cpp
+3 -2
View File
@@ -37,10 +37,11 @@
#include <pluginlib/class_list_macros.h>
#include <pcl/io/io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <string>
#include "pcl_ros/transforms.hpp"
#include "pcl_ros/io/concatenate_data.hpp"
#include <pcl_conversions/pcl_conversions.h>
//////////////////////////////////////////////////////////////////////////////////////////////
void
@@ -244,7 +245,7 @@ pcl_ros::PointCloudConcatenateDataSynchronizer::combineClouds(
const PointCloud2 & in2,
PointCloud2 & out)
{
//ROS_INFO ("Two pointclouds received: %zu and %zu.", in1.data.size (), in2.data.size ());
// ROS_INFO ("Two pointclouds received: %zu and %zu.", in1.data.size (), in2.data.size ());
PointCloud2::Ptr in1_t(new PointCloud2());
PointCloud2::Ptr in2_t(new PointCloud2());
pcl_ros/src/pcl_ros/io/concatenate_fields.cpp
+14 -11
View File
@@ -39,11 +39,12 @@
#include <pluginlib/class_list_macros.h>
#include <pcl/io/io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <vector>
#include "pcl_ros/io/concatenate_fields.hpp"
#include <pcl_conversions/pcl_conversions.h>
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit()
{
@@ -66,7 +67,7 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::onInit()
onInitPostProcess();
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::PointCloudConcatenateFieldsSynchronizer::subscribe()
{
@@ -75,19 +76,20 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::subscribe()
&PointCloudConcatenateFieldsSynchronizer::input_callback, this);
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::PointCloudConcatenateFieldsSynchronizer::unsubscribe()
{
sub_input_.shutdown();
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback(const PointCloudConstPtr & cloud)
{
NODELET_DEBUG(
"[input_callback] PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
"[input_callback] PointCloud with %d data points (%s), stamp %f, 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());
@@ -98,7 +100,8 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback(const PointClou
queue_.size() > 0)
{
NODELET_WARN(
"[input_callback] Maximum seconds limit (%f) reached. Difference is %f, erasing message in queue with stamp %f.", maximum_seconds_,
"[input_callback] Maximum seconds limit (%f) reached. Difference is %f, erasing message "
"in queue with stamp %f.", maximum_seconds_,
(*queue_.begin()).first.toSec(),
fabs( ( (*queue_.begin()).first - cloud->header.stamp).toSec() ));
queue_.erase(queue_.begin());
@@ -107,7 +110,7 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback(const PointClou
// Push back new data
queue_[cloud->header.stamp].push_back(cloud);
if ((int)queue_[cloud->header.stamp].size() >= input_messages_) {
if (static_cast<int>(queue_[cloud->header.stamp].size()) >= input_messages_) {
// Concatenate together and publish
std::vector<PointCloudConstPtr> & clouds = queue_[cloud->header.stamp];
PointCloud cloud_out = *clouds[0];
@@ -122,7 +125,8 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback(const PointClou
if (clouds[i]->width != cloud_out.width || clouds[i]->height != cloud_out.height) {
NODELET_ERROR(
"[input_callback] Width/height of pointcloud %zu (%dx%d) differs from the others (%dx%d)!",
"[input_callback] Width/height of pointcloud %zu (%dx%d) differs "
"from the others (%dx%d)!",
i, clouds[i]->width, clouds[i]->height, cloud_out.width, cloud_out.height);
break;
}
@@ -162,11 +166,10 @@ pcl_ros::PointCloudConcatenateFieldsSynchronizer::input_callback(const PointClou
// Clean the queue to avoid overflowing
if (maximum_queue_size_ > 0) {
while ((int)queue_.size() > maximum_queue_size_) {
while (static_cast<int>(queue_.size()) > maximum_queue_size_) {
queue_.erase(queue_.begin());
}
}
}
typedef pcl_ros::PointCloudConcatenateFieldsSynchronizer PointCloudConcatenateFieldsSynchronizer;
pcl_ros/src/pcl_ros/io/io.cpp
+8 -7
View File
@@ -38,20 +38,21 @@
#include <pluginlib/class_list_macros.h>
#include <sensor_msgs/PointCloud2.h>
#include <message_filters/subscriber.h>
//#include <pcl_ros/subscriber.hpp>
// #include <pcl_ros/subscriber.hpp>
#include <nodelet_topic_tools/nodelet_mux.h>
#include <nodelet_topic_tools/nodelet_demux.h>
typedef nodelet::NodeletMUX<sensor_msgs::PointCloud2,
message_filters::Subscriber<sensor_msgs::PointCloud2>> NodeletMUX;
//typedef nodelet::NodeletDEMUX<sensor_msgs::PointCloud2, pcl_ros::Subscriber<sensor_msgs::PointCloud2> > NodeletDEMUX;
// typedef nodelet::NodeletDEMUX<sensor_msgs::PointCloud2,
// pcl_ros::Subscriber<sensor_msgs::PointCloud2> > NodeletDEMUX;
typedef nodelet::NodeletDEMUX<sensor_msgs::PointCloud2> NodeletDEMUX;
//#include "pcd_io.cpp"
//#include "bag_io.cpp"
//#include "concatenate_fields.cpp"
//#include "concatenate_data.cpp"
// #include "pcd_io.cpp"
// #include "bag_io.cpp"
// #include "concatenate_fields.cpp"
// #include "concatenate_data.cpp"
PLUGINLIB_EXPORT_CLASS(NodeletMUX, nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(NodeletDEMUX, nodelet::Nodelet);
//PLUGINLIB_EXPORT_CLASS(NodeletDEMUX_ROS,nodelet::Nodelet);
// PLUGINLIB_EXPORT_CLASS(NodeletDEMUX_ROS,nodelet::Nodelet);
pcl_ros/src/pcl_ros/io/pcd_io.cpp
+1
View File
@@ -37,6 +37,7 @@
#include <pluginlib/class_list_macros.h>
#include <pcl_ros/io/pcd_io.hpp>
#include <string>
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros/src/pcl_ros/segmentation/extract_clusters.cpp
+24 -15
View File
@@ -38,9 +38,10 @@
#include <pluginlib/class_list_macros.h>
#include <pcl/io/io.h>
#include <pcl/PointIndices.h>
#include <pcl_conversions/pcl_conversions.h>
#include <vector>
#include "pcl_ros/segmentation/extract_clusters.hpp"
#include <pcl_conversions/pcl_conversions.h>
using pcl_conversions::fromPCL;
using pcl_conversions::moveFromPCL;
@@ -69,7 +70,7 @@ pcl_ros::EuclideanClusterExtraction::onInit()
return;
}
//private_nh.getParam ("use_indices", use_indices_);
// private_nh.getParam ("use_indices", use_indices_);
pnh_->getParam("publish_indices", publish_indices_);
if (publish_indices_) {
@@ -110,16 +111,19 @@ pcl_ros::EuclideanClusterExtraction::subscribe()
sub_indices_filter_.subscribe(*pnh_, "indices", max_queue_size_);
if (approximate_sync_) {
sync_input_indices_a_ = boost::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloud,
PointIndices>>>(max_queue_size_);
sync_input_indices_a_ =
boost::make_shared<message_filters::Synchronizer<
message_filters::sync_policies::ApproximateTime<
PointCloud, PointIndices>>>(max_queue_size_);
sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback(
bind(
&EuclideanClusterExtraction::
input_indices_callback, this, _1, _2));
} else {
sync_input_indices_e_ = boost::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloud,
PointIndices>>>(max_queue_size_);
sync_input_indices_e_ =
boost::make_shared<message_filters::Synchronizer<
message_filters::sync_policies::ExactTime<PointCloud, PointIndices>>>(max_queue_size_);
sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_e_->registerCallback(
bind(
@@ -206,8 +210,10 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback(
std_msgs::Header indices_header = indices->header;
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.",
" - 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(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(),
cloud_header.stamp.toSec(), cloud_header.frame_id.c_str(), pnh_->resolveName("input").c_str(),
@@ -215,7 +221,8 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback(
indices_header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
} else {
NODELET_DEBUG(
"[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.",
"[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on "
"topic %s received.",
getName().c_str(), cloud->width * cloud->height, fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str());
@@ -235,10 +242,11 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback(
if (publish_indices_) {
for (size_t i = 0; i < clusters.size(); ++i) {
if ((int)i >= max_clusters_) {
if (static_cast<int>(i) >= max_clusters_) {
break;
}
// TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number.
// TODO(xxx): HACK!!! We need to change the PointCloud2 message to add for an incremental
// sequence ID number.
pcl_msgs::PointIndices ros_pi;
moveFromPCL(clusters[i], ros_pi);
ros_pi.header.stamp += ros::Duration(i * 0.001);
@@ -250,15 +258,16 @@ pcl_ros::EuclideanClusterExtraction::input_indices_callback(
clusters.size(), pnh_->resolveName("output").c_str());
} else {
for (size_t i = 0; i < clusters.size(); ++i) {
if ((int)i >= max_clusters_) {
if (static_cast<int>(i) >= max_clusters_) {
break;
}
PointCloud output;
copyPointCloud(*cloud, clusters[i].indices, output);
//PointCloud output_blob; // Convert from the templated output to the PointCloud blob
//pcl::toROSMsg (output, output_blob);
// TODO: HACK!!! We need to change the PointCloud2 message to add for an incremental sequence ID number.
// PointCloud output_blob; // Convert from the templated output to the PointCloud blob
// pcl::toROSMsg (output, output_blob);
// TODO(xxx): HACK!!! We need to change the PointCloud2 message to add for an incremental
// sequence ID number.
std_msgs::Header header = fromPCL(output.header);
header.stamp += ros::Duration(i * 0.001);
toPCL(header, output.header);
pcl_ros/src/pcl_ros/segmentation/extract_polygonal_prism_data.cpp
+23 -14
View File
@@ -36,11 +36,11 @@
*/
#include <pluginlib/class_list_macros.h>
#include <pcl/io/io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <vector>
#include "pcl_ros/transforms.hpp"
#include "pcl_ros/segmentation/extract_polygonal_prism_data.hpp"
#include <pcl/io/io.h>
#include <pcl_conversions/pcl_conversions.h>
using pcl_conversions::moveFromPCL;
using pcl_conversions::moveToPCL;
@@ -73,11 +73,13 @@ pcl_ros::ExtractPolygonalPrismData::subscribe()
// Create the objects here
if (approximate_sync_) {
sync_input_hull_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
PointCloud, PointIndices>>>(max_queue_size_);
sync_input_hull_indices_a_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ApproximateTime<PointCloud, PointCloud, PointIndices>>>(max_queue_size_);
} else {
sync_input_hull_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
PointCloud, PointIndices>>>(max_queue_size_);
sync_input_hull_indices_e_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ExactTime<PointCloud, PointCloud, PointIndices>>>(max_queue_size_);
}
if (use_indices_) {
@@ -183,9 +185,12 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback(
if (indices) {
NODELET_DEBUG(
"[%s::input_indices_hull_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\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.",
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.\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(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@@ -198,8 +203,10 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback(
} else {
NODELET_DEBUG(
"[%s::input_indices_hull_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@@ -212,7 +219,8 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback(
if (cloud->header.frame_id != hull->header.frame_id) {
NODELET_DEBUG(
"[%s::input_hull_callback] Planar hull has a different TF frame (%s) than the input point cloud (%s)! Using TF to transform.",
"[%s::input_hull_callback] Planar hull has a different TF frame (%s) than the input "
"point cloud (%s)! Using TF to transform.",
getName().c_str(), hull->header.frame_id.c_str(), cloud->header.frame_id.c_str());
PointCloud planar_hull;
if (!pcl_ros::transformPointCloud(cloud->header.frame_id, *hull, planar_hull, tf_listener_)) {
@@ -233,7 +241,8 @@ pcl_ros::ExtractPolygonalPrismData::input_hull_indices_callback(
impl_.setInputCloud(pcl_ptr(cloud));
impl_.setIndices(indices_ptr);
// Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL)
// Final check if the data is empty
// (remember that indices are set to the size of the data -- if indices* = NULL)
if (!cloud->points.empty()) {
pcl::PointIndices pcl_inliers;
moveToPCL(inliers, pcl_inliers);
pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp
+62 -41
View File
@@ -36,10 +36,10 @@
*/
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/segmentation/sac_segmentation.hpp"
#include <pcl/io/io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <vector>
#include "pcl_ros/segmentation/sac_segmentation.hpp"
using pcl_conversions::fromPCL;
@@ -61,7 +61,7 @@ pcl_ros::SACSegmentation::onInit()
NODELET_ERROR("[onInit] Need a 'model_type' parameter to be set before continuing!");
return;
}
double threshold; // unused - set via dynamic reconfigure in the callback
double threshold; // unused - set via dynamic reconfigure in the callback
if (!pnh_->getParam("distance_threshold", threshold)) {
NODELET_ERROR("[onInit] Need a 'distance_threshold' parameter to be set before continuing!");
return;
@@ -80,7 +80,8 @@ pcl_ros::SACSegmentation::onInit()
{
if (axis_param.size() != 3) {
NODELET_ERROR(
"[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!",
"[%s::onInit] Parameter 'axis' given but with a different number of values (%d) "
"than required (3)!",
getName().c_str(), axis_param.size());
return;
}
@@ -149,27 +150,28 @@ pcl_ros::SACSegmentation::subscribe()
// Synchronize the two topics. No need for an approximate synchronizer here, as we'll
// match the timestamps exactly
sync_input_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
PointIndices>>>(max_queue_size_);
sync_input_indices_e_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ExactTime<PointCloud, PointIndices>>>(max_queue_size_);
sync_input_indices_e_->connectInput(sub_input_filter_, nf_pi_);
sync_input_indices_e_->registerCallback(
bind(
&SACSegmentation::input_indices_callback, this,
_1, _2));
}
// "latched_indices" not set, proceed with regular <input,indices> pairs
else {
} else { // "latched_indices" not set, proceed with regular <input,indices> pairs
if (approximate_sync_) {
sync_input_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
PointIndices>>>(max_queue_size_);
sync_input_indices_a_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ApproximateTime<PointCloud, PointIndices>>>(max_queue_size_);
sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback(
bind(
&SACSegmentation::input_indices_callback, this,
_1, _2));
} else {
sync_input_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
PointIndices>>>(max_queue_size_);
sync_input_indices_e_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ExactTime<PointCloud, PointIndices>>>(max_queue_size_);
sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_e_->registerCallback(
bind(
@@ -205,7 +207,7 @@ pcl_ros::SACSegmentation::config_callback(SACSegmentationConfig & config, uint32
boost::mutex::scoped_lock lock(mutex_);
if (impl_.getDistanceThreshold() != config.distance_threshold) {
//sac_->setDistanceThreshold (threshold_); - done in initSAC
// sac_->setDistanceThreshold (threshold_); - done in initSAC
impl_.setDistanceThreshold(config.distance_threshold);
NODELET_DEBUG(
"[%s::config_callback] Setting new distance to model threshold to: %f.",
@@ -228,14 +230,14 @@ pcl_ros::SACSegmentation::config_callback(SACSegmentationConfig & config, uint32
}
if (impl_.getMaxIterations() != config.max_iterations) {
//sac_->setMaxIterations (max_iterations_); - done in initSAC
// sac_->setMaxIterations (max_iterations_); - done in initSAC
impl_.setMaxIterations(config.max_iterations);
NODELET_DEBUG(
"[%s::config_callback] Setting new maximum number of iterations to: %d.",
getName().c_str(), config.max_iterations);
}
if (impl_.getProbability() != config.probability) {
//sac_->setProbability (probability_); - done in initSAC
// sac_->setProbability (probability_); - done in initSAC
impl_.setProbability(config.probability);
NODELET_DEBUG(
"[%s::config_callback] Setting new probability to: %f.",
@@ -307,8 +309,10 @@ pcl_ros::SACSegmentation::input_indices_callback(
if (indices && !indices->header.frame_id.empty()) {
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.",
" - 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(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@@ -317,7 +321,8 @@ pcl_ros::SACSegmentation::input_indices_callback(
indices->header.frame_id.c_str(), pnh_->resolveName("indices").c_str());
} else {
NODELET_DEBUG(
"[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.",
"[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and "
"frame %s on topic %s received.",
getName().c_str(), cloud->width * cloud->height, fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
"input").c_str());
@@ -329,11 +334,13 @@ pcl_ros::SACSegmentation::input_indices_callback(
PointCloudConstPtr cloud_tf;
/* if (!tf_input_frame_.empty () && cloud->header.frame_id != tf_input_frame_)
{
NODELET_DEBUG ("[input_callback] Transforming input dataset from %s to %s.", cloud->header.frame_id.c_str (), tf_input_frame_.c_str ());
NODELET_DEBUG ("[input_callback] 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
PointCloud cloud_transformed;
if (!pcl::transformPointCloud (tf_input_frame_, cloud->header.stamp, *cloud, cloud_transformed, tf_listener_))
if (!pcl::transformPointCloud (tf_input_frame_, cloud->header.stamp, *cloud,
cloud_transformed, tf_listener_))
return;
cloud_tf.reset (new PointCloud (cloud_transformed));
}
@@ -348,7 +355,8 @@ pcl_ros::SACSegmentation::input_indices_callback(
impl_.setInputCloud(pcl_ptr(cloud_tf));
impl_.setIndices(indices_ptr);
// Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL)
// Final check if the data is empty
// (remember that indices are set to the size of the data -- if indices* = NULL)
if (!cloud->points.empty()) {
pcl::PointIndices pcl_inliers;
pcl::ModelCoefficients pcl_model;
@@ -362,7 +370,7 @@ pcl_ros::SACSegmentation::input_indices_callback(
// Probably need to transform the model of the plane here
// Check if we have enough inliers, clear inliers + model if not
if ((int)inliers.indices.size() <= min_inliers_) {
if (static_cast<int>(inliers.indices.size()) <= min_inliers_) {
inliers.indices.clear();
model.values.clear();
}
@@ -371,7 +379,8 @@ pcl_ros::SACSegmentation::input_indices_callback(
pub_indices_.publish(boost::make_shared<const PointIndices>(inliers));
pub_model_.publish(boost::make_shared<const ModelCoefficients>(model));
NODELET_DEBUG(
"[%s::input_indices_callback] Published PointIndices with %zu values on topic %s, and ModelCoefficients with %zu values on topic %s",
"[%s::input_indices_callback] Published PointIndices with %zu values on topic %s, "
"and ModelCoefficients with %zu values on topic %s",
getName().c_str(), inliers.indices.size(), pnh_->resolveName("inliers").c_str(),
model.values.size(), pnh_->resolveName("model").c_str());
@@ -405,7 +414,7 @@ pcl_ros::SACSegmentationFromNormals::onInit()
getName().c_str());
return;
}
double threshold; // unused - set via dynamic reconfigure in the callback
double threshold; // unused - set via dynamic reconfigure in the callback
if (!pnh_->getParam("distance_threshold", threshold)) {
NODELET_ERROR(
"[%s::onInit] Need a 'distance_threshold' parameter to be set before continuing!",
@@ -426,7 +435,8 @@ pcl_ros::SACSegmentationFromNormals::onInit()
{
if (axis_param.size() != 3) {
NODELET_ERROR(
"[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than required (3)!",
"[%s::onInit] Parameter 'axis' given but with a different number of values (%d) than "
"required (3)!",
getName().c_str(), axis_param.size());
return;
}
@@ -475,15 +485,18 @@ pcl_ros::SACSegmentationFromNormals::subscribe()
sub_input_filter_.subscribe(*pnh_, "input", max_queue_size_);
sub_normals_filter_.subscribe(*pnh_, "normals", max_queue_size_);
// Subscribe to an axis direction along which the model search is to be constrained (the first 3 model coefficients will be checked)
// Subscribe to an axis direction along which the model search is to be constrained (the first
// 3 model coefficients will be checked)
sub_axis_ = pnh_->subscribe("axis", 1, &SACSegmentationFromNormals::axis_callback, this);
if (approximate_sync_) {
sync_input_normals_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
PointCloudN, PointIndices>>>(max_queue_size_);
sync_input_normals_indices_a_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ApproximateTime<PointCloud, PointCloudN, PointIndices>>>(max_queue_size_);
} else {
sync_input_normals_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
PointCloudN, PointIndices>>>(max_queue_size_);
sync_input_normals_indices_e_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ExactTime<PointCloud, PointCloudN, PointIndices>>>(max_queue_size_);
}
// If we're supposed to look for PointIndices (indices)
@@ -658,7 +671,7 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback(
return;
}
if (!isValid(cloud)) {// || !isValid (cloud_normals, "normals"))
if (!isValid(cloud)) { // || !isValid (cloud_normals, "normals"))
NODELET_ERROR("[%s::input_normals_indices_callback] Invalid input!", getName().c_str());
pub_indices_.publish(boost::make_shared<const PointIndices>(inliers));
pub_model_.publish(boost::make_shared<const ModelCoefficients>(model));
@@ -676,9 +689,12 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback(
if (indices && !indices->header.frame_id.empty()) {
NODELET_DEBUG(
"[%s::input_normals_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\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.",
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.\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(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@@ -692,8 +708,10 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback(
} else {
NODELET_DEBUG(
"[%s::input_normals_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
@@ -711,7 +729,8 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback(
int cloud_normals_nr_points = cloud_normals->width * cloud_normals->height;
if (cloud_nr_points != cloud_normals_nr_points) {
NODELET_ERROR(
"[%s::input_normals_indices_callback] Number of points in the input dataset (%d) differs from the number of points in the normals (%d)!",
"[%s::input_normals_indices_callback] Number of points in the input dataset (%d) differs "
"from the number of points in the normals (%d)!",
getName().c_str(), cloud_nr_points, cloud_normals_nr_points);
pub_indices_.publish(boost::make_shared<const PointIndices>(inliers));
pub_model_.publish(boost::make_shared<const ModelCoefficients>(model));
@@ -728,7 +747,8 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback(
impl_.setIndices(indices_ptr);
// Final check if the data is empty (remember that indices are set to the size of the data -- if indices* = NULL)
// Final check if the data is empty
// (remember that indices are set to the size of the data -- if indices* = NULL)
if (!cloud->points.empty()) {
pcl::PointIndices pcl_inliers;
pcl::ModelCoefficients pcl_model;
@@ -740,7 +760,7 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback(
}
// Check if we have enough inliers, clear inliers + model if not
if ((int)inliers.indices.size() <= min_inliers_) {
if (static_cast<int>(inliers.indices.size()) <= min_inliers_) {
inliers.indices.clear();
model.values.clear();
}
@@ -749,7 +769,8 @@ pcl_ros::SACSegmentationFromNormals::input_normals_indices_callback(
pub_indices_.publish(boost::make_shared<const PointIndices>(inliers));
pub_model_.publish(boost::make_shared<const ModelCoefficients>(model));
NODELET_DEBUG(
"[%s::input_normals_callback] Published PointIndices with %zu values on topic %s, and ModelCoefficients with %zu values on topic %s",
"[%s::input_normals_callback] Published PointIndices with %zu values on topic %s, and "
"ModelCoefficients with %zu values on topic %s",
getName().c_str(), inliers.indices.size(), pnh_->resolveName("inliers").c_str(),
model.values.size(), pnh_->resolveName("model").c_str());
if (inliers.indices.empty()) {
pcl_ros/src/pcl_ros/segmentation/segment_differences.cpp
+15 -11
View File
@@ -36,10 +36,10 @@
*/
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/segmentation/segment_differences.hpp"
#include <pcl/io/io.h>
#include "pcl_ros/segmentation/segment_differences.hpp"
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::SegmentDifferences::onInit()
{
@@ -57,7 +57,7 @@ pcl_ros::SegmentDifferences::onInit()
onInitPostProcess();
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::SegmentDifferences::subscribe()
{
@@ -72,16 +72,18 @@ pcl_ros::SegmentDifferences::subscribe()
srv_->setCallback(f);
if (approximate_sync_) {
sync_input_target_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
PointCloud>>>(max_queue_size_);
sync_input_target_a_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ApproximateTime<PointCloud, PointCloud>>>(max_queue_size_);
sync_input_target_a_->connectInput(sub_input_filter_, sub_target_filter_);
sync_input_target_a_->registerCallback(
bind(
&SegmentDifferences::input_target_callback, this,
_1, _2));
} else {
sync_input_target_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
PointCloud>>>(max_queue_size_);
sync_input_target_e_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ExactTime<PointCloud, PointCloud>>>(max_queue_size_);
sync_input_target_e_->connectInput(sub_input_filter_, sub_target_filter_);
sync_input_target_e_->registerCallback(
bind(
@@ -90,7 +92,7 @@ pcl_ros::SegmentDifferences::subscribe()
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::SegmentDifferences::unsubscribe()
{
@@ -98,7 +100,7 @@ pcl_ros::SegmentDifferences::unsubscribe()
sub_target_filter_.unsubscribe();
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::SegmentDifferences::config_callback(SegmentDifferencesConfig & config, uint32_t level)
{
@@ -131,8 +133,10 @@ pcl_ros::SegmentDifferences::input_target_callback(
NODELET_DEBUG(
"[%s::input_indices_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.",
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.\n"
" - PointCloud with %d data points (%s), stamp %f, and "
"frame %s on topic %s received.",
getName().c_str(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(), pnh_->resolveName(
pcl_ros/src/pcl_ros/segmentation/segmentation.cpp
+4 -4
View File
@@ -36,7 +36,7 @@
*/
// Include the implementations here instead of compiling them separately to speed up compile time
//#include "extract_clusters.cpp"
//#include "extract_polygonal_prism_data.cpp"
//#include "sac_segmentation.cpp"
//#include "segment_differences.cpp"
// #include "extract_clusters.cpp"
// #include "extract_polygonal_prism_data.cpp"
// #include "sac_segmentation.cpp"
// #include "segment_differences.cpp"
pcl_ros/src/pcl_ros/surface/convex_hull.cpp
+14 -8
View File
@@ -37,8 +37,9 @@
#include <pluginlib/class_list_macros.h>
#include <pcl/common/io.h>
#include "pcl_ros/surface/convex_hull.hpp"
#include <geometry_msgs/PolygonStamped.h>
#include <vector>
#include "pcl_ros/surface/convex_hull.hpp"
//////////////////////////////////////////////////////////////////////////////////////////////
void
@@ -73,8 +74,9 @@ pcl_ros::ConvexHull2D::subscribe()
sub_indices_filter_.subscribe(*pnh_, "indices", 1);
if (approximate_sync_) {
sync_input_indices_a_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ApproximateTime<PointCloud,
PointIndices>>>(max_queue_size_);
sync_input_indices_a_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ApproximateTime<PointCloud, PointIndices>>>(max_queue_size_);
// surface not enabled, connect the input-indices duo and register
sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback(
@@ -82,8 +84,9 @@ pcl_ros::ConvexHull2D::subscribe()
&ConvexHull2D::input_indices_callback, this, _1,
_2));
} else {
sync_input_indices_e_ = boost::make_shared<message_filters::Synchronizer<sync_policies::ExactTime<PointCloud,
PointIndices>>>(max_queue_size_);
sync_input_indices_e_ =
boost::make_shared<message_filters::Synchronizer<
sync_policies::ExactTime<PointCloud, PointIndices>>>(max_queue_size_);
// surface not enabled, connect the input-indices duo and register
sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_e_->registerCallback(
@@ -146,8 +149,10 @@ pcl_ros::ConvexHull2D::input_indices_callback(
if (indices) {
NODELET_DEBUG(
"[%s::input_indices_model_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.",
" - 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(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(),
@@ -156,7 +161,8 @@ pcl_ros::ConvexHull2D::input_indices_callback(
indices->header.frame_id.c_str(), getMTPrivateNodeHandle().resolveName("indices").c_str());
} else {
NODELET_DEBUG(
"[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.",
"[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and "
"frame %s on topic %s received.",
getName().c_str(), cloud->width * cloud->height, fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(),
getMTPrivateNodeHandle().resolveName("input").c_str());
pcl_ros/src/pcl_ros/surface/moving_least_squares.cpp
+22 -12
View File
@@ -36,21 +36,24 @@
*/
#include <pluginlib/class_list_macros.h>
#include "pcl_ros/surface/moving_least_squares.hpp"
#include <pcl/io/io.h>
#include <vector>
#include "pcl_ros/surface/moving_least_squares.hpp"
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl_ros::MovingLeastSquares::onInit()
{
PCLNodelet::onInit();
//ros::NodeHandle private_nh = getMTPrivateNodeHandle ();
// ros::NodeHandle private_nh = getMTPrivateNodeHandle ();
pub_output_ = advertise<PointCloudIn>(*pnh_, "output", max_queue_size_);
pub_normals_ = advertise<NormalCloudOut>(*pnh_, "normals", max_queue_size_);
//if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("search_radius", search_radius_))
// if (!pnh_->getParam ("k_search", k_) && !pnh_->getParam ("search_radius", search_radius_))
if (!pnh_->getParam("search_radius", search_radius_)) {
//NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'search_radius' set! Need to set at least one of these parameters before continuing.", getName ().c_str ());
// NODELET_ERROR ("[%s::onInit] Neither 'k_search' nor 'search_radius' set! Need to set "
// "at least one of these parameters before continuing.", getName ().c_str ());
NODELET_ERROR(
"[%s::onInit] Need a 'search_radius' parameter to be set before continuing!",
getName().c_str());
@@ -93,8 +96,10 @@ pcl_ros::MovingLeastSquares::subscribe()
sub_indices_filter_.subscribe(*pnh_, "indices", 1);
if (approximate_sync_) {
sync_input_indices_a_ = boost::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<PointCloudIn,
PointIndices>>>(max_queue_size_);
sync_input_indices_a_ =
boost::make_shared<message_filters::Synchronizer<
message_filters::sync_policies::ApproximateTime<
PointCloudIn, PointIndices>>>(max_queue_size_);
// surface not enabled, connect the input-indices duo and register
sync_input_indices_a_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_a_->registerCallback(
@@ -102,8 +107,10 @@ pcl_ros::MovingLeastSquares::subscribe()
&MovingLeastSquares::input_indices_callback,
this, _1, _2));
} else {
sync_input_indices_e_ = boost::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ExactTime<PointCloudIn,
PointIndices>>>(max_queue_size_);
sync_input_indices_e_ =
boost::make_shared<message_filters::Synchronizer<
message_filters::sync_policies::ExactTime<PointCloudIn,
PointIndices>>>(max_queue_size_);
// surface not enabled, connect the input-indices duo and register
sync_input_indices_e_->connectInput(sub_input_filter_, sub_indices_filter_);
sync_input_indices_e_->registerCallback(
@@ -168,8 +175,10 @@ pcl_ros::MovingLeastSquares::input_indices_callback(
if (indices) {
NODELET_DEBUG(
"[%s::input_indices_model_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.",
" - 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(),
cloud->width * cloud->height, pcl::getFieldsList(*cloud).c_str(), fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(),
@@ -178,7 +187,8 @@ pcl_ros::MovingLeastSquares::input_indices_callback(
indices->header.frame_id.c_str(), getMTPrivateNodeHandle().resolveName("indices").c_str());
} else {
NODELET_DEBUG(
"[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.",
"[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on "
"topic %s received.",
getName().c_str(), cloud->width * cloud->height, fromPCL(
cloud->header).stamp.toSec(), cloud->header.frame_id.c_str(),
getMTPrivateNodeHandle().resolveName("input").c_str());
@@ -198,7 +208,7 @@ pcl_ros::MovingLeastSquares::input_indices_callback(
// Initialize the spatial locator
// Do the reconstructon
//impl_.process (output);
// impl_.process (output);
// Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied
pcl_ros/src/pcl_ros/surface/surface.cpp
+2 -2
View File
@@ -42,5 +42,5 @@
**/
// Include the implementations here instead of compiling them separately to speed up compile time
//#include "convex_hull.cpp"
//#include "moving_least_squares.cpp"
// #include "convex_hull.cpp"
// #include "moving_least_squares.cpp"
pcl_ros/src/test/test_tf_message_filter_pcl.cpp
+21 -19
View File
@@ -48,7 +48,9 @@
#include <gtest/gtest.h>
using namespace tf;
#include <algorithm>
#include <string>
#include <vector>
// using a random point type, as we want to make sure that it does work with
// other points than just XYZ
@@ -78,7 +80,7 @@ void setStamp(ros::Time & stamp, std::uint64_t & pcl_stamp)
class Notification
{
public:
Notification(int expected_count)
explicit Notification(int expected_count)
: count_(0),
expected_count_(expected_count),
failure_count_(0)
@@ -90,7 +92,7 @@ public:
++count_;
}
void failure(const PCDType::ConstPtr & message, FilterFailureReason reason)
void failure(const PCDType::ConstPtr & message, tf::FilterFailureReason reason)
{
++failure_count_;
}
@@ -104,7 +106,7 @@ TEST(MessageFilter, noTransforms)
{
tf::TransformListener tf_client;
Notification n(1);
MessageFilter<PCDType> filter(tf_client, "frame1", 1);
tf::MessageFilter<PCDType> filter(tf_client, "frame1", 1);
filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
PCDType::Ptr msg(new PCDType);
@@ -120,7 +122,7 @@ TEST(MessageFilter, noTransformsSameFrame)
{
tf::TransformListener tf_client;
Notification n(1);
MessageFilter<PCDType> filter(tf_client, "frame1", 1);
tf::MessageFilter<PCDType> filter(tf_client, "frame1", 1);
filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
PCDType::Ptr msg(new PCDType);
@@ -136,7 +138,7 @@ TEST(MessageFilter, preexistingTransforms)
{
tf::TransformListener tf_client;
Notification n(1);
MessageFilter<PCDType> filter(tf_client, "frame1", 1);
tf::MessageFilter<PCDType> filter(tf_client, "frame1", 1);
filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
PCDType::Ptr msg(new PCDType);
@@ -161,7 +163,7 @@ TEST(MessageFilter, postTransforms)
{
tf::TransformListener tf_client;
Notification n(1);
MessageFilter<PCDType> filter(tf_client, "frame1", 1);
tf::MessageFilter<PCDType> filter(tf_client, "frame1", 1);
filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
ros::Time stamp = ros::Time::now();
@@ -190,7 +192,7 @@ TEST(MessageFilter, queueSize)
{
tf::TransformListener tf_client;
Notification n(10);
MessageFilter<PCDType> filter(tf_client, "frame1", 10);
tf::MessageFilter<PCDType> filter(tf_client, "frame1", 10);
filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
@@ -226,7 +228,7 @@ TEST(MessageFilter, setTargetFrame)
{
tf::TransformListener tf_client;
Notification n(1);
MessageFilter<PCDType> filter(tf_client, "frame1", 1);
tf::MessageFilter<PCDType> filter(tf_client, "frame1", 1);
filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
filter.setTargetFrame("frame1000");
@@ -252,7 +254,7 @@ TEST(MessageFilter, multipleTargetFrames)
{
tf::TransformListener tf_client;
Notification n(1);
MessageFilter<PCDType> filter(tf_client, "", 1);
tf::MessageFilter<PCDType> filter(tf_client, "", 1);
filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
std::vector<std::string> target_frames;
@@ -277,9 +279,9 @@ TEST(MessageFilter, multipleTargetFrames)
ros::WallDuration(0.1).sleep();
ros::spinOnce();
EXPECT_EQ(0, n.count_); // frame1->frame3 exists, frame2->frame3 does not (yet)
EXPECT_EQ(0, n.count_); // frame1->frame3 exists, frame2->frame3 does not (yet)
//ros::Time::setNow(ros::Time::now() + ros::Duration(1.0));
// ros::Time::setNow(ros::Time::now() + ros::Duration(1.0));
transform.child_frame_id_ = "frame2";
tf_client.setTransform(transform);
@@ -295,7 +297,7 @@ TEST(MessageFilter, tolerance)
ros::Duration offset(0.2);
tf::TransformListener tf_client;
Notification n(1);
MessageFilter<PCDType> filter(tf_client, "frame1", 1);
tf::MessageFilter<PCDType> filter(tf_client, "frame1", 1);
filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
filter.setTolerance(offset);
@@ -314,9 +316,9 @@ TEST(MessageFilter, tolerance)
msg->header.stamp = pcl_stamp;
filter.add(msg);
EXPECT_EQ(0, n.count_); //No return due to lack of space for offset
EXPECT_EQ(0, n.count_); // No return due to lack of space for offset
//ros::Time::setNow(ros::Time::now() + ros::Duration(0.1));
// ros::Time::setNow(ros::Time::now() + ros::Duration(0.1));
transform.stamp_ += offset * 1.1;
tf_client.setTransform(transform);
@@ -324,7 +326,7 @@ TEST(MessageFilter, tolerance)
ros::WallDuration(0.1).sleep();
ros::spinOnce();
EXPECT_EQ(1, n.count_); // Now have data for the message published earlier
EXPECT_EQ(1, n.count_); // Now have data for the message published earlier
stamp += offset;
setStamp(stamp, pcl_stamp);
@@ -332,14 +334,14 @@ TEST(MessageFilter, tolerance)
filter.add(msg);
EXPECT_EQ(1, n.count_); // Latest message is off the end of the offset
EXPECT_EQ(1, n.count_); // Latest message is off the end of the offset
}
TEST(MessageFilter, outTheBackFailure)
{
tf::TransformListener tf_client;
Notification n(1);
MessageFilter<PCDType> filter(tf_client, "frame1", 1);
tf::MessageFilter<PCDType> filter(tf_client, "frame1", 1);
filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
ros::Time stamp = ros::Time::now();
@@ -366,7 +368,7 @@ TEST(MessageFilter, emptyFrameIDFailure)
{
tf::TransformListener tf_client;
Notification n(1);
MessageFilter<PCDType> filter(tf_client, "frame1", 1);
tf::MessageFilter<PCDType> filter(tf_client, "frame1", 1);
filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
PCDType::Ptr msg(new PCDType);
pcl_ros/src/transforms.cpp
+15 -9
View File
@@ -38,6 +38,8 @@
#include <pcl/common/io.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <limits>
#include <string>
#include "pcl_ros/transforms.hpp"
#include "pcl_ros/impl/transforms.hpp"
@@ -142,21 +144,24 @@ transformPointCloud(
in.fields[z_idx].offset, 0);
for (size_t i = 0; i < in.width * in.height; ++i) {
Eigen::Vector4f pt(*(float *)&in.data[xyz_offset[0]], *(float *)&in.data[xyz_offset[1]],
*(float *)&in.data[xyz_offset[2]], 1);
Eigen::Vector4f pt(*reinterpret_cast<float *>(&in.data[xyz_offset[0]]),
*reinterpret_cast<float *>(&in.data[xyz_offset[1]]),
*reinterpret_cast<float *>(&in.data[xyz_offset[2]], 1));
Eigen::Vector4f pt_out;
bool max_range_point = false;
int distance_ptr_offset = i * in.point_step + in.fields[dist_idx].offset;
float * distance_ptr = (dist_idx < 0 ? NULL : (float *)(&in.data[distance_ptr_offset]));
float * distance_ptr =
(dist_idx < 0 ? NULL : reinterpret_cast<float *>(&in.data[distance_ptr_offset]));
if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2])) {
if (distance_ptr == NULL || !std::isfinite(*distance_ptr)) { // Invalid point
if (distance_ptr == NULL || !std::isfinite(*distance_ptr)) { // Invalid point
pt_out = pt;
} else { // max range point
} else { // max range point
pt[0] = *distance_ptr; // Replace x with the x value saved in distance
pt_out = transform * pt;
max_range_point = true;
//std::cout << pt[0]<<","<<pt[1]<<","<<pt[2]<<" => "<<pt_out[0]<<","<<pt_out[1]<<","<<pt_out[2]<<"\n";
// std::cout << pt[0]<<","<<pt[1]<<","<<pt[2]<<" => "<<pt_out[0]<<",
// "<<pt_out[1]<<","<<pt_out[2]<<"\n";
}
} else {
pt_out = transform * pt;
@@ -164,7 +169,7 @@ transformPointCloud(
if (max_range_point) {
// Save x value in distance again
*(float *)(&out.data[distance_ptr_offset]) = pt_out[0];
*reinterpret_cast<float *>(&out.data[distance_ptr_offset]) = pt_out[0];
pt_out[0] = std::numeric_limits<float>::quiet_NaN();
}
@@ -181,7 +186,8 @@ transformPointCloud(
if (vp_idx != -1) {
// Transform the viewpoint info too
for (size_t i = 0; i < out.width * out.height; ++i) {
float * pstep = (float *)&out.data[i * out.point_step + out.fields[vp_idx].offset];
float * pstep =
reinterpret_cast<float *>(&out.data[i * out.point_step + out.fields[vp_idx].offset]);
// Assume vp_x, vp_y, vp_z are consecutive
Eigen::Vector4f vp_in(pstep[0], pstep[1], pstep[2], 1);
Eigen::Vector4f vp_out = transform * vp_in;
@@ -212,7 +218,7 @@ transformAsMatrix(const tf::Transform & bt, Eigen::Matrix4f & out_mat)
out_mat(2, 3) = origin.z();
}
} // namespace pcl_ros
} // namespace pcl_ros
//////////////////////////////////////////////////////////////////////////////////////////////
template void pcl_ros::transformPointCloudWithNormals<pcl::PointNormal>(